1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.semianalytical.dsst.forces;
18
19 import java.lang.reflect.Array;
20 import java.util.ArrayList;
21 import java.util.Collections;
22 import java.util.HashMap;
23 import java.util.List;
24 import java.util.Map;
25 import java.util.Set;
26
27 import org.hipparchus.Field;
28 import org.hipparchus.CalculusFieldElement;
29 import org.hipparchus.analysis.CalculusFieldUnivariateVectorFunction;
30 import org.hipparchus.analysis.UnivariateVectorFunction;
31 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
32 import org.hipparchus.geometry.euclidean.threed.Vector3D;
33 import org.hipparchus.util.FastMath;
34 import org.hipparchus.util.FieldSinCos;
35 import org.hipparchus.util.MathArrays;
36 import org.hipparchus.util.SinCos;
37 import org.orekit.attitudes.Attitude;
38 import org.orekit.attitudes.AttitudeProvider;
39 import org.orekit.attitudes.FieldAttitude;
40 import org.orekit.forces.ForceModel;
41 import org.orekit.orbits.EquinoctialOrbit;
42 import org.orekit.orbits.FieldEquinoctialOrbit;
43 import org.orekit.orbits.FieldOrbit;
44 import org.orekit.orbits.Orbit;
45 import org.orekit.orbits.OrbitType;
46 import org.orekit.orbits.PositionAngle;
47 import org.orekit.propagation.FieldSpacecraftState;
48 import org.orekit.propagation.PropagationType;
49 import org.orekit.propagation.SpacecraftState;
50 import org.orekit.propagation.semianalytical.dsst.utilities.AuxiliaryElements;
51 import org.orekit.propagation.semianalytical.dsst.utilities.CjSjCoefficient;
52 import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
53 import org.orekit.propagation.semianalytical.dsst.utilities.FieldCjSjCoefficient;
54 import org.orekit.propagation.semianalytical.dsst.utilities.FieldShortPeriodicsInterpolatedCoefficient;
55 import org.orekit.propagation.semianalytical.dsst.utilities.ShortPeriodicsInterpolatedCoefficient;
56 import org.orekit.time.AbsoluteDate;
57 import org.orekit.time.FieldAbsoluteDate;
58 import org.orekit.utils.FieldTimeSpanMap;
59 import org.orekit.utils.ParameterDriver;
60 import org.orekit.utils.TimeSpanMap;
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98 public abstract class AbstractGaussianContribution implements DSSTForceModel {
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114 private static final int I = 1;
115
116
117
118
119
120
121
122
123 private static final double MU_SCALE = FastMath.scalb(1.0, 32);
124
125
126 private static final int[] GAUSS_ORDER = { 12, 16, 20, 24, 32, 40, 48 };
127
128
129 private static final int MAX_ORDER_RANK = GAUSS_ORDER.length - 1;
130
131
132 private static final int INTERPOLATION_POINTS = 3;
133
134
135 private static final int JMAX = 12;
136
137
138 private final ForceModel contribution;
139
140
141 private final double threshold;
142
143
144 private GaussQuadrature integrator;
145
146
147 private boolean isDirty;
148
149
150 private AttitudeProvider attitudeProvider;
151
152
153 private final String coefficientsKeyPrefix;
154
155
156 private GaussianShortPeriodicCoefficients gaussianSPCoefs;
157
158
159 private Map<Field<?>, FieldGaussianShortPeriodicCoefficients<?>> gaussianFieldSPCoefs;
160
161
162 private final ParameterDriver gmParameterDriver;
163
164
165
166
167
168
169
170
171
172
173 protected AbstractGaussianContribution(final String coefficientsKeyPrefix, final double threshold,
174 final ForceModel contribution, final double mu) {
175
176 gmParameterDriver = new ParameterDriver(DSSTNewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT, mu, MU_SCALE,
177 0.0, Double.POSITIVE_INFINITY);
178
179 this.coefficientsKeyPrefix = coefficientsKeyPrefix;
180 this.contribution = contribution;
181 this.threshold = threshold;
182 this.integrator = new GaussQuadrature(GAUSS_ORDER[MAX_ORDER_RANK]);
183 this.isDirty = true;
184
185 gaussianFieldSPCoefs = new HashMap<>();
186 }
187
188
189 @Override
190 public void init(final SpacecraftState initialState, final AbsoluteDate target) {
191
192 contribution.init(initialState, target);
193 }
194
195
196 @Override
197 public List<ParameterDriver> getParametersDrivers() {
198
199
200 final List<ParameterDriver> drivers = new ArrayList<>();
201
202
203 for (final ParameterDriver driver : getParametersDriversWithoutMu()) {
204 drivers.add(driver);
205 }
206
207
208 drivers.add(gmParameterDriver);
209 return drivers;
210
211 }
212
213
214
215
216
217
218
219
220
221
222
223 protected abstract List<ParameterDriver> getParametersDriversWithoutMu();
224
225
226 @Override
227 public List<ShortPeriodTerms> initializeShortPeriodTerms(final AuxiliaryElements auxiliaryElements, final PropagationType type,
228 final double[] parameters) {
229
230 final List<ShortPeriodTerms> list = new ArrayList<ShortPeriodTerms>();
231 gaussianSPCoefs = new GaussianShortPeriodicCoefficients(coefficientsKeyPrefix, JMAX, INTERPOLATION_POINTS,
232 new TimeSpanMap<Slot>(new Slot(JMAX, INTERPOLATION_POINTS)));
233 list.add(gaussianSPCoefs);
234 return list;
235
236 }
237
238
239 @Override
240 public <T extends CalculusFieldElement<T>> List<FieldShortPeriodTerms<T>> initializeShortPeriodTerms(
241 final FieldAuxiliaryElements<T> auxiliaryElements, final PropagationType type, final T[] parameters) {
242
243 final Field<T> field = auxiliaryElements.getDate().getField();
244
245 final FieldGaussianShortPeriodicCoefficients<T> fgspc = new FieldGaussianShortPeriodicCoefficients<>(
246 coefficientsKeyPrefix, JMAX, INTERPOLATION_POINTS,
247 new FieldTimeSpanMap<>(new FieldSlot<>(JMAX, INTERPOLATION_POINTS), field));
248 gaussianFieldSPCoefs.put(field, fgspc);
249 return Collections.singletonList(fgspc);
250 }
251
252
253
254
255
256
257
258
259
260
261 private AbstractGaussianContributionContext initializeStep(final AuxiliaryElements auxiliaryElements,
262 final double[] parameters) {
263 return new AbstractGaussianContributionContext(auxiliaryElements, parameters);
264 }
265
266
267
268
269
270
271
272
273
274
275
276 private <T extends CalculusFieldElement<T>> FieldAbstractGaussianContributionContext<T> initializeStep(
277 final FieldAuxiliaryElements<T> auxiliaryElements, final T[] parameters) {
278 return new FieldAbstractGaussianContributionContext<>(auxiliaryElements, parameters);
279 }
280
281
282 @Override
283 public double[] getMeanElementRate(final SpacecraftState state, final AuxiliaryElements auxiliaryElements,
284 final double[] parameters) {
285
286
287 final AbstractGaussianContributionContext context = initializeStep(auxiliaryElements, parameters);
288 double[] meanElementRate = new double[6];
289
290 final double[] ll = getLLimits(state, auxiliaryElements);
291
292 if (ll[0] < ll[1]) {
293 meanElementRate = getMeanElementRate(state, integrator, ll[0], ll[1], context, parameters);
294 if (isDirty) {
295 boolean next = true;
296 for (int i = 0; i < MAX_ORDER_RANK && next; i++) {
297 final double[] meanRates = getMeanElementRate(state, new GaussQuadrature(GAUSS_ORDER[i]), ll[0],
298 ll[1], context, parameters);
299 if (getRatesDiff(meanElementRate, meanRates, context) < threshold) {
300 integrator = new GaussQuadrature(GAUSS_ORDER[i]);
301 next = false;
302 }
303 }
304 isDirty = false;
305 }
306 }
307 return meanElementRate;
308 }
309
310
311 @Override
312 public <T extends CalculusFieldElement<T>> T[] getMeanElementRate(final FieldSpacecraftState<T> state,
313 final FieldAuxiliaryElements<T> auxiliaryElements, final T[] parameters) {
314
315
316 final FieldAbstractGaussianContributionContext<T> context = initializeStep(auxiliaryElements, parameters);
317 final Field<T> field = state.getDate().getField();
318
319 T[] meanElementRate = MathArrays.buildArray(field, 6);
320
321 final T[] ll = getLLimits(state, auxiliaryElements);
322
323 if (ll[0].getReal() < ll[1].getReal()) {
324 meanElementRate = getMeanElementRate(state, integrator, ll[0], ll[1], context, parameters);
325 if (isDirty) {
326 boolean next = true;
327 for (int i = 0; i < MAX_ORDER_RANK && next; i++) {
328 final T[] meanRates = getMeanElementRate(state, new GaussQuadrature(GAUSS_ORDER[i]), ll[0], ll[1],
329 context, parameters);
330 if (getRatesDiff(meanElementRate, meanRates, context).getReal() < threshold) {
331 integrator = new GaussQuadrature(GAUSS_ORDER[i]);
332 next = false;
333 }
334 }
335 isDirty = false;
336 }
337 }
338
339 return meanElementRate;
340 }
341
342
343
344
345
346
347
348
349
350 protected abstract double[] getLLimits(SpacecraftState state, AuxiliaryElements auxiliaryElements);
351
352
353
354
355
356
357
358
359
360
361 protected abstract <T extends CalculusFieldElement<T>> T[] getLLimits(FieldSpacecraftState<T> state,
362 FieldAuxiliaryElements<T> auxiliaryElements);
363
364
365
366
367
368
369
370
371
372
373
374
375 protected double[] getMeanElementRate(final SpacecraftState state, final GaussQuadrature gauss, final double low,
376 final double high, final AbstractGaussianContributionContext context, final double[] parameters) {
377
378
379 final AuxiliaryElements auxiliaryElements = context.getAuxiliaryElements();
380
381 final double[] meanElementRate = gauss.integrate(new IntegrableFunction(state, true, 0, parameters), low, high);
382
383
384 final double coef = 1. / (2. * FastMath.PI * auxiliaryElements.getB());
385
386 for (int i = 0; i < 6; i++) {
387 meanElementRate[i] *= coef;
388 }
389 return meanElementRate;
390 }
391
392
393
394
395
396
397
398
399
400
401
402
403
404 protected <T extends CalculusFieldElement<T>> T[] getMeanElementRate(final FieldSpacecraftState<T> state,
405 final GaussQuadrature gauss, final T low, final T high,
406 final FieldAbstractGaussianContributionContext<T> context, final T[] parameters) {
407
408
409 final Field<T> field = context.getA().getField();
410
411
412 final FieldAuxiliaryElements<T> auxiliaryElements = context.getFieldAuxiliaryElements();
413
414 final T[] meanElementRate = gauss.integrate(new FieldIntegrableFunction<>(state, true, 0, parameters, field),
415 low, high, field);
416
417 final T coef = auxiliaryElements.getB().multiply(low.getPi()).multiply(2.).reciprocal();
418
419 for (int i = 0; i < 6; i++) {
420 meanElementRate[i] = meanElementRate[i].multiply(coef);
421 }
422 return meanElementRate;
423 }
424
425
426
427
428
429
430
431
432
433
434 private double getRatesDiff(final double[] meanRef, final double[] meanCur,
435 final AbstractGaussianContributionContext context) {
436
437
438 final AuxiliaryElements auxiliaryElements = context.getAuxiliaryElements();
439
440 double maxDiff = FastMath.abs(meanRef[0] - meanCur[0]) / auxiliaryElements.getSma();
441
442 for (int i = 1; i < meanRef.length; i++) {
443 maxDiff = FastMath.max(maxDiff, FastMath.abs(meanRef[i] - meanCur[i]));
444 }
445 return maxDiff;
446 }
447
448
449
450
451
452
453
454
455
456
457
458 private <T extends CalculusFieldElement<T>> T getRatesDiff(final T[] meanRef, final T[] meanCur,
459 final FieldAbstractGaussianContributionContext<T> context) {
460
461
462 final FieldAuxiliaryElements<T> auxiliaryElements = context.getFieldAuxiliaryElements();
463
464 T maxDiff = FastMath.abs(meanRef[0].subtract(meanCur[0])).divide(auxiliaryElements.getSma());
465 ;
466
467 for (int i = 1; i < meanRef.length; i++) {
468 maxDiff = FastMath.max(maxDiff, FastMath.abs(meanRef[i].subtract(meanCur[i])));
469 }
470 return maxDiff;
471 }
472
473
474 @Override
475 public void registerAttitudeProvider(final AttitudeProvider provider) {
476 this.attitudeProvider = provider;
477 }
478
479
480 @Override
481 public void updateShortPeriodTerms(final double[] parameters, final SpacecraftState... meanStates) {
482
483 final Slot slot = gaussianSPCoefs.createSlot(meanStates);
484 for (final SpacecraftState meanState : meanStates) {
485
486
487 final AuxiliaryElements auxiliaryElements = new AuxiliaryElements(meanState.getOrbit(), I);
488
489
490 final AbstractGaussianContributionContext context = initializeStep(auxiliaryElements, parameters);
491
492
493 final double[][] currentRhoSigmaj = computeRhoSigmaCoefficients(meanState.getDate(), auxiliaryElements);
494
495
496 final FourierCjSjCoefficients fourierCjSj = new FourierCjSjCoefficients(meanState, JMAX, auxiliaryElements,
497 parameters);
498
499
500 final UijVijCoefficients uijvij = new UijVijCoefficients(currentRhoSigmaj, fourierCjSj, JMAX);
501
502 gaussianSPCoefs.computeCoefficients(meanState, slot, fourierCjSj, uijvij, context.getMeanMotion(),
503 auxiliaryElements.getSma());
504
505 }
506
507 }
508
509
510 @Override
511 @SuppressWarnings("unchecked")
512 public <T extends CalculusFieldElement<T>> void updateShortPeriodTerms(final T[] parameters,
513 final FieldSpacecraftState<T>... meanStates) {
514
515
516 final Field<T> field = meanStates[0].getDate().getField();
517
518 final FieldGaussianShortPeriodicCoefficients<T> fgspc = (FieldGaussianShortPeriodicCoefficients<T>) gaussianFieldSPCoefs
519 .get(field);
520 final FieldSlot<T> slot = fgspc.createSlot(meanStates);
521 for (final FieldSpacecraftState<T> meanState : meanStates) {
522
523
524 final FieldAuxiliaryElements<T> auxiliaryElements = new FieldAuxiliaryElements<>(meanState.getOrbit(), I);
525
526
527 final FieldAbstractGaussianContributionContext<T> context = initializeStep(auxiliaryElements, parameters);
528
529
530 final T[][] currentRhoSigmaj = computeRhoSigmaCoefficients(meanState.getDate(), context, field);
531
532
533 final FieldFourierCjSjCoefficients<T> fourierCjSj = new FieldFourierCjSjCoefficients<>(meanState, JMAX,
534 auxiliaryElements, parameters, field);
535
536
537 final FieldUijVijCoefficients<T> uijvij = new FieldUijVijCoefficients<>(currentRhoSigmaj, fourierCjSj, JMAX,
538 field);
539
540 fgspc.computeCoefficients(meanState, slot, fourierCjSj, uijvij, context.getMeanMotion(),
541 auxiliaryElements.getSma(), field);
542
543 }
544
545 }
546
547
548
549
550
551
552
553
554
555
556
557
558 private double[][] computeRhoSigmaCoefficients(final AbsoluteDate date, final AuxiliaryElements auxiliaryElements) {
559 final double[][] currentRhoSigmaj = new double[2][3 * JMAX + 1];
560 final CjSjCoefficient cjsjKH = new CjSjCoefficient(auxiliaryElements.getK(), auxiliaryElements.getH());
561 final double b = 1. / (1 + auxiliaryElements.getB());
562
563
564 double mbtj = 1;
565
566 for (int j = 1; j <= 3 * JMAX; j++) {
567
568
569 mbtj *= -b;
570 final double coef = (1 + j * auxiliaryElements.getB()) * mbtj;
571 currentRhoSigmaj[0][j] = coef * cjsjKH.getCj(j);
572 currentRhoSigmaj[1][j] = coef * cjsjKH.getSj(j);
573 }
574 return currentRhoSigmaj;
575 }
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590 private <T extends CalculusFieldElement<T>> T[][] computeRhoSigmaCoefficients(final FieldAbsoluteDate<T> date,
591 final FieldAbstractGaussianContributionContext<T> context, final Field<T> field) {
592
593 final T zero = field.getZero();
594
595 final FieldAuxiliaryElements<T> auxiliaryElements = context.getFieldAuxiliaryElements();
596 final T[][] currentRhoSigmaj = MathArrays.buildArray(field, 2, 3 * JMAX + 1);
597 final FieldCjSjCoefficient<T> cjsjKH = new FieldCjSjCoefficient<>(auxiliaryElements.getK(),
598 auxiliaryElements.getH(), field);
599 final T b = auxiliaryElements.getB().add(1.).reciprocal();
600
601
602 T mbtj = zero.add(1.);
603
604 for (int j = 1; j <= 3 * JMAX; j++) {
605
606
607 mbtj = mbtj.multiply(b.negate());
608 final T coef = mbtj.multiply(auxiliaryElements.getB().multiply(j).add(1.));
609 currentRhoSigmaj[0][j] = coef.multiply(cjsjKH.getCj(j));
610 currentRhoSigmaj[1][j] = coef.multiply(cjsjKH.getSj(j));
611 }
612 return currentRhoSigmaj;
613 }
614
615
616
617
618
619
620
621 protected class FieldIntegrableFunction<T extends CalculusFieldElement<T>>
622 implements CalculusFieldUnivariateVectorFunction<T> {
623
624
625 private final FieldSpacecraftState<T> state;
626
627
628
629
630
631 private final boolean meanMode;
632
633
634
635
636
637
638
639 private final int j;
640
641
642 private final FieldAbstractGaussianContributionContext<T> context;
643
644
645 private final FieldAuxiliaryElements<T> auxiliaryElements;
646
647
648 private final T[] parameters;
649
650
651
652
653
654
655
656
657
658
659
660
661 public FieldIntegrableFunction(final FieldSpacecraftState<T> state, final boolean meanMode, final int j,
662 final T[] parameters, final Field<T> field) {
663
664 this.meanMode = meanMode;
665 this.j = j;
666 this.parameters = parameters.clone();
667 this.auxiliaryElements = new FieldAuxiliaryElements<>(state.getOrbit(), I);
668 this.context = new FieldAbstractGaussianContributionContext<>(auxiliaryElements, this.parameters);
669
670 final T[] stateVector = MathArrays.buildArray(field, 6);
671 OrbitType.EQUINOCTIAL.mapOrbitToArray(state.getOrbit(), PositionAngle.TRUE, stateVector, null);
672 final FieldOrbit<T> fixedOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(stateVector, null,
673 PositionAngle.TRUE, state.getDate(), context.getMu(), state.getFrame());
674 this.state = new FieldSpacecraftState<>(fixedOrbit, state.getAttitude(), state.getMass());
675 }
676
677
678 @Override
679 public T[] value(final T x) {
680
681
682 final Field<T> field = auxiliaryElements.getDate().getField();
683 final int dimension = 6;
684
685
686 final T shiftedLm = trueToMean(x);
687 final T dLm = shiftedLm.subtract(auxiliaryElements.getLM());
688 final T dt = dLm.divide(context.getMeanMotion());
689
690 final FieldSinCos<T> scL = FastMath.sinCos(x);
691 final T cosL = scL.cos();
692 final T sinL = scL.sin();
693 final T roa = auxiliaryElements.getB().multiply(auxiliaryElements.getB()).divide(auxiliaryElements.getH().multiply(sinL).add(auxiliaryElements.getK().multiply(cosL)).add(1.));
694 final T roa2 = roa.multiply(roa);
695 final T r = auxiliaryElements.getSma().multiply(roa);
696 final T X = r.multiply(cosL);
697 final T Y = r.multiply(sinL);
698 final T naob = context.getMeanMotion().multiply(auxiliaryElements.getSma())
699 .divide(auxiliaryElements.getB());
700 final T Xdot = naob.multiply(auxiliaryElements.getH().add(sinL)).negate();
701 final T Ydot = naob.multiply(auxiliaryElements.getK().add(cosL));
702 final FieldVector3D<T> vel = new FieldVector3D<>(Xdot, auxiliaryElements.getVectorF(), Ydot,
703 auxiliaryElements.getVectorG());
704
705
706 FieldVector3D<T> acc = FieldVector3D.getZero(field);
707
708
709 final FieldOrbit<T> shiftedOrbit = state.getOrbit().shiftedBy(dt);
710
711
712 final FieldOrbit<T> recomposedOrbit = new FieldEquinoctialOrbit<>(shiftedOrbit.getA(),
713 shiftedOrbit.getEquinoctialEx(), shiftedOrbit.getEquinoctialEy(), shiftedOrbit.getHx(),
714 shiftedOrbit.getHy(), shiftedOrbit.getLv(), PositionAngle.TRUE, shiftedOrbit.getFrame(),
715 state.getDate(), context.getMu());
716
717
718 final FieldAttitude<T> recomposedAttitude = attitudeProvider.getAttitude(recomposedOrbit,
719 recomposedOrbit.getDate(), recomposedOrbit.getFrame());
720
721
722 final FieldSpacecraftState<T> shiftedState = new FieldSpacecraftState<>(recomposedOrbit, recomposedAttitude,
723 state.getMass());
724
725 acc = contribution.acceleration(shiftedState, parameters);
726
727
728 final T[] deriv = MathArrays.buildArray(field, dimension);
729
730 deriv[0] = getAoV(vel).dotProduct(acc);
731
732 deriv[1] = getKoV(X, Y, Xdot, Ydot).dotProduct(acc);
733
734 deriv[2] = getHoV(X, Y, Xdot, Ydot).dotProduct(acc);
735
736 deriv[3] = getQoV(X).dotProduct(acc);
737
738 deriv[4] = getPoV(Y).dotProduct(acc);
739
740 deriv[5] = getLoV(X, Y, Xdot, Ydot).dotProduct(acc);
741
742
743 T[] val = null;
744 if (meanMode) {
745 val = MathArrays.buildArray(field, dimension);
746 for (int i = 0; i < 6; i++) {
747
748 val[i] = deriv[i].multiply(roa2);
749 }
750 } else {
751 val = MathArrays.buildArray(field, dimension * 2);
752
753 final FieldSinCos<T> scjL = FastMath.sinCos(x.multiply(j));
754 final T cosjL = j == 1 ? cosL : scjL.cos();
755 final T sinjL = j == 1 ? sinL : scjL.sin();
756
757 for (int i = 0; i < 6; i++) {
758
759 val[i] = deriv[i].multiply(cosjL);
760
761 val[i + 6] = deriv[i].multiply(sinjL);
762 }
763 }
764
765 return val;
766 }
767
768
769
770
771
772
773 private T trueToMean(final T x) {
774 return eccentricToMean(trueToEccentric(x));
775 }
776
777
778
779
780
781
782 private T trueToEccentric (final T lv) {
783 final FieldSinCos<T> sclV = FastMath.sinCos(lv);
784 final T cosLv = sclV.cos();
785 final T sinLv = sclV.sin();
786 final T num = auxiliaryElements.getH().multiply(cosLv).subtract(auxiliaryElements.getK().multiply(sinLv));
787 final T den = auxiliaryElements.getB().add(auxiliaryElements.getK().multiply(cosLv)).add(auxiliaryElements.getH().multiply(sinLv)).add(1.);
788 return FastMath.atan(num.divide(den)).multiply(2.).add(lv);
789 }
790
791
792
793
794
795
796 private T eccentricToMean (final T le) {
797 final FieldSinCos<T> scle = FastMath.sinCos(le);
798 return le.subtract(auxiliaryElements.getK().multiply(scle.sin())).add(auxiliaryElements.getH().multiply(scle.cos()));
799 }
800
801
802
803
804
805
806 private FieldVector3D<T> getAoV(final FieldVector3D<T> vel) {
807 return new FieldVector3D<>(context.getTon2a(), vel);
808 }
809
810
811
812
813
814
815
816
817
818
819
820
821
822 private FieldVector3D<T> getHoV(final T X, final T Y, final T Xdot, final T Ydot) {
823 final T kf = (Xdot.multiply(Y).multiply(2.).subtract(X.multiply(Ydot))).multiply(context.getOoMU());
824 final T kg = X.multiply(Xdot).multiply(context.getOoMU());
825 final T kw = auxiliaryElements.getK().multiply(
826 auxiliaryElements.getQ().multiply(Y).multiply(I).subtract(auxiliaryElements.getP().multiply(X)))
827 .multiply(context.getOOAB());
828 return new FieldVector3D<>(kf, auxiliaryElements.getVectorF(), kg.negate(), auxiliaryElements.getVectorG(),
829 kw, auxiliaryElements.getVectorW());
830 }
831
832
833
834
835
836
837
838
839
840
841
842
843
844 private FieldVector3D<T> getKoV(final T X, final T Y, final T Xdot, final T Ydot) {
845 final T kf = Y.multiply(Ydot).multiply(context.getOoMU());
846 final T kg = (X.multiply(Ydot).multiply(2.).subtract(Xdot.multiply(Y))).multiply(context.getOoMU());
847 final T kw = auxiliaryElements.getH().multiply(
848 auxiliaryElements.getQ().multiply(Y).multiply(I).subtract(auxiliaryElements.getP().multiply(X)))
849 .multiply(context.getOOAB());
850 return new FieldVector3D<>(kf.negate(), auxiliaryElements.getVectorF(), kg, auxiliaryElements.getVectorG(),
851 kw.negate(), auxiliaryElements.getVectorW());
852 }
853
854
855
856
857
858
859
860 private FieldVector3D<T> getPoV(final T Y) {
861 return new FieldVector3D<>(context.getCo2AB().multiply(Y), auxiliaryElements.getVectorW());
862 }
863
864
865
866
867
868
869
870 private FieldVector3D<T> getQoV(final T X) {
871 return new FieldVector3D<>(context.getCo2AB().multiply(X).multiply(I), auxiliaryElements.getVectorW());
872 }
873
874
875
876
877
878
879
880
881
882
883
884
885
886 private FieldVector3D<T> getLoV(final T X, final T Y, final T Xdot, final T Ydot) {
887 final FieldVector3D<T> pos = new FieldVector3D<>(X, auxiliaryElements.getVectorF(), Y,
888 auxiliaryElements.getVectorG());
889 final FieldVector3D<T> v2 = new FieldVector3D<>(auxiliaryElements.getK(), getHoV(X, Y, Xdot, Ydot),
890 auxiliaryElements.getH().negate(), getKoV(X, Y, Xdot, Ydot));
891 return new FieldVector3D<>(context.getOOA().multiply(-2.), pos, context.getOoBpo(), v2,
892 context.getOOA().multiply(auxiliaryElements.getQ().multiply(Y).multiply(I)
893 .subtract(auxiliaryElements.getP().multiply(X))),
894 auxiliaryElements.getVectorW());
895 }
896
897 }
898
899
900 protected class IntegrableFunction implements UnivariateVectorFunction {
901
902
903 private final SpacecraftState state;
904
905
906
907
908
909 private final boolean meanMode;
910
911
912
913
914
915
916
917 private final int j;
918
919
920 private final AbstractGaussianContributionContext context;
921
922
923 private final AuxiliaryElements auxiliaryElements;
924
925
926 private final double[] parameters;
927
928
929
930
931
932
933
934
935
936
937
938 IntegrableFunction(final SpacecraftState state, final boolean meanMode, final int j,
939 final double[] parameters) {
940
941 this.meanMode = meanMode;
942 this.j = j;
943 this.parameters = parameters.clone();
944 this.auxiliaryElements = new AuxiliaryElements(state.getOrbit(), I);
945 this.context = new AbstractGaussianContributionContext(auxiliaryElements, this.parameters);
946
947 final double[] stateVector = new double[6];
948 OrbitType.EQUINOCTIAL.mapOrbitToArray(state.getOrbit(), PositionAngle.TRUE, stateVector, null);
949 final Orbit fixedOrbit = OrbitType.EQUINOCTIAL.mapArrayToOrbit(stateVector, null, PositionAngle.TRUE,
950 state.getDate(), context.getMu(), state.getFrame());
951 this.state = new SpacecraftState(fixedOrbit, state.getAttitude(), state.getMass());
952 }
953
954
955 @Override
956 public double[] value(final double x) {
957
958
959 final double shiftedLm = trueToMean(x);
960 final double dLm = shiftedLm - auxiliaryElements.getLM();
961 final double dt = dLm / context.getMeanMotion();
962
963 final SinCos scL = FastMath.sinCos(x);
964 final double cosL = scL.cos();
965 final double sinL = scL.sin();
966 final double roa = auxiliaryElements.getB() * auxiliaryElements.getB() / (1. + auxiliaryElements.getH() * sinL + auxiliaryElements.getK() * cosL);
967 final double roa2 = roa * roa;
968 final double r = auxiliaryElements.getSma() * roa;
969 final double X = r * cosL;
970 final double Y = r * sinL;
971 final double naob = context.getMeanMotion() * auxiliaryElements.getSma() / auxiliaryElements.getB();
972 final double Xdot = -naob * (auxiliaryElements.getH() + sinL);
973 final double Ydot = naob * (auxiliaryElements.getK() + cosL);
974 final Vector3D vel = new Vector3D(Xdot, auxiliaryElements.getVectorF(), Ydot,
975 auxiliaryElements.getVectorG());
976
977
978 Vector3D acc = Vector3D.ZERO;
979
980
981 final Orbit shiftedOrbit = state.getOrbit().shiftedBy(dt);
982
983
984 final Orbit recomposedOrbit = new EquinoctialOrbit(shiftedOrbit.getA(), shiftedOrbit.getEquinoctialEx(),
985 shiftedOrbit.getEquinoctialEy(), shiftedOrbit.getHx(), shiftedOrbit.getHy(), shiftedOrbit.getLv(),
986 PositionAngle.TRUE, shiftedOrbit.getFrame(), state.getDate(), context.getMu());
987
988
989 final Attitude recomposedAttitude = attitudeProvider.getAttitude(recomposedOrbit, recomposedOrbit.getDate(),
990 recomposedOrbit.getFrame());
991
992
993 final SpacecraftState shiftedState = new SpacecraftState(recomposedOrbit, recomposedAttitude,
994 state.getMass());
995
996 acc = contribution.acceleration(shiftedState, parameters);
997
998
999 final double[] deriv = new double[6];
1000
1001 deriv[0] = getAoV(vel).dotProduct(acc);
1002
1003 deriv[1] = getKoV(X, Y, Xdot, Ydot).dotProduct(acc);
1004
1005 deriv[2] = getHoV(X, Y, Xdot, Ydot).dotProduct(acc);
1006
1007 deriv[3] = getQoV(X).dotProduct(acc);
1008
1009 deriv[4] = getPoV(Y).dotProduct(acc);
1010
1011 deriv[5] = getLoV(X, Y, Xdot, Ydot).dotProduct(acc);
1012
1013
1014 double[] val = null;
1015 if (meanMode) {
1016 val = new double[6];
1017 for (int i = 0; i < 6; i++) {
1018
1019 val[i] = roa2 * deriv[i];
1020 }
1021 } else {
1022 val = new double[12];
1023
1024 final SinCos scjL = FastMath.sinCos(j * x);
1025 final double cosjL = j == 1 ? cosL : scjL.cos();
1026 final double sinjL = j == 1 ? sinL : scjL.sin();
1027
1028 for (int i = 0; i < 6; i++) {
1029
1030 val[i] = cosjL * deriv[i];
1031
1032 val[i + 6] = sinjL * deriv[i];
1033 }
1034 }
1035 return val;
1036 }
1037
1038
1039
1040
1041
1042
1043 private double trueToEccentric (final double lv) {
1044 final SinCos scLv = FastMath.sinCos(lv);
1045 final double num = auxiliaryElements.getH() * scLv.cos() - auxiliaryElements.getK() * scLv.sin();
1046 final double den = auxiliaryElements.getB() + 1. + auxiliaryElements.getK() * scLv.cos() + auxiliaryElements.getH() * scLv.sin();
1047 return lv + 2. * FastMath.atan(num / den);
1048 }
1049
1050
1051
1052
1053
1054
1055 private double eccentricToMean (final double le) {
1056 final SinCos scLe = FastMath.sinCos(le);
1057 return le - auxiliaryElements.getK() * scLe.sin() + auxiliaryElements.getH() * scLe.cos();
1058 }
1059
1060
1061
1062
1063
1064
1065 private double trueToMean(final double lv) {
1066 return eccentricToMean(trueToEccentric(lv));
1067 }
1068
1069
1070
1071
1072
1073
1074 private Vector3D getAoV(final Vector3D vel) {
1075 return new Vector3D(context.getTon2a(), vel);
1076 }
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090 private Vector3D getHoV(final double X, final double Y, final double Xdot, final double Ydot) {
1091 final double kf = (2. * Xdot * Y - X * Ydot) * context.getOoMU();
1092 final double kg = X * Xdot * context.getOoMU();
1093 final double kw = auxiliaryElements.getK() *
1094 (I * auxiliaryElements.getQ() * Y - auxiliaryElements.getP() * X) * context.getOOAB();
1095 return new Vector3D(kf, auxiliaryElements.getVectorF(), -kg, auxiliaryElements.getVectorG(), kw,
1096 auxiliaryElements.getVectorW());
1097 }
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111 private Vector3D getKoV(final double X, final double Y, final double Xdot, final double Ydot) {
1112 final double kf = Y * Ydot * context.getOoMU();
1113 final double kg = (2. * X * Ydot - Xdot * Y) * context.getOoMU();
1114 final double kw = auxiliaryElements.getH() *
1115 (I * auxiliaryElements.getQ() * Y - auxiliaryElements.getP() * X) * context.getOOAB();
1116 return new Vector3D(-kf, auxiliaryElements.getVectorF(), kg, auxiliaryElements.getVectorG(), -kw,
1117 auxiliaryElements.getVectorW());
1118 }
1119
1120
1121
1122
1123
1124
1125
1126 private Vector3D getPoV(final double Y) {
1127 return new Vector3D(context.getCo2AB() * Y, auxiliaryElements.getVectorW());
1128 }
1129
1130
1131
1132
1133
1134
1135
1136 private Vector3D getQoV(final double X) {
1137 return new Vector3D(I * context.getCo2AB() * X, auxiliaryElements.getVectorW());
1138 }
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152 private Vector3D getLoV(final double X, final double Y, final double Xdot, final double Ydot) {
1153 final Vector3D pos = new Vector3D(X, auxiliaryElements.getVectorF(), Y, auxiliaryElements.getVectorG());
1154 final Vector3D v2 = new Vector3D(auxiliaryElements.getK(), getHoV(X, Y, Xdot, Ydot),
1155 -auxiliaryElements.getH(), getKoV(X, Y, Xdot, Ydot));
1156 return new Vector3D(-2. * context.getOOA(), pos, context.getOoBpo(), v2,
1157 (I * auxiliaryElements.getQ() * Y - auxiliaryElements.getP() * X) * context.getOOA(),
1158 auxiliaryElements.getVectorW());
1159 }
1160
1161 }
1162
1163
1164
1165
1166
1167
1168
1169 protected static class GaussQuadrature {
1170
1171
1172
1173
1174 private static final double[] P_12 = { -0.98156063424671910000, -0.90411725637047490000,
1175 -0.76990267419430470000, -0.58731795428661740000, -0.36783149899818024000, -0.12523340851146890000,
1176 0.12523340851146890000, 0.36783149899818024000, 0.58731795428661740000, 0.76990267419430470000,
1177 0.90411725637047490000, 0.98156063424671910000 };
1178
1179
1180 private static final double[] W_12 = { 0.04717533638651220000, 0.10693932599531830000, 0.16007832854334633000,
1181 0.20316742672306584000, 0.23349253653835478000, 0.24914704581340286000, 0.24914704581340286000,
1182 0.23349253653835478000, 0.20316742672306584000, 0.16007832854334633000, 0.10693932599531830000,
1183 0.04717533638651220000 };
1184
1185
1186 private static final double[] P_16 = { -0.98940093499164990000, -0.94457502307323260000,
1187 -0.86563120238783160000, -0.75540440835500310000, -0.61787624440264380000, -0.45801677765722737000,
1188 -0.28160355077925890000, -0.09501250983763745000, 0.09501250983763745000, 0.28160355077925890000,
1189 0.45801677765722737000, 0.61787624440264380000, 0.75540440835500310000, 0.86563120238783160000,
1190 0.94457502307323260000, 0.98940093499164990000 };
1191
1192
1193 private static final double[] W_16 = { 0.02715245941175405800, 0.06225352393864777000, 0.09515851168249283000,
1194 0.12462897125553388000, 0.14959598881657685000, 0.16915651939500256000, 0.18260341504492360000,
1195 0.18945061045506847000, 0.18945061045506847000, 0.18260341504492360000, 0.16915651939500256000,
1196 0.14959598881657685000, 0.12462897125553388000, 0.09515851168249283000, 0.06225352393864777000,
1197 0.02715245941175405800 };
1198
1199
1200 private static final double[] P_20 = { -0.99312859918509490000, -0.96397192727791390000,
1201 -0.91223442825132600000, -0.83911697182221890000, -0.74633190646015080000, -0.63605368072651510000,
1202 -0.51086700195082700000, -0.37370608871541955000, -0.22778585114164507000, -0.07652652113349734000,
1203 0.07652652113349734000, 0.22778585114164507000, 0.37370608871541955000, 0.51086700195082700000,
1204 0.63605368072651510000, 0.74633190646015080000, 0.83911697182221890000, 0.91223442825132600000,
1205 0.96397192727791390000, 0.99312859918509490000 };
1206
1207
1208 private static final double[] W_20 = { 0.01761400713915226400, 0.04060142980038684000, 0.06267204833410904000,
1209 0.08327674157670477000, 0.10193011981724048000, 0.11819453196151844000, 0.13168863844917678000,
1210 0.14209610931838212000, 0.14917298647260380000, 0.15275338713072600000, 0.15275338713072600000,
1211 0.14917298647260380000, 0.14209610931838212000, 0.13168863844917678000, 0.11819453196151844000,
1212 0.10193011981724048000, 0.08327674157670477000, 0.06267204833410904000, 0.04060142980038684000,
1213 0.01761400713915226400 };
1214
1215
1216 private static final double[] P_24 = { -0.99518721999702130000, -0.97472855597130950000,
1217 -0.93827455200273270000, -0.88641552700440100000, -0.82000198597390300000, -0.74012419157855440000,
1218 -0.64809365193697550000, -0.54542147138883950000, -0.43379350762604520000, -0.31504267969616340000,
1219 -0.19111886747361634000, -0.06405689286260563000, 0.06405689286260563000, 0.19111886747361634000,
1220 0.31504267969616340000, 0.43379350762604520000, 0.54542147138883950000, 0.64809365193697550000,
1221 0.74012419157855440000, 0.82000198597390300000, 0.88641552700440100000, 0.93827455200273270000,
1222 0.97472855597130950000, 0.99518721999702130000 };
1223
1224
1225 private static final double[] W_24 = { 0.01234122979998733500, 0.02853138862893380600, 0.04427743881741981000,
1226 0.05929858491543691500, 0.07334648141108027000, 0.08619016153195320000, 0.09761865210411391000,
1227 0.10744427011596558000, 0.11550566805372553000, 0.12167047292780335000, 0.12583745634682825000,
1228 0.12793819534675221000, 0.12793819534675221000, 0.12583745634682825000, 0.12167047292780335000,
1229 0.11550566805372553000, 0.10744427011596558000, 0.09761865210411391000, 0.08619016153195320000,
1230 0.07334648141108027000, 0.05929858491543691500, 0.04427743881741981000, 0.02853138862893380600,
1231 0.01234122979998733500 };
1232
1233
1234 private static final double[] P_32 = { -0.99726386184948160000, -0.98561151154526840000,
1235 -0.96476225558750640000, -0.93490607593773970000, -0.89632115576605220000, -0.84936761373256990000,
1236 -0.79448379596794250000, -0.73218211874028970000, -0.66304426693021520000, -0.58771575724076230000,
1237 -0.50689990893222950000, -0.42135127613063540000, -0.33186860228212767000, -0.23928736225213710000,
1238 -0.14447196158279646000, -0.04830766568773831000, 0.04830766568773831000, 0.14447196158279646000,
1239 0.23928736225213710000, 0.33186860228212767000, 0.42135127613063540000, 0.50689990893222950000,
1240 0.58771575724076230000, 0.66304426693021520000, 0.73218211874028970000, 0.79448379596794250000,
1241 0.84936761373256990000, 0.89632115576605220000, 0.93490607593773970000, 0.96476225558750640000,
1242 0.98561151154526840000, 0.99726386184948160000 };
1243
1244
1245 private static final double[] W_32 = { 0.00701861000947013600, 0.01627439473090571200, 0.02539206530926214200,
1246 0.03427386291302141000, 0.04283589802222658600, 0.05099805926237621600, 0.05868409347853559000,
1247 0.06582222277636193000, 0.07234579410884862000, 0.07819389578707042000, 0.08331192422694673000,
1248 0.08765209300440380000, 0.09117387869576390000, 0.09384439908080441000, 0.09563872007927487000,
1249 0.09654008851472784000, 0.09654008851472784000, 0.09563872007927487000, 0.09384439908080441000,
1250 0.09117387869576390000, 0.08765209300440380000, 0.08331192422694673000, 0.07819389578707042000,
1251 0.07234579410884862000, 0.06582222277636193000, 0.05868409347853559000, 0.05099805926237621600,
1252 0.04283589802222658600, 0.03427386291302141000, 0.02539206530926214200, 0.01627439473090571200,
1253 0.00701861000947013600 };
1254
1255
1256 private static final double[] P_40 = { -0.99823770971055930000, -0.99072623869945710000,
1257 -0.97725994998377420000, -0.95791681921379170000, -0.93281280827867660000, -0.90209880696887420000,
1258 -0.86595950321225960000, -0.82461223083331170000, -0.77830565142651940000, -0.72731825518992710000,
1259 -0.67195668461417960000, -0.61255388966798030000, -0.54946712509512820000, -0.48307580168617870000,
1260 -0.41377920437160500000, -0.34199409082575850000, -0.26815218500725370000, -0.19269758070137110000,
1261 -0.11608407067525522000, -0.03877241750605081600, 0.03877241750605081600, 0.11608407067525522000,
1262 0.19269758070137110000, 0.26815218500725370000, 0.34199409082575850000, 0.41377920437160500000,
1263 0.48307580168617870000, 0.54946712509512820000, 0.61255388966798030000, 0.67195668461417960000,
1264 0.72731825518992710000, 0.77830565142651940000, 0.82461223083331170000, 0.86595950321225960000,
1265 0.90209880696887420000, 0.93281280827867660000, 0.95791681921379170000, 0.97725994998377420000,
1266 0.99072623869945710000, 0.99823770971055930000 };
1267
1268
1269 private static final double[] W_40 = { 0.00452127709853309800, 0.01049828453115270400, 0.01642105838190797300,
1270 0.02224584919416689000, 0.02793700698002338000, 0.03346019528254786500, 0.03878216797447199000,
1271 0.04387090818567333000, 0.04869580763507221000, 0.05322784698393679000, 0.05743976909939157000,
1272 0.06130624249292891000, 0.06480401345660108000, 0.06791204581523394000, 0.07061164739128681000,
1273 0.07288658239580408000, 0.07472316905796833000, 0.07611036190062619000, 0.07703981816424793000,
1274 0.07750594797842482000, 0.07750594797842482000, 0.07703981816424793000, 0.07611036190062619000,
1275 0.07472316905796833000, 0.07288658239580408000, 0.07061164739128681000, 0.06791204581523394000,
1276 0.06480401345660108000, 0.06130624249292891000, 0.05743976909939157000, 0.05322784698393679000,
1277 0.04869580763507221000, 0.04387090818567333000, 0.03878216797447199000, 0.03346019528254786500,
1278 0.02793700698002338000, 0.02224584919416689000, 0.01642105838190797300, 0.01049828453115270400,
1279 0.00452127709853309800 };
1280
1281
1282 private static final double[] P_48 = { -0.99877100725242610000, -0.99353017226635080000,
1283 -0.98412458372282700000, -0.97059159254624720000, -0.95298770316043080000, -0.93138669070655440000,
1284 -0.90587913671556960000, -0.87657202027424800000, -0.84358826162439350000, -0.80706620402944250000,
1285 -0.76715903251574020000, -0.72403413092381470000, -0.67787237963266400000, -0.62886739677651370000,
1286 -0.57722472608397270000, -0.52316097472223300000, -0.46690290475095840000, -0.40868648199071680000,
1287 -0.34875588629216070000, -0.28736248735545555000, -0.22476379039468908000, -0.16122235606889174000,
1288 -0.09700469920946270000, -0.03238017096286937000, 0.03238017096286937000, 0.09700469920946270000,
1289 0.16122235606889174000, 0.22476379039468908000, 0.28736248735545555000, 0.34875588629216070000,
1290 0.40868648199071680000, 0.46690290475095840000, 0.52316097472223300000, 0.57722472608397270000,
1291 0.62886739677651370000, 0.67787237963266400000, 0.72403413092381470000, 0.76715903251574020000,
1292 0.80706620402944250000, 0.84358826162439350000, 0.87657202027424800000, 0.90587913671556960000,
1293 0.93138669070655440000, 0.95298770316043080000, 0.97059159254624720000, 0.98412458372282700000,
1294 0.99353017226635080000, 0.99877100725242610000 };
1295
1296
1297 private static final double[] W_48 = { 0.00315334605230596250, 0.00732755390127620800, 0.01147723457923446900,
1298 0.01557931572294386600, 0.01961616045735556700, 0.02357076083932435600, 0.02742650970835688000,
1299 0.03116722783279807000, 0.03477722256477045000, 0.03824135106583080600, 0.04154508294346483000,
1300 0.04467456085669424000, 0.04761665849249054000, 0.05035903555385448000, 0.05289018948519365000,
1301 0.05519950369998416500, 0.05727729210040315000, 0.05911483969839566000, 0.06070443916589384000,
1302 0.06203942315989268000, 0.06311419228625403000, 0.06392423858464817000, 0.06446616443595010000,
1303 0.06473769681268386000, 0.06473769681268386000, 0.06446616443595010000, 0.06392423858464817000,
1304 0.06311419228625403000, 0.06203942315989268000, 0.06070443916589384000, 0.05911483969839566000,
1305 0.05727729210040315000, 0.05519950369998416500, 0.05289018948519365000, 0.05035903555385448000,
1306 0.04761665849249054000, 0.04467456085669424000, 0.04154508294346483000, 0.03824135106583080600,
1307 0.03477722256477045000, 0.03116722783279807000, 0.02742650970835688000, 0.02357076083932435600,
1308 0.01961616045735556700, 0.01557931572294386600, 0.01147723457923446900, 0.00732755390127620800,
1309 0.00315334605230596250 };
1310
1311
1312 private final double[] nodePoints;
1313
1314
1315 private final double[] nodeWeights;
1316
1317
1318 private final int numberOfPoints;
1319
1320
1321
1322
1323
1324
1325 GaussQuadrature(final int numberOfPoints) {
1326
1327 this.numberOfPoints = numberOfPoints;
1328
1329 switch (numberOfPoints) {
1330 case 12:
1331 this.nodePoints = P_12.clone();
1332 this.nodeWeights = W_12.clone();
1333 break;
1334 case 16:
1335 this.nodePoints = P_16.clone();
1336 this.nodeWeights = W_16.clone();
1337 break;
1338 case 20:
1339 this.nodePoints = P_20.clone();
1340 this.nodeWeights = W_20.clone();
1341 break;
1342 case 24:
1343 this.nodePoints = P_24.clone();
1344 this.nodeWeights = W_24.clone();
1345 break;
1346 case 32:
1347 this.nodePoints = P_32.clone();
1348 this.nodeWeights = W_32.clone();
1349 break;
1350 case 40:
1351 this.nodePoints = P_40.clone();
1352 this.nodeWeights = W_40.clone();
1353 break;
1354 case 48:
1355 default:
1356 this.nodePoints = P_48.clone();
1357 this.nodeWeights = W_48.clone();
1358 break;
1359 }
1360
1361 }
1362
1363
1364
1365
1366
1367
1368
1369
1370
1371 public double[] integrate(final UnivariateVectorFunction f, final double lowerBound, final double upperBound) {
1372
1373 final double[] adaptedPoints = nodePoints.clone();
1374 final double[] adaptedWeights = nodeWeights.clone();
1375 transform(adaptedPoints, adaptedWeights, lowerBound, upperBound);
1376 return basicIntegrate(f, adaptedPoints, adaptedWeights);
1377 }
1378
1379
1380
1381
1382
1383
1384
1385
1386
1387
1388
1389 public <T extends CalculusFieldElement<T>> T[] integrate(final CalculusFieldUnivariateVectorFunction<T> f,
1390 final T lowerBound, final T upperBound, final Field<T> field) {
1391
1392 final T zero = field.getZero();
1393
1394 final T[] adaptedPoints = MathArrays.buildArray(field, numberOfPoints);
1395 final T[] adaptedWeights = MathArrays.buildArray(field, numberOfPoints);
1396
1397 for (int i = 0; i < numberOfPoints; i++) {
1398 adaptedPoints[i] = zero.add(nodePoints[i]);
1399 adaptedWeights[i] = zero.add(nodeWeights[i]);
1400 }
1401
1402 transform(adaptedPoints, adaptedWeights, lowerBound, upperBound);
1403 return basicIntegrate(f, adaptedPoints, adaptedWeights, field);
1404 }
1405
1406
1407
1408
1409
1410
1411
1412
1413
1414
1415
1416
1417
1418 private void transform(final double[] points, final double[] weights, final double a, final double b) {
1419
1420 final double scale = (b - a) / 2;
1421 final double shift = a + scale;
1422 for (int i = 0; i < points.length; i++) {
1423 points[i] = points[i] * scale + shift;
1424 weights[i] *= scale;
1425 }
1426 }
1427
1428
1429
1430
1431
1432
1433
1434
1435
1436
1437
1438
1439
1440 private <T extends CalculusFieldElement<T>> void transform(final T[] points, final T[] weights, final T a,
1441 final T b) {
1442
1443 final T scale = (b.subtract(a)).divide(2.);
1444 final T shift = a.add(scale);
1445 for (int i = 0; i < points.length; i++) {
1446 points[i] = scale.multiply(points[i]).add(shift);
1447 weights[i] = scale.multiply(weights[i]);
1448 }
1449 }
1450
1451
1452
1453
1454
1455
1456
1457
1458
1459
1460
1461 private double[] basicIntegrate(final UnivariateVectorFunction f, final double[] points,
1462 final double[] weights) {
1463 double x = points[0];
1464 double w = weights[0];
1465 double[] v = f.value(x);
1466 final double[] y = new double[v.length];
1467 for (int j = 0; j < v.length; j++) {
1468 y[j] = w * v[j];
1469 }
1470 final double[] t = y.clone();
1471 final double[] c = new double[v.length];
1472 final double[] s = t.clone();
1473 for (int i = 1; i < points.length; i++) {
1474 x = points[i];
1475 w = weights[i];
1476 v = f.value(x);
1477 for (int j = 0; j < v.length; j++) {
1478 y[j] = w * v[j] - c[j];
1479 t[j] = s[j] + y[j];
1480 c[j] = (t[j] - s[j]) - y[j];
1481 s[j] = t[j];
1482 }
1483 }
1484 return s;
1485 }
1486
1487
1488
1489
1490
1491
1492
1493
1494
1495
1496
1497
1498
1499 private <T extends CalculusFieldElement<T>> T[] basicIntegrate(final CalculusFieldUnivariateVectorFunction<T> f,
1500 final T[] points, final T[] weights, final Field<T> field) {
1501
1502 T x = points[0];
1503 T w = weights[0];
1504 T[] v = f.value(x);
1505
1506 final T[] y = MathArrays.buildArray(field, v.length);
1507 for (int j = 0; j < v.length; j++) {
1508 y[j] = v[j].multiply(w);
1509 }
1510 final T[] t = y.clone();
1511 final T[] c = MathArrays.buildArray(field, v.length);
1512 ;
1513 final T[] s = t.clone();
1514 for (int i = 1; i < points.length; i++) {
1515 x = points[i];
1516 w = weights[i];
1517 v = f.value(x);
1518 for (int j = 0; j < v.length; j++) {
1519 y[j] = v[j].multiply(w).subtract(c[j]);
1520 t[j] = y[j].add(s[j]);
1521 c[j] = (t[j].subtract(s[j])).subtract(y[j]);
1522 s[j] = t[j];
1523 }
1524 }
1525 return s;
1526 }
1527
1528 }
1529
1530
1531
1532
1533
1534
1535
1536
1537
1538
1539 protected class FourierCjSjCoefficients {
1540
1541
1542 private final int jMax;
1543
1544
1545
1546
1547
1548
1549
1550
1551
1552
1553
1554
1555
1556 private final double[][] cCoef;
1557
1558
1559
1560
1561
1562
1563
1564
1565
1566
1567
1568
1569
1570 private final double[][] sCoef;
1571
1572
1573
1574
1575
1576
1577
1578
1579 FourierCjSjCoefficients(final SpacecraftState state, final int jMax, final AuxiliaryElements auxiliaryElements,
1580 final double[] parameters) {
1581
1582
1583 this.jMax = jMax;
1584
1585
1586 final int rows = jMax + 1;
1587 cCoef = new double[rows][6];
1588 sCoef = new double[rows][6];
1589
1590
1591 computeCoefficients(state, auxiliaryElements, parameters);
1592 }
1593
1594
1595
1596
1597
1598
1599
1600
1601
1602
1603
1604 private void computeCoefficients(final SpacecraftState state, final AuxiliaryElements auxiliaryElements,
1605 final double[] parameters) {
1606
1607
1608 final double[] ll = getLLimits(state, auxiliaryElements);
1609
1610 if (ll[0] < ll[1]) {
1611
1612 final double ooPI = 1 / FastMath.PI;
1613
1614
1615 for (int j = 0; j <= jMax; j++) {
1616 final double[] curentCoefficients = integrator
1617 .integrate(new IntegrableFunction(state, false, j, parameters), ll[0], ll[1]);
1618
1619
1620 for (int i = 0; i < 6; i++) {
1621 cCoef[j][i] = ooPI * curentCoefficients[i];
1622 sCoef[j][i] = ooPI * curentCoefficients[i + 6];
1623 }
1624 }
1625 }
1626 }
1627
1628
1629
1630
1631
1632
1633
1634 public double getCij(final int i, final int j) {
1635 return cCoef[j][i];
1636 }
1637
1638
1639
1640
1641
1642
1643
1644 public double getSij(final int i, final int j) {
1645 return sCoef[j][i];
1646 }
1647 }
1648
1649
1650
1651
1652
1653
1654
1655
1656
1657
1658 protected class FieldFourierCjSjCoefficients<T extends CalculusFieldElement<T>> {
1659
1660
1661 private final int jMax;
1662
1663
1664
1665
1666
1667
1668
1669
1670
1671
1672
1673
1674
1675 private final T[][] cCoef;
1676
1677
1678
1679
1680
1681
1682
1683
1684
1685
1686
1687
1688
1689 private final T[][] sCoef;
1690
1691
1692
1693
1694
1695
1696
1697
1698
1699 FieldFourierCjSjCoefficients(final FieldSpacecraftState<T> state, final int jMax,
1700 final FieldAuxiliaryElements<T> auxiliaryElements, final T[] parameters, final Field<T> field) {
1701
1702 this.jMax = jMax;
1703
1704
1705 final int rows = jMax + 1;
1706 cCoef = MathArrays.buildArray(field, rows, 6);
1707 sCoef = MathArrays.buildArray(field, rows, 6);
1708
1709
1710 computeCoefficients(state, auxiliaryElements, parameters, field);
1711 }
1712
1713
1714
1715
1716
1717
1718
1719
1720
1721
1722
1723
1724 private void computeCoefficients(final FieldSpacecraftState<T> state,
1725 final FieldAuxiliaryElements<T> auxiliaryElements, final T[] parameters, final Field<T> field) {
1726
1727 final T zero = field.getZero();
1728
1729 final T[] ll = getLLimits(state, auxiliaryElements);
1730
1731 if (ll[0].getReal() < ll[1].getReal()) {
1732
1733 final T ooPI = zero.getPi().reciprocal();
1734
1735
1736 for (int j = 0; j <= jMax; j++) {
1737 final T[] curentCoefficients = integrator.integrate(
1738 new FieldIntegrableFunction<>(state, false, j, parameters, field), ll[0], ll[1], field);
1739
1740
1741 for (int i = 0; i < 6; i++) {
1742 cCoef[j][i] = curentCoefficients[i].multiply(ooPI);
1743 sCoef[j][i] = curentCoefficients[i + 6].multiply(ooPI);
1744 }
1745 }
1746 }
1747 }
1748
1749
1750
1751
1752
1753
1754
1755 public T getCij(final int i, final int j) {
1756 return cCoef[j][i];
1757 }
1758
1759
1760
1761
1762
1763
1764
1765 public T getSij(final int i, final int j) {
1766 return sCoef[j][i];
1767 }
1768 }
1769
1770
1771
1772
1773
1774
1775
1776
1777
1778
1779
1780
1781
1782
1783 protected static class GaussianShortPeriodicCoefficients implements ShortPeriodTerms {
1784
1785
1786 private final int jMax;
1787
1788
1789 private final int interpolationPoints;
1790
1791
1792 private final String coefficientsKeyPrefix;
1793
1794
1795 private final transient TimeSpanMap<Slot> slots;
1796
1797
1798
1799
1800
1801
1802
1803
1804
1805 GaussianShortPeriodicCoefficients(final String coefficientsKeyPrefix, final int jMax,
1806 final int interpolationPoints, final TimeSpanMap<Slot> slots) {
1807
1808 this.jMax = jMax;
1809 this.interpolationPoints = interpolationPoints;
1810 this.coefficientsKeyPrefix = coefficientsKeyPrefix;
1811 this.slots = slots;
1812 }
1813
1814
1815
1816
1817
1818
1819 public Slot createSlot(final SpacecraftState... meanStates) {
1820 final Slot slot = new Slot(jMax, interpolationPoints);
1821 final AbsoluteDate first = meanStates[0].getDate();
1822 final AbsoluteDate last = meanStates[meanStates.length - 1].getDate();
1823 final int compare = first.compareTo(last);
1824 if (compare < 0) {
1825 slots.addValidAfter(slot, first);
1826 } else if (compare > 0) {
1827 slots.addValidBefore(slot, first);
1828 } else {
1829
1830 slots.addValidAfter(slot, AbsoluteDate.PAST_INFINITY);
1831 }
1832 return slot;
1833 }
1834
1835
1836
1837
1838
1839
1840
1841
1842
1843
1844
1845 private void computeCoefficients(final SpacecraftState state, final Slot slot,
1846 final FourierCjSjCoefficients fourierCjSj, final UijVijCoefficients uijvij, final double n,
1847 final double a) {
1848
1849
1850 final AbsoluteDate date = state.getDate();
1851
1852
1853 final double k20 = computeK20(jMax, uijvij.currentRhoSigmaj);
1854
1855
1856 final double oon = 1. / n;
1857
1858 final double to2an = 1.5 * oon / a;
1859
1860 final double to4an = to2an / 2;
1861
1862
1863 final int size = jMax + 1;
1864 final double[] di1 = new double[6];
1865 final double[] di2 = new double[6];
1866 final double[][] currentCij = new double[size][6];
1867 final double[][] currentSij = new double[size][6];
1868 for (int i = 0; i < 6; i++) {
1869
1870
1871 di1[i] = -oon * fourierCjSj.getCij(i, 0);
1872 if (i == 5) {
1873 di1[i] += to2an * uijvij.getU1(0, 0);
1874 }
1875 di2[i] = 0.;
1876 if (i == 5) {
1877 di2[i] += -to4an * fourierCjSj.getCij(0, 0);
1878 }
1879
1880
1881 currentCij[0][i] = -di2[i] * k20;
1882
1883 for (int j = 1; j <= jMax; j++) {
1884
1885 currentCij[j][i] = oon * uijvij.getU1(j, i);
1886 if (i == 5) {
1887 currentCij[j][i] += -to2an * uijvij.getU2(j);
1888 }
1889 currentSij[j][i] = oon * uijvij.getV1(j, i);
1890 if (i == 5) {
1891 currentSij[j][i] += -to2an * uijvij.getV2(j);
1892 }
1893
1894
1895 currentCij[0][i] += -(currentCij[j][i] * uijvij.currentRhoSigmaj[0][j] +
1896 currentSij[j][i] * uijvij.currentRhoSigmaj[1][j]);
1897 }
1898
1899 }
1900
1901
1902 slot.cij[0].addGridPoint(date, currentCij[0]);
1903 slot.dij[1].addGridPoint(date, di1);
1904 slot.dij[2].addGridPoint(date, di2);
1905 for (int j = 1; j <= jMax; j++) {
1906 slot.cij[j].addGridPoint(date, currentCij[j]);
1907 slot.sij[j].addGridPoint(date, currentSij[j]);
1908 }
1909
1910 }
1911
1912
1913
1914
1915
1916
1917
1918
1919
1920
1921
1922
1923
1924 private double computeK20(final int kMax, final double[][] currentRhoSigmaj) {
1925 double k20 = 0.;
1926
1927 for (int kIndex = 1; kIndex <= kMax; kIndex++) {
1928
1929
1930
1931 double currentTerm = currentRhoSigmaj[1][kIndex] * currentRhoSigmaj[1][kIndex] +
1932 currentRhoSigmaj[0][kIndex] * currentRhoSigmaj[0][kIndex];
1933
1934
1935 currentTerm *= 2. / (kIndex * kIndex);
1936
1937
1938 k20 += currentTerm;
1939 }
1940
1941 return k20;
1942 }
1943
1944
1945 @Override
1946 public double[] value(final Orbit meanOrbit) {
1947
1948
1949 final Slot slot = slots.get(meanOrbit.getDate());
1950
1951
1952 final double L = meanOrbit.getLv();
1953
1954
1955 final double center = L - meanOrbit.getLM();
1956
1957 final double center2 = center * center;
1958
1959
1960 final double[] shortPeriodicVariation = slot.cij[0].value(meanOrbit.getDate());
1961 final double[] d1 = slot.dij[1].value(meanOrbit.getDate());
1962 final double[] d2 = slot.dij[2].value(meanOrbit.getDate());
1963 for (int i = 0; i < 6; i++) {
1964 shortPeriodicVariation[i] += center * d1[i] + center2 * d2[i];
1965 }
1966
1967 for (int j = 1; j <= JMAX; j++) {
1968 final double[] c = slot.cij[j].value(meanOrbit.getDate());
1969 final double[] s = slot.sij[j].value(meanOrbit.getDate());
1970 final SinCos sc = FastMath.sinCos(j * L);
1971 final double cos = sc.cos();
1972 final double sin = sc.sin();
1973 for (int i = 0; i < 6; i++) {
1974
1975 shortPeriodicVariation[i] += c[i] * cos;
1976 shortPeriodicVariation[i] += s[i] * sin;
1977 }
1978 }
1979
1980 return shortPeriodicVariation;
1981
1982 }
1983
1984
1985 public String getCoefficientsKeyPrefix() {
1986 return coefficientsKeyPrefix;
1987 }
1988
1989
1990
1991
1992
1993
1994
1995
1996
1997
1998 @Override
1999 public Map<String, double[]> getCoefficients(final AbsoluteDate date, final Set<String> selected) {
2000
2001
2002 final Slot slot = slots.get(date);
2003
2004 final Map<String, double[]> coefficients = new HashMap<String, double[]>(2 * JMAX + 3);
2005 storeIfSelected(coefficients, selected, slot.cij[0].value(date), "d", 0);
2006 storeIfSelected(coefficients, selected, slot.dij[1].value(date), "d", 1);
2007 storeIfSelected(coefficients, selected, slot.dij[2].value(date), "d", 2);
2008 for (int j = 1; j <= JMAX; j++) {
2009 storeIfSelected(coefficients, selected, slot.cij[j].value(date), "c", j);
2010 storeIfSelected(coefficients, selected, slot.sij[j].value(date), "s", j);
2011 }
2012
2013 return coefficients;
2014
2015 }
2016
2017
2018
2019
2020
2021
2022
2023
2024
2025
2026 private void storeIfSelected(final Map<String, double[]> map, final Set<String> selected, final double[] value,
2027 final String id, final int... indices) {
2028 final StringBuilder keyBuilder = new StringBuilder(getCoefficientsKeyPrefix());
2029 keyBuilder.append(id);
2030 for (int index : indices) {
2031 keyBuilder.append('[').append(index).append(']');
2032 }
2033 final String key = keyBuilder.toString();
2034 if (selected.isEmpty() || selected.contains(key)) {
2035 map.put(key, value);
2036 }
2037 }
2038
2039 }
2040
2041
2042
2043
2044
2045
2046
2047
2048
2049
2050
2051
2052
2053
2054 protected static class FieldGaussianShortPeriodicCoefficients<T extends CalculusFieldElement<T>>
2055 implements FieldShortPeriodTerms<T> {
2056
2057
2058 private final int jMax;
2059
2060
2061 private final int interpolationPoints;
2062
2063
2064 private final String coefficientsKeyPrefix;
2065
2066
2067 private final transient FieldTimeSpanMap<FieldSlot<T>, T> slots;
2068
2069
2070
2071
2072
2073
2074
2075
2076
2077 FieldGaussianShortPeriodicCoefficients(final String coefficientsKeyPrefix, final int jMax,
2078 final int interpolationPoints, final FieldTimeSpanMap<FieldSlot<T>, T> slots) {
2079
2080 this.jMax = jMax;
2081 this.interpolationPoints = interpolationPoints;
2082 this.coefficientsKeyPrefix = coefficientsKeyPrefix;
2083 this.slots = slots;
2084 }
2085
2086
2087
2088
2089
2090
2091 @SuppressWarnings("unchecked")
2092 public FieldSlot<T> createSlot(final FieldSpacecraftState<T>... meanStates) {
2093 final FieldSlot<T> slot = new FieldSlot<>(jMax, interpolationPoints);
2094 final FieldAbsoluteDate<T> first = meanStates[0].getDate();
2095 final FieldAbsoluteDate<T> last = meanStates[meanStates.length - 1].getDate();
2096 if (first.compareTo(last) <= 0) {
2097 slots.addValidAfter(slot, first);
2098 } else {
2099 slots.addValidBefore(slot, first);
2100 }
2101 return slot;
2102 }
2103
2104
2105
2106
2107
2108
2109
2110
2111
2112
2113
2114
2115 private void computeCoefficients(final FieldSpacecraftState<T> state, final FieldSlot<T> slot,
2116 final FieldFourierCjSjCoefficients<T> fourierCjSj, final FieldUijVijCoefficients<T> uijvij, final T n,
2117 final T a, final Field<T> field) {
2118
2119
2120 final T zero = field.getZero();
2121
2122
2123 final FieldAbsoluteDate<T> date = state.getDate();
2124
2125
2126 final T k20 = computeK20(jMax, uijvij.currentRhoSigmaj, field);
2127
2128
2129 final T oon = n.reciprocal();
2130
2131 final T to2an = oon.multiply(1.5).divide(a);
2132
2133 final T to4an = to2an.divide(2.);
2134
2135
2136 final int size = jMax + 1;
2137 final T[] di1 = MathArrays.buildArray(field, 6);
2138 final T[] di2 = MathArrays.buildArray(field, 6);
2139 final T[][] currentCij = MathArrays.buildArray(field, size, 6);
2140 final T[][] currentSij = MathArrays.buildArray(field, size, 6);
2141 for (int i = 0; i < 6; i++) {
2142
2143
2144 di1[i] = oon.negate().multiply(fourierCjSj.getCij(i, 0));
2145 if (i == 5) {
2146 di1[i] = di1[i].add(to2an.multiply(uijvij.getU1(0, 0)));
2147 }
2148 di2[i] = zero;
2149 if (i == 5) {
2150 di2[i] = di2[i].add(to4an.negate().multiply(fourierCjSj.getCij(0, 0)));
2151 }
2152
2153
2154 currentCij[0][i] = di2[i].negate().multiply(k20);
2155
2156 for (int j = 1; j <= jMax; j++) {
2157
2158 currentCij[j][i] = oon.multiply(uijvij.getU1(j, i));
2159 if (i == 5) {
2160 currentCij[j][i] = currentCij[j][i].add(to2an.negate().multiply(uijvij.getU2(j)));
2161 }
2162 currentSij[j][i] = oon.multiply(uijvij.getV1(j, i));
2163 if (i == 5) {
2164 currentSij[j][i] = currentSij[j][i].add(to2an.negate().multiply(uijvij.getV2(j)));
2165 }
2166
2167
2168 currentCij[0][i] = currentCij[0][i].add(currentCij[j][i].multiply(uijvij.currentRhoSigmaj[0][j])
2169 .add(currentSij[j][i].multiply(uijvij.currentRhoSigmaj[1][j])).negate());
2170 }
2171
2172 }
2173
2174
2175 slot.cij[0].addGridPoint(date, currentCij[0]);
2176 slot.dij[1].addGridPoint(date, di1);
2177 slot.dij[2].addGridPoint(date, di2);
2178 for (int j = 1; j <= jMax; j++) {
2179 slot.cij[j].addGridPoint(date, currentCij[j]);
2180 slot.sij[j].addGridPoint(date, currentSij[j]);
2181 }
2182
2183 }
2184
2185
2186
2187
2188
2189
2190
2191
2192
2193
2194
2195
2196
2197
2198 private T computeK20(final int kMax, final T[][] currentRhoSigmaj, final Field<T> field) {
2199 final T zero = field.getZero();
2200 T k20 = zero;
2201
2202 for (int kIndex = 1; kIndex <= kMax; kIndex++) {
2203
2204
2205
2206 T currentTerm = currentRhoSigmaj[1][kIndex].multiply(currentRhoSigmaj[1][kIndex])
2207 .add(currentRhoSigmaj[0][kIndex].multiply(currentRhoSigmaj[0][kIndex]));
2208
2209
2210 currentTerm = currentTerm.multiply(2. / (kIndex * kIndex));
2211
2212
2213 k20 = k20.add(currentTerm);
2214 }
2215
2216 return k20;
2217 }
2218
2219
2220 @Override
2221 public T[] value(final FieldOrbit<T> meanOrbit) {
2222
2223
2224 final FieldSlot<T> slot = slots.get(meanOrbit.getDate());
2225
2226
2227 final T L = meanOrbit.getLv();
2228
2229
2230 final T center = L.subtract(meanOrbit.getLM());
2231
2232 final T center2 = center.multiply(center);
2233
2234
2235 final T[] shortPeriodicVariation = slot.cij[0].value(meanOrbit.getDate());
2236 final T[] d1 = slot.dij[1].value(meanOrbit.getDate());
2237 final T[] d2 = slot.dij[2].value(meanOrbit.getDate());
2238 for (int i = 0; i < 6; i++) {
2239 shortPeriodicVariation[i] = shortPeriodicVariation[i]
2240 .add(center.multiply(d1[i]).add(center2.multiply(d2[i])));
2241 }
2242
2243 for (int j = 1; j <= JMAX; j++) {
2244 final T[] c = slot.cij[j].value(meanOrbit.getDate());
2245 final T[] s = slot.sij[j].value(meanOrbit.getDate());
2246 final FieldSinCos<T> sc = FastMath.sinCos(L.multiply(j));
2247 final T cos = sc.cos();
2248 final T sin = sc.sin();
2249 for (int i = 0; i < 6; i++) {
2250
2251 shortPeriodicVariation[i] = shortPeriodicVariation[i].add(c[i].multiply(cos));
2252 shortPeriodicVariation[i] = shortPeriodicVariation[i].add(s[i].multiply(sin));
2253 }
2254 }
2255
2256 return shortPeriodicVariation;
2257
2258 }
2259
2260
2261 public String getCoefficientsKeyPrefix() {
2262 return coefficientsKeyPrefix;
2263 }
2264
2265
2266
2267
2268
2269
2270
2271
2272
2273
2274 @Override
2275 public Map<String, T[]> getCoefficients(final FieldAbsoluteDate<T> date, final Set<String> selected) {
2276
2277
2278 final FieldSlot<T> slot = slots.get(date);
2279
2280 final Map<String, T[]> coefficients = new HashMap<String, T[]>(2 * JMAX + 3);
2281 storeIfSelected(coefficients, selected, slot.cij[0].value(date), "d", 0);
2282 storeIfSelected(coefficients, selected, slot.dij[1].value(date), "d", 1);
2283 storeIfSelected(coefficients, selected, slot.dij[2].value(date), "d", 2);
2284 for (int j = 1; j <= JMAX; j++) {
2285 storeIfSelected(coefficients, selected, slot.cij[j].value(date), "c", j);
2286 storeIfSelected(coefficients, selected, slot.sij[j].value(date), "s", j);
2287 }
2288
2289 return coefficients;
2290
2291 }
2292
2293
2294
2295
2296
2297
2298
2299
2300
2301
2302 private void storeIfSelected(final Map<String, T[]> map, final Set<String> selected, final T[] value,
2303 final String id, final int... indices) {
2304 final StringBuilder keyBuilder = new StringBuilder(getCoefficientsKeyPrefix());
2305 keyBuilder.append(id);
2306 for (int index : indices) {
2307 keyBuilder.append('[').append(index).append(']');
2308 }
2309 final String key = keyBuilder.toString();
2310 if (selected.isEmpty() || selected.contains(key)) {
2311 map.put(key, value);
2312 }
2313 }
2314
2315 }
2316
2317
2318
2319
2320
2321
2322
2323
2324
2325
2326
2327
2328 protected static class UijVijCoefficients {
2329
2330
2331
2332
2333
2334
2335
2336
2337
2338
2339
2340
2341
2342 private final double[][] u1ij;
2343
2344
2345
2346
2347
2348
2349
2350
2351
2352
2353
2354 private final double[][] v1ij;
2355
2356
2357
2358
2359
2360
2361
2362
2363 private final double[] u2ij;
2364
2365
2366
2367
2368
2369
2370
2371
2372 private final double[] v2ij;
2373
2374
2375
2376
2377
2378 private final double[][] currentRhoSigmaj;
2379
2380
2381
2382
2383
2384 private final FourierCjSjCoefficients fourierCjSj;
2385
2386
2387 private final int jMax;
2388
2389
2390
2391
2392
2393
2394
2395
2396
2397 UijVijCoefficients(final double[][] currentRhoSigmaj, final FourierCjSjCoefficients fourierCjSj,
2398 final int jMax) {
2399 this.currentRhoSigmaj = currentRhoSigmaj;
2400 this.fourierCjSj = fourierCjSj;
2401 this.jMax = jMax;
2402
2403
2404 this.u1ij = new double[6][2 * jMax + 1];
2405 this.v1ij = new double[6][2 * jMax + 1];
2406 this.u2ij = new double[jMax + 1];
2407 this.v2ij = new double[jMax + 1];
2408
2409
2410 computeU1V1Coefficients();
2411 computeU2V2Coefficients();
2412 }
2413
2414
2415 private void computeU1V1Coefficients() {
2416
2417
2418
2419 u1ij[0][0] = 0;
2420 for (int j = 1; j <= jMax; j++) {
2421
2422 final double ooj = 1. / j;
2423
2424 for (int i = 0; i < 6; i++) {
2425
2426 u1ij[i][j] = fourierCjSj.getSij(i, j);
2427 v1ij[i][j] = fourierCjSj.getCij(i, j);
2428
2429
2430 if (j > 1) {
2431
2432 for (int kIndex = 1; kIndex <= j - 1; kIndex++) {
2433
2434
2435 u1ij[i][j] += fourierCjSj.getCij(i, j - kIndex) * currentRhoSigmaj[1][kIndex] +
2436 fourierCjSj.getSij(i, j - kIndex) * currentRhoSigmaj[0][kIndex];
2437
2438
2439
2440 v1ij[i][j] += fourierCjSj.getCij(i, j - kIndex) * currentRhoSigmaj[0][kIndex] -
2441 fourierCjSj.getSij(i, j - kIndex) * currentRhoSigmaj[1][kIndex];
2442 }
2443 }
2444
2445
2446
2447 if (j != jMax) {
2448 for (int kIndex = 1; kIndex <= jMax - j; kIndex++) {
2449
2450
2451 u1ij[i][j] += -fourierCjSj.getCij(i, j + kIndex) * currentRhoSigmaj[1][kIndex] +
2452 fourierCjSj.getSij(i, j + kIndex) * currentRhoSigmaj[0][kIndex];
2453
2454
2455
2456 v1ij[i][j] += fourierCjSj.getCij(i, j + kIndex) * currentRhoSigmaj[0][kIndex] +
2457 fourierCjSj.getSij(i, j + kIndex) * currentRhoSigmaj[1][kIndex];
2458 }
2459 }
2460
2461 for (int kIndex = 1; kIndex <= jMax; kIndex++) {
2462
2463
2464 u1ij[i][j] += -fourierCjSj.getCij(i, kIndex) * currentRhoSigmaj[1][j + kIndex] -
2465 fourierCjSj.getSij(i, kIndex) * currentRhoSigmaj[0][j + kIndex];
2466
2467
2468
2469 v1ij[i][j] += fourierCjSj.getCij(i, kIndex) * currentRhoSigmaj[0][j + kIndex] +
2470 fourierCjSj.getSij(i, kIndex) * currentRhoSigmaj[1][j + kIndex];
2471 }
2472
2473
2474 u1ij[i][j] *= -ooj;
2475 v1ij[i][j] *= ooj;
2476
2477
2478 if (i == 0) {
2479
2480 u1ij[0][0] += -u1ij[0][j] * currentRhoSigmaj[0][j] - v1ij[0][j] * currentRhoSigmaj[1][j];
2481 }
2482 }
2483 }
2484
2485
2486
2487
2488 for (int j = jMax + 1; j <= 2 * jMax; j++) {
2489
2490 final double ooj = 1. / j;
2491
2492 u1ij[0][j] = 0.;
2493 v1ij[0][j] = 0.;
2494
2495
2496 for (int kIndex = j - jMax; kIndex <= j - 1; kIndex++) {
2497
2498
2499 u1ij[0][j] += fourierCjSj.getCij(0, j - kIndex) * currentRhoSigmaj[1][kIndex] +
2500 fourierCjSj.getSij(0, j - kIndex) * currentRhoSigmaj[0][kIndex];
2501
2502
2503
2504 v1ij[0][j] += fourierCjSj.getCij(0, j - kIndex) * currentRhoSigmaj[0][kIndex] -
2505 fourierCjSj.getSij(0, j - kIndex) * currentRhoSigmaj[1][kIndex];
2506 }
2507 for (int kIndex = 1; kIndex <= jMax; kIndex++) {
2508
2509
2510 u1ij[0][j] += -fourierCjSj.getCij(0, kIndex) * currentRhoSigmaj[1][j + kIndex] -
2511 fourierCjSj.getSij(0, kIndex) * currentRhoSigmaj[0][j + kIndex];
2512
2513
2514
2515 v1ij[0][j] += fourierCjSj.getCij(0, kIndex) * currentRhoSigmaj[0][j + kIndex] +
2516 fourierCjSj.getSij(0, kIndex) * currentRhoSigmaj[1][j + kIndex];
2517 }
2518
2519
2520 u1ij[0][j] *= -ooj;
2521 v1ij[0][j] *= ooj;
2522 }
2523 }
2524
2525
2526
2527
2528
2529
2530
2531 private void computeU2V2Coefficients() {
2532 for (int j = 1; j <= jMax; j++) {
2533
2534 final double ooj = 1. / j;
2535
2536
2537 u2ij[j] = v1ij[0][j];
2538 v2ij[j] = u1ij[0][j];
2539
2540
2541 if (j > 1) {
2542 for (int l = 1; l <= j - 1; l++) {
2543
2544
2545 u2ij[j] += u1ij[0][j - l] * currentRhoSigmaj[1][l] + v1ij[0][j - l] * currentRhoSigmaj[0][l];
2546
2547
2548
2549 v2ij[j] += u1ij[0][j - l] * currentRhoSigmaj[0][l] - v1ij[0][j - l] * currentRhoSigmaj[1][l];
2550 }
2551 }
2552
2553 for (int l = 1; l <= jMax; l++) {
2554
2555
2556
2557
2558 u2ij[j] += -u1ij[0][j + l] * currentRhoSigmaj[1][l] + u1ij[0][l] * currentRhoSigmaj[1][j + l] +
2559 v1ij[0][j + l] * currentRhoSigmaj[0][l] - v1ij[0][l] * currentRhoSigmaj[0][j + l];
2560
2561
2562
2563
2564
2565 u2ij[j] += u1ij[0][j + l] * currentRhoSigmaj[0][l] + u1ij[0][l] * currentRhoSigmaj[0][j + l] +
2566 v1ij[0][j + l] * currentRhoSigmaj[1][l] + v1ij[0][l] * currentRhoSigmaj[1][j + l];
2567 }
2568
2569
2570 u2ij[j] *= -ooj;
2571 v2ij[j] *= ooj;
2572 }
2573 }
2574
2575
2576
2577
2578
2579
2580
2581
2582 public double getU1(final int j, final int i) {
2583 return u1ij[i][j];
2584 }
2585
2586
2587
2588
2589
2590
2591
2592
2593 public double getV1(final int j, final int i) {
2594 return v1ij[i][j];
2595 }
2596
2597
2598
2599
2600
2601
2602
2603 public double getU2(final int j) {
2604 return u2ij[j];
2605 }
2606
2607
2608
2609
2610
2611
2612
2613 public double getV2(final int j) {
2614 return v2ij[j];
2615 }
2616 }
2617
2618
2619
2620
2621
2622
2623
2624
2625
2626
2627
2628
2629 protected static class FieldUijVijCoefficients<T extends CalculusFieldElement<T>> {
2630
2631
2632
2633
2634
2635
2636
2637
2638
2639
2640
2641
2642
2643 private final T[][] u1ij;
2644
2645
2646
2647
2648
2649
2650
2651
2652
2653
2654
2655 private final T[][] v1ij;
2656
2657
2658
2659
2660
2661
2662
2663
2664 private final T[] u2ij;
2665
2666
2667
2668
2669
2670
2671
2672
2673 private final T[] v2ij;
2674
2675
2676
2677
2678
2679 private final T[][] currentRhoSigmaj;
2680
2681
2682
2683
2684
2685 private final FieldFourierCjSjCoefficients<T> fourierCjSj;
2686
2687
2688 private final int jMax;
2689
2690
2691
2692
2693
2694
2695
2696
2697
2698
2699 FieldUijVijCoefficients(final T[][] currentRhoSigmaj, final FieldFourierCjSjCoefficients<T> fourierCjSj,
2700 final int jMax, final Field<T> field) {
2701 this.currentRhoSigmaj = currentRhoSigmaj;
2702 this.fourierCjSj = fourierCjSj;
2703 this.jMax = jMax;
2704
2705
2706 this.u1ij = MathArrays.buildArray(field, 6, 2 * jMax + 1);
2707 this.v1ij = MathArrays.buildArray(field, 6, 2 * jMax + 1);
2708 this.u2ij = MathArrays.buildArray(field, jMax + 1);
2709 this.v2ij = MathArrays.buildArray(field, jMax + 1);
2710
2711
2712 computeU1V1Coefficients(field);
2713 computeU2V2Coefficients(field);
2714 }
2715
2716
2717
2718
2719
2720 private void computeU1V1Coefficients(final Field<T> field) {
2721
2722 final T zero = field.getZero();
2723
2724
2725
2726
2727 u1ij[0][0] = zero;
2728 for (int j = 1; j <= jMax; j++) {
2729
2730 final double ooj = 1. / j;
2731
2732 for (int i = 0; i < 6; i++) {
2733
2734 u1ij[i][j] = fourierCjSj.getSij(i, j);
2735 v1ij[i][j] = fourierCjSj.getCij(i, j);
2736
2737
2738 if (j > 1) {
2739
2740 for (int kIndex = 1; kIndex <= j - 1; kIndex++) {
2741
2742
2743 u1ij[i][j] = u1ij[i][j]
2744 .add(fourierCjSj.getCij(i, j - kIndex).multiply(currentRhoSigmaj[1][kIndex]).add(
2745 fourierCjSj.getSij(i, j - kIndex).multiply(currentRhoSigmaj[0][kIndex])));
2746
2747
2748
2749 v1ij[i][j] = v1ij[i][j].add(
2750 fourierCjSj.getCij(i, j - kIndex).multiply(currentRhoSigmaj[0][kIndex]).subtract(
2751 fourierCjSj.getSij(i, j - kIndex).multiply(currentRhoSigmaj[1][kIndex])));
2752 }
2753 }
2754
2755
2756
2757 if (j != jMax) {
2758 for (int kIndex = 1; kIndex <= jMax - j; kIndex++) {
2759
2760
2761 u1ij[i][j] = u1ij[i][j].add(fourierCjSj.getCij(i, j + kIndex).negate()
2762 .multiply(currentRhoSigmaj[1][kIndex])
2763 .add(fourierCjSj.getSij(i, j + kIndex).multiply(currentRhoSigmaj[0][kIndex])));
2764
2765
2766
2767 v1ij[i][j] = v1ij[i][j]
2768 .add(fourierCjSj.getCij(i, j + kIndex).multiply(currentRhoSigmaj[0][kIndex]).add(
2769 fourierCjSj.getSij(i, j + kIndex).multiply(currentRhoSigmaj[1][kIndex])));
2770 }
2771 }
2772
2773 for (int kIndex = 1; kIndex <= jMax; kIndex++) {
2774
2775
2776 u1ij[i][j] = u1ij[i][j].add(fourierCjSj.getCij(i, kIndex).negate()
2777 .multiply(currentRhoSigmaj[1][j + kIndex])
2778 .subtract(fourierCjSj.getSij(i, kIndex).multiply(currentRhoSigmaj[0][j + kIndex])));
2779
2780
2781
2782 v1ij[i][j] = v1ij[i][j]
2783 .add(fourierCjSj.getCij(i, kIndex).multiply(currentRhoSigmaj[0][j + kIndex])
2784 .add(fourierCjSj.getSij(i, kIndex).multiply(currentRhoSigmaj[1][j + kIndex])));
2785 }
2786
2787
2788 u1ij[i][j] = u1ij[i][j].multiply(-ooj);
2789 v1ij[i][j] = v1ij[i][j].multiply(ooj);
2790
2791
2792 if (i == 0) {
2793
2794 u1ij[0][0] = u1ij[0][0].add(u1ij[0][j].negate().multiply(currentRhoSigmaj[0][j])
2795 .subtract(v1ij[0][j].multiply(currentRhoSigmaj[1][j])));
2796 }
2797 }
2798 }
2799
2800
2801
2802
2803 for (int j = jMax + 1; j <= 2 * jMax; j++) {
2804
2805 final double ooj = 1. / j;
2806
2807 u1ij[0][j] = zero;
2808 v1ij[0][j] = zero;
2809
2810
2811 for (int kIndex = j - jMax; kIndex <= j - 1; kIndex++) {
2812
2813
2814 u1ij[0][j] = u1ij[0][j].add(fourierCjSj.getCij(0, j - kIndex).multiply(currentRhoSigmaj[1][kIndex])
2815 .add(fourierCjSj.getSij(0, j - kIndex).multiply(currentRhoSigmaj[0][kIndex])));
2816
2817
2818
2819 v1ij[0][j] = v1ij[0][j].add(fourierCjSj.getCij(0, j - kIndex).multiply(currentRhoSigmaj[0][kIndex])
2820 .subtract(fourierCjSj.getSij(0, j - kIndex).multiply(currentRhoSigmaj[1][kIndex])));
2821 }
2822 for (int kIndex = 1; kIndex <= jMax; kIndex++) {
2823
2824
2825 u1ij[0][j] = u1ij[0][j]
2826 .add(fourierCjSj.getCij(0, kIndex).negate().multiply(currentRhoSigmaj[1][j + kIndex])
2827 .subtract(fourierCjSj.getSij(0, kIndex).multiply(currentRhoSigmaj[0][j + kIndex])));
2828
2829
2830
2831 v1ij[0][j] = v1ij[0][j].add(fourierCjSj.getCij(0, kIndex).multiply(currentRhoSigmaj[0][j + kIndex])
2832 .add(fourierCjSj.getSij(0, kIndex).multiply(currentRhoSigmaj[1][j + kIndex])));
2833 }
2834
2835
2836 u1ij[0][j] = u1ij[0][j].multiply(-ooj);
2837 v1ij[0][j] = v1ij[0][j].multiply(ooj);
2838 }
2839 }
2840
2841
2842
2843
2844
2845
2846
2847
2848 private void computeU2V2Coefficients(final Field<T> field) {
2849 for (int j = 1; j <= jMax; j++) {
2850
2851 final double ooj = 1. / j;
2852
2853
2854 u2ij[j] = v1ij[0][j];
2855 v2ij[j] = u1ij[0][j];
2856
2857
2858 if (j > 1) {
2859 for (int l = 1; l <= j - 1; l++) {
2860
2861
2862 u2ij[j] = u2ij[j].add(u1ij[0][j - l].multiply(currentRhoSigmaj[1][l])
2863 .add(v1ij[0][j - l].multiply(currentRhoSigmaj[0][l])));
2864
2865
2866
2867 v2ij[j] = v2ij[j].add(u1ij[0][j - l].multiply(currentRhoSigmaj[0][l])
2868 .subtract(v1ij[0][j - l].multiply(currentRhoSigmaj[1][l])));
2869 }
2870 }
2871
2872 for (int l = 1; l <= jMax; l++) {
2873
2874
2875
2876
2877 u2ij[j] = u2ij[j].add(u1ij[0][j + l].negate().multiply(currentRhoSigmaj[1][l])
2878 .add(u1ij[0][l].multiply(currentRhoSigmaj[1][j + l]))
2879 .add(v1ij[0][j + l].multiply(currentRhoSigmaj[0][l]))
2880 .subtract(v1ij[0][l].multiply(currentRhoSigmaj[0][j + l])));
2881
2882
2883
2884
2885
2886 u2ij[j] = u2ij[j].add(u1ij[0][j + l].multiply(currentRhoSigmaj[0][l])
2887 .add(u1ij[0][l].multiply(currentRhoSigmaj[0][j + l]))
2888 .add(v1ij[0][j + l].multiply(currentRhoSigmaj[1][l]))
2889 .add(v1ij[0][l].multiply(currentRhoSigmaj[1][j + l])));
2890 }
2891
2892
2893 u2ij[j] = u2ij[j].multiply(-ooj);
2894 v2ij[j] = v2ij[j].multiply(ooj);
2895 }
2896 }
2897
2898
2899
2900
2901
2902
2903
2904
2905 public T getU1(final int j, final int i) {
2906 return u1ij[i][j];
2907 }
2908
2909
2910
2911
2912
2913
2914
2915
2916 public T getV1(final int j, final int i) {
2917 return v1ij[i][j];
2918 }
2919
2920
2921
2922
2923
2924
2925
2926 public T getU2(final int j) {
2927 return u2ij[j];
2928 }
2929
2930
2931
2932
2933
2934
2935
2936 public T getV2(final int j) {
2937 return v2ij[j];
2938 }
2939 }
2940
2941
2942 protected static class Slot {
2943
2944
2945
2946
2947
2948
2949
2950
2951
2952
2953
2954
2955
2956 private final ShortPeriodicsInterpolatedCoefficient[] dij;
2957
2958
2959
2960
2961
2962
2963
2964
2965
2966
2967
2968
2969
2970
2971 private final ShortPeriodicsInterpolatedCoefficient[] cij;
2972
2973
2974
2975
2976
2977
2978
2979
2980
2981
2982
2983
2984
2985
2986 private final ShortPeriodicsInterpolatedCoefficient[] sij;
2987
2988
2989
2990
2991
2992
2993 Slot(final int jMax, final int interpolationPoints) {
2994
2995 dij = new ShortPeriodicsInterpolatedCoefficient[3];
2996 cij = new ShortPeriodicsInterpolatedCoefficient[jMax + 1];
2997 sij = new ShortPeriodicsInterpolatedCoefficient[jMax + 1];
2998
2999
3000
3001 for (int j = 0; j <= jMax; j++) {
3002 cij[j] = new ShortPeriodicsInterpolatedCoefficient(interpolationPoints);
3003 if (j > 0) {
3004 sij[j] = new ShortPeriodicsInterpolatedCoefficient(interpolationPoints);
3005 }
3006
3007 if (j == 1 || j == 2) {
3008 dij[j] = new ShortPeriodicsInterpolatedCoefficient(interpolationPoints);
3009 }
3010 }
3011
3012 }
3013
3014 }
3015
3016
3017 protected static class FieldSlot<T extends CalculusFieldElement<T>> {
3018
3019
3020
3021
3022
3023
3024
3025
3026
3027
3028
3029
3030
3031 private final FieldShortPeriodicsInterpolatedCoefficient<T>[] dij;
3032
3033
3034
3035
3036
3037
3038
3039
3040
3041
3042
3043
3044
3045
3046 private final FieldShortPeriodicsInterpolatedCoefficient<T>[] cij;
3047
3048
3049
3050
3051
3052
3053
3054
3055
3056
3057
3058
3059
3060
3061 private final FieldShortPeriodicsInterpolatedCoefficient<T>[] sij;
3062
3063
3064
3065
3066
3067
3068 @SuppressWarnings("unchecked")
3069 FieldSlot(final int jMax, final int interpolationPoints) {
3070
3071 dij = (FieldShortPeriodicsInterpolatedCoefficient<T>[]) Array
3072 .newInstance(FieldShortPeriodicsInterpolatedCoefficient.class, 3);
3073 cij = (FieldShortPeriodicsInterpolatedCoefficient<T>[]) Array
3074 .newInstance(FieldShortPeriodicsInterpolatedCoefficient.class, jMax + 1);
3075 sij = (FieldShortPeriodicsInterpolatedCoefficient<T>[]) Array
3076 .newInstance(FieldShortPeriodicsInterpolatedCoefficient.class, jMax + 1);
3077
3078
3079
3080 for (int j = 0; j <= jMax; j++) {
3081 cij[j] = new FieldShortPeriodicsInterpolatedCoefficient<>(interpolationPoints);
3082 if (j > 0) {
3083 sij[j] = new FieldShortPeriodicsInterpolatedCoefficient<>(interpolationPoints);
3084 }
3085
3086 if (j == 1 || j == 2) {
3087 dij[j] = new FieldShortPeriodicsInterpolatedCoefficient<>(interpolationPoints);
3088 }
3089 }
3090
3091 }
3092
3093 }
3094
3095 }