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