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