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