1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.integration;
18
19 import java.util.ArrayList;
20 import java.util.Arrays;
21 import java.util.Collection;
22 import java.util.Collections;
23 import java.util.HashMap;
24 import java.util.LinkedList;
25 import java.util.List;
26 import java.util.Map;
27 import java.util.Queue;
28
29 import org.hipparchus.CalculusFieldElement;
30 import org.hipparchus.Field;
31 import org.hipparchus.analysis.solvers.FieldBracketingNthOrderBrentSolver;
32 import org.hipparchus.exception.MathIllegalArgumentException;
33 import org.hipparchus.exception.MathIllegalStateException;
34 import org.hipparchus.ode.FieldDenseOutputModel;
35 import org.hipparchus.ode.FieldExpandableODE;
36 import org.hipparchus.ode.FieldODEIntegrator;
37 import org.hipparchus.ode.FieldODEState;
38 import org.hipparchus.ode.FieldODEStateAndDerivative;
39 import org.hipparchus.ode.FieldOrdinaryDifferentialEquation;
40 import org.hipparchus.ode.FieldSecondaryODE;
41 import org.hipparchus.ode.events.Action;
42 import org.hipparchus.ode.events.FieldAdaptableInterval;
43 import org.hipparchus.ode.events.FieldODEEventDetector;
44 import org.hipparchus.ode.events.FieldODEEventHandler;
45 import org.hipparchus.ode.sampling.AbstractFieldODEStateInterpolator;
46 import org.hipparchus.ode.sampling.FieldODEStateInterpolator;
47 import org.hipparchus.ode.sampling.FieldODEStepHandler;
48 import org.hipparchus.util.MathArrays;
49 import org.hipparchus.util.Precision;
50 import org.orekit.attitudes.AttitudeProvider;
51 import org.orekit.errors.OrekitException;
52 import org.orekit.errors.OrekitInternalError;
53 import org.orekit.errors.OrekitMessages;
54 import org.orekit.frames.Frame;
55 import org.orekit.orbits.OrbitType;
56 import org.orekit.orbits.PositionAngleType;
57 import org.orekit.propagation.FieldAbstractPropagator;
58 import org.orekit.propagation.FieldAdditionalDataProvider;
59 import org.orekit.propagation.FieldBoundedPropagator;
60 import org.orekit.propagation.FieldEphemerisGenerator;
61 import org.orekit.propagation.FieldSpacecraftState;
62 import org.orekit.propagation.PropagationType;
63 import org.orekit.propagation.events.FieldEventDetector;
64 import org.orekit.propagation.events.handlers.FieldEventHandler;
65 import org.orekit.propagation.sampling.FieldOrekitStepHandler;
66 import org.orekit.propagation.sampling.FieldOrekitStepInterpolator;
67 import org.orekit.time.FieldAbsoluteDate;
68 import org.orekit.utils.FieldDataDictionary;
69
70
71
72
73
74
75
76 public abstract class FieldAbstractIntegratedPropagator<T extends CalculusFieldElement<T>> extends FieldAbstractPropagator<T> {
77
78
79
80
81 private static final String SECONDARY_DIMENSION = "Orekit-secondary-dimension";
82
83
84 private final List<FieldEventDetector<T>> detectors;
85
86
87 private final List<FieldStoringStepHandler> ephemerisGenerators;
88
89
90 private final FieldODEIntegrator<T> integrator;
91
92
93
94
95 private final Map<String, Integer> secondaryOffsets;
96
97
98
99
100 private final List<FieldAdditionalDerivativesProvider<T>> additionalDerivativesProviders;
101
102
103 private int calls;
104
105
106 private FieldStateMapper<T> stateMapper;
107
108
109
110
111
112 private AttitudeProvider attitudeProviderForDerivatives;
113
114
115 private boolean resetAtEnd;
116
117
118
119
120
121
122
123 private final PropagationType propagationType;
124
125
126
127
128
129
130 protected FieldAbstractIntegratedPropagator(final Field<T> field, final FieldODEIntegrator<T> integrator, final PropagationType propagationType) {
131 super(field);
132 detectors = new ArrayList<>();
133 ephemerisGenerators = new ArrayList<>();
134 additionalDerivativesProviders = new ArrayList<>();
135 this.secondaryOffsets = new HashMap<>();
136 this.integrator = integrator;
137 this.propagationType = propagationType;
138 this.resetAtEnd = true;
139 }
140
141
142
143
144
145
146
147
148
149
150
151
152
153 public void setResetAtEnd(final boolean resetAtEnd) {
154 this.resetAtEnd = resetAtEnd;
155 }
156
157
158
159
160
161 public boolean getResetAtEnd() {
162 return this.resetAtEnd;
163 }
164
165
166
167
168
169 protected AttitudeProvider initializeAttitudeProviderForDerivatives() {
170 return getAttitudeProvider();
171 }
172
173
174
175
176 protected void initMapper(final Field<T> field) {
177 final T zero = field.getZero();
178 stateMapper = createMapper(null, zero.add(Double.NaN), null, null, null, null);
179 }
180
181
182
183
184
185 public String getIntegratorName() {
186 return integrator.getName();
187 }
188
189
190 @Override
191 public void setAttitudeProvider(final AttitudeProvider attitudeProvider) {
192 super.setAttitudeProvider(attitudeProvider);
193 stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
194 stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
195 attitudeProvider, stateMapper.getFrame());
196 }
197
198
199
200
201 protected void setOrbitType(final OrbitType orbitType) {
202 stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
203 orbitType, stateMapper.getPositionAngleType(),
204 stateMapper.getAttitudeProvider(), stateMapper.getFrame());
205 }
206
207
208
209
210 protected OrbitType getOrbitType() {
211 return stateMapper.getOrbitType();
212 }
213
214
215
216
217
218 protected PropagationType isMeanOrbit() {
219 return propagationType;
220 }
221
222
223
224
225
226 public PropagationType getPropagationType() {
227 return propagationType;
228 }
229
230
231
232
233
234
235
236
237
238
239 protected void setPositionAngleType(final PositionAngleType positionAngleType) {
240 stateMapper = createMapper(stateMapper.getReferenceDate(), stateMapper.getMu(),
241 stateMapper.getOrbitType(), positionAngleType,
242 stateMapper.getAttitudeProvider(), stateMapper.getFrame());
243 }
244
245
246
247
248 protected PositionAngleType getPositionAngleType() {
249 return stateMapper.getPositionAngleType();
250 }
251
252
253
254
255 public void setMu(final T mu) {
256 stateMapper = createMapper(stateMapper.getReferenceDate(), mu,
257 stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
258 stateMapper.getAttitudeProvider(), stateMapper.getFrame());
259 }
260
261
262
263
264
265 public T getMu() {
266 return stateMapper.getMu();
267 }
268
269
270
271
272
273
274 public int getCalls() {
275 return calls;
276 }
277
278
279 @Override
280 public boolean isAdditionalDataManaged(final String name) {
281
282
283 if (super.isAdditionalDataManaged(name)) {
284 return true;
285 }
286
287
288 for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
289 if (provider.getName().equals(name)) {
290 return true;
291 }
292 }
293
294 return false;
295 }
296
297
298 @Override
299 public String[] getManagedAdditionalData() {
300 final String[] alreadyIntegrated = super.getManagedAdditionalData();
301 final String[] managed = new String[alreadyIntegrated.length + additionalDerivativesProviders.size()];
302 System.arraycopy(alreadyIntegrated, 0, managed, 0, alreadyIntegrated.length);
303 for (int i = 0; i < additionalDerivativesProviders.size(); ++i) {
304 managed[i + alreadyIntegrated.length] = additionalDerivativesProviders.get(i).getName();
305 }
306 return managed;
307 }
308
309
310
311
312
313
314 public void addAdditionalDerivativesProvider(final FieldAdditionalDerivativesProvider<T> provider) {
315
316 if (this.isAdditionalDataManaged(provider.getName())) {
317
318 throw new OrekitException(OrekitMessages.ADDITIONAL_STATE_NAME_ALREADY_IN_USE,
319 provider.getName());
320 }
321
322
323 additionalDerivativesProviders.add(provider);
324
325 secondaryOffsets.clear();
326
327 }
328
329
330
331
332
333 public List<FieldAdditionalDerivativesProvider<T>> getAdditionalDerivativesProviders() {
334 return Collections.unmodifiableList(additionalDerivativesProviders);
335 }
336
337
338 public <D extends FieldEventDetector<T>> void addEventDetector(final D detector) {
339 detectors.add(detector);
340 }
341
342
343 public Collection<FieldEventDetector<T>> getEventDetectors() {
344 return Collections.unmodifiableCollection(detectors);
345 }
346
347
348 public void clearEventsDetectors() {
349 detectors.clear();
350 }
351
352
353
354 protected void setUpUserEventDetectors() {
355 for (final FieldEventDetector<T> detector : detectors) {
356 setUpEventDetector(integrator, detector);
357 }
358 }
359
360
361
362
363
364 protected void setUpEventDetector(final FieldODEIntegrator<T> integ, final FieldEventDetector<T> detector) {
365 integ.addEventDetector(new FieldAdaptedEventDetector(detector));
366 }
367
368
369
370
371
372 public void clearEphemerisGenerators() {
373 ephemerisGenerators.clear();
374 }
375
376
377 @Override
378 public FieldEphemerisGenerator<T> getEphemerisGenerator() {
379 final FieldStoringStepHandler storingHandler = new FieldStoringStepHandler();
380 ephemerisGenerators.add(storingHandler);
381 return storingHandler;
382 }
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400 protected abstract FieldStateMapper<T> createMapper(FieldAbsoluteDate<T> referenceDate, T mu,
401 OrbitType orbitType, PositionAngleType positionAngleType,
402 AttitudeProvider attitudeProvider, Frame frame);
403
404
405
406
407
408 protected abstract MainStateEquations<T> getMainStateEquations(FieldODEIntegrator<T> integ);
409
410
411 @Override
412 public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> target) {
413 if (getStartDate() == null) {
414 if (getInitialState() == null) {
415 throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
416 }
417 setStartDate(getInitialState().getDate());
418 }
419 return propagate(getStartDate(), target);
420 }
421
422
423 public FieldSpacecraftState<T> propagate(final FieldAbsoluteDate<T> tStart, final FieldAbsoluteDate<T> tEnd) {
424
425 if (getInitialState() == null) {
426 throw new OrekitException(OrekitMessages.INITIAL_STATE_NOT_SPECIFIED_FOR_ORBIT_PROPAGATION);
427 }
428
429
430 try (IntegratorResetter<T> resetter = new IntegratorResetter<>(integrator)) {
431
432
433 initializeAdditionalData(tEnd);
434
435 if (!tStart.equals(getInitialState().getDate())) {
436
437
438 try (IntegratorResetter<T> startResetter = new IntegratorResetter<>(integrator)) {
439 integrateDynamics(tStart);
440 }
441 }
442
443
444 setUpUserEventDetectors();
445
446
447 for (final FieldOrekitStepHandler<T> handler : getMultiplexer().getHandlers()) {
448 integrator.addStepHandler(new FieldAdaptedStepHandler(handler));
449 }
450 for (final FieldStoringStepHandler generator : ephemerisGenerators) {
451 generator.setEndDate(tEnd);
452 integrator.addStepHandler(generator);
453 }
454
455
456 final FieldSpacecraftState<T> state = integrateDynamics(tEnd);
457
458
459 getEventDetectors().forEach(detector -> detector.finish(state));
460
461 return state;
462 }
463
464 }
465
466
467
468
469
470
471
472
473
474
475 public void resetInitialState(final FieldSpacecraftState<T> state, final PropagationType stateType) {
476
477 resetInitialState(state);
478 }
479
480
481
482
483
484 private FieldSpacecraftState<T> integrateDynamics(final FieldAbsoluteDate<T> tEnd) {
485 try {
486
487 initializePropagation();
488
489 if (getInitialState().getDate().equals(tEnd)) {
490
491 return getInitialState();
492 }
493
494 stateMapper = createMapper(getInitialState().getDate(), stateMapper.getMu(),
495 stateMapper.getOrbitType(), stateMapper.getPositionAngleType(),
496 stateMapper.getAttitudeProvider(), getInitialState().getFrame());
497
498
499 if (Double.isNaN(getMu().getReal()) && getInitialState().isOrbitDefined()) {
500 setMu(getInitialState().getOrbit().getMu());
501 }
502
503 if (getInitialState().getMass().getReal() <= 0.0) {
504 throw new OrekitException(OrekitMessages.NOT_POSITIVE_SPACECRAFT_MASS,
505 getInitialState().getMass());
506 }
507
508
509 final FieldSpacecraftState<T> initialIntegrationState = getInitialIntegrationState();
510 final FieldODEState<T> mathInitialState = createInitialState(initialIntegrationState);
511 final FieldExpandableODE<T> mathODE = createODE(integrator);
512
513
514 final FieldODEStateAndDerivative<T> mathFinalState;
515 beforeIntegration(initialIntegrationState, tEnd);
516 mathFinalState = integrator.integrate(mathODE, mathInitialState,
517 tEnd.durationFrom(getInitialState().getDate()));
518
519 afterIntegration();
520
521
522 FieldSpacecraftState<T> finalState =
523 stateMapper.mapArrayToState(stateMapper.mapDoubleToDate(mathFinalState.getTime(), tEnd),
524 mathFinalState.getPrimaryState(),
525 mathFinalState.getPrimaryDerivative(),
526 propagationType);
527
528 finalState = updateAdditionalStatesAndDerivatives(finalState, mathFinalState);
529
530 if (resetAtEnd) {
531 resetInitialState(finalState, propagationType);
532 setStartDate(finalState.getDate());
533 }
534
535 return finalState;
536
537 } catch (OrekitException pe) {
538 throw pe;
539 } catch (MathIllegalArgumentException | MathIllegalStateException me) {
540 throw OrekitException.unwrap(me);
541 }
542 }
543
544
545
546
547
548
549
550
551
552 private FieldSpacecraftState<T> updateAdditionalStatesAndDerivatives(final FieldSpacecraftState<T> originalState,
553 final FieldODEStateAndDerivative<T> os) {
554 FieldSpacecraftState<T> updatedState = originalState;
555 if (os.getNumberOfSecondaryStates() > 0) {
556 final T[] secondary = os.getSecondaryState(1);
557 final T[] secondaryDerivative = os.getSecondaryDerivative(1);
558 for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
559 final String name = provider.getName();
560 final int offset = secondaryOffsets.get(name);
561 final int dimension = provider.getDimension();
562 updatedState = updatedState.addAdditionalData(name, Arrays.copyOfRange(secondary, offset, offset + dimension));
563 updatedState = updatedState.addAdditionalStateDerivative(name, Arrays.copyOfRange(secondaryDerivative, offset, offset + dimension));
564 }
565 }
566 return updateAdditionalData(updatedState);
567 }
568
569
570
571
572 protected FieldSpacecraftState<T> getInitialIntegrationState() {
573 return getInitialState();
574 }
575
576
577
578
579
580 private FieldODEState<T> createInitialState(final FieldSpacecraftState<T> initialState) {
581
582
583 final T[] primary = MathArrays.buildArray(initialState.getMass().getField(), getBasicDimension());
584 stateMapper.mapStateToArray(initialState, primary, null);
585
586 if (secondaryOffsets.isEmpty()) {
587
588 int offset = 0;
589 for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
590 secondaryOffsets.put(provider.getName(), offset);
591 offset += provider.getDimension();
592 }
593 secondaryOffsets.put(SECONDARY_DIMENSION, offset);
594 }
595
596 return new FieldODEState<>(initialState.getMass().getField().getZero(), primary, secondary(initialState));
597
598 }
599
600
601
602
603
604
605 private T[][] secondary(final FieldSpacecraftState<T> state) {
606
607 if (secondaryOffsets.isEmpty()) {
608 return null;
609 }
610
611 final T[][] secondary = MathArrays.buildArray(state.getDate().getField(), 1, secondaryOffsets.get(SECONDARY_DIMENSION));
612 for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
613 final String name = provider.getName();
614 final int offset = secondaryOffsets.get(name);
615 final T[] additional = state.getAdditionalState(name);
616 System.arraycopy(additional, 0, secondary[0], offset, additional.length);
617 }
618
619 return secondary;
620
621 }
622
623
624
625
626
627
628 private T[][] secondaryDerivative(final FieldSpacecraftState<T> state) {
629
630 if (secondaryOffsets.isEmpty()) {
631 return null;
632 }
633
634 final T[][] secondaryDerivative = MathArrays.buildArray(state.getDate().getField(), 1, secondaryOffsets.get(SECONDARY_DIMENSION));
635 for (final FieldAdditionalDerivativesProvider<T> providcer : additionalDerivativesProviders) {
636 final String name = providcer.getName();
637 final int offset = secondaryOffsets.get(name);
638 final T[] additionalDerivative = state.getAdditionalStateDerivative(name);
639 System.arraycopy(additionalDerivative, 0, secondaryDerivative[0], offset, additionalDerivative.length);
640 }
641
642 return secondaryDerivative;
643
644 }
645
646
647
648
649
650 private FieldExpandableODE<T> createODE(final FieldODEIntegrator<T> integ) {
651
652 final FieldExpandableODE<T> ode =
653 new FieldExpandableODE<>(new ConvertedMainStateEquations(getMainStateEquations(integ)));
654
655
656 if (!additionalDerivativesProviders.isEmpty()) {
657 ode.addSecondaryEquations(new ConvertedSecondaryStateEquations());
658 }
659
660 return ode;
661
662 }
663
664
665
666
667
668
669
670
671 protected void beforeIntegration(final FieldSpacecraftState<T> initialState,
672 final FieldAbsoluteDate<T> tEnd) {
673
674 }
675
676
677
678
679
680
681 protected void afterIntegration() {
682
683 }
684
685
686
687
688 public int getBasicDimension() {
689 return 7;
690
691 }
692
693
694
695
696 protected FieldODEIntegrator<T> getIntegrator() {
697 return integrator;
698 }
699
700
701
702
703
704 private FieldSpacecraftState<T> convert(final FieldODEStateAndDerivative<T> os) {
705
706 FieldSpacecraftState<T> s =
707 stateMapper.mapArrayToState(os.getTime(),
708 os.getPrimaryState(),
709 os.getPrimaryDerivative(),
710 propagationType);
711 if (os.getNumberOfSecondaryStates() > 0) {
712 final T[] secondary = os.getSecondaryState(1);
713 final T[] secondaryDerivative = os.getSecondaryDerivative(1);
714 for (final FieldAdditionalDerivativesProvider<T> equations : additionalDerivativesProviders) {
715 final String name = equations.getName();
716 final int offset = secondaryOffsets.get(name);
717 final int dimension = equations.getDimension();
718 s = s.addAdditionalData(name, Arrays.copyOfRange(secondary, offset, offset + dimension));
719 s = s.addAdditionalStateDerivative(name, Arrays.copyOfRange(secondaryDerivative, offset, offset + dimension));
720 }
721 }
722 s = updateAdditionalData(s);
723
724 return s;
725
726 }
727
728
729
730
731
732 private FieldODEStateAndDerivative<T> convert(final FieldSpacecraftState<T> state) {
733
734
735 final T[] primary = MathArrays.buildArray(getField(), getBasicDimension());
736 final T[] primaryDot = MathArrays.buildArray(getField(), getBasicDimension());
737 stateMapper.mapStateToArray(state, primary, primaryDot);
738
739
740 final T[][] secondary = secondary(state);
741 final T[][] secondaryDerivative = secondaryDerivative(state);
742
743 return new FieldODEStateAndDerivative<>(stateMapper.mapDateToDouble(state.getDate()),
744 primary, primaryDot,
745 secondary, secondaryDerivative);
746
747 }
748
749
750
751
752 public interface MainStateEquations<T extends CalculusFieldElement<T>> {
753
754
755
756
757
758
759
760
761
762
763
764 void init(FieldSpacecraftState<T> initialState, FieldAbsoluteDate<T> target);
765
766
767
768
769
770 T[] computeDerivatives(FieldSpacecraftState<T> state);
771
772 }
773
774
775 private class ConvertedMainStateEquations implements FieldOrdinaryDifferentialEquation<T> {
776
777
778 private final MainStateEquations<T> main;
779
780
781
782
783 ConvertedMainStateEquations(final MainStateEquations<T> main) {
784 this.main = main;
785 calls = 0;
786 }
787
788
789 public int getDimension() {
790 return getBasicDimension();
791 }
792
793 @Override
794 public void init(final T t0, final T[] y0, final T finalTime) {
795
796 FieldSpacecraftState<T> initialState = stateMapper.mapArrayToState(t0, y0, null, PropagationType.MEAN);
797 initialState = updateAdditionalData(initialState);
798 initialState = updateStatesFromAdditionalDerivativesIfKnown(initialState);
799 final FieldAbsoluteDate<T> target = stateMapper.mapDoubleToDate(finalTime);
800 main.init(initialState, target);
801 attitudeProviderForDerivatives = initializeAttitudeProviderForDerivatives();
802 }
803
804
805
806
807
808
809
810
811 private FieldSpacecraftState<T> updateStatesFromAdditionalDerivativesIfKnown(final FieldSpacecraftState<T> originalState) {
812 FieldSpacecraftState<T> updatedState = originalState;
813 final FieldSpacecraftState<T> storedInitialState = getInitialState();
814 final T originalTime = stateMapper.mapDateToDouble(originalState.getDate());
815 if (storedInitialState != null && stateMapper.mapDateToDouble(storedInitialState.getDate()).subtract(originalTime).isZero()) {
816 for (final FieldAdditionalDerivativesProvider<T> provider: additionalDerivativesProviders) {
817 final String name = provider.getName();
818 final T[] value = storedInitialState.getAdditionalState(name);
819 updatedState = updatedState.addAdditionalData(name, value);
820 }
821 }
822 return updatedState;
823 }
824
825
826 public T[] computeDerivatives(final T t, final T[] y) {
827
828
829 ++calls;
830
831
832 stateMapper.setAttitudeProvider(attitudeProviderForDerivatives);
833 FieldSpacecraftState<T> currentState = stateMapper.mapArrayToState(t, y, null, PropagationType.MEAN);
834 stateMapper.setAttitudeProvider(getAttitudeProvider());
835 currentState = updateAdditionalData(currentState);
836
837
838 return main.computeDerivatives(currentState);
839
840 }
841
842 }
843
844
845 private class ConvertedSecondaryStateEquations implements FieldSecondaryODE<T> {
846
847
848 private final int combinedDimension;
849
850
851
852 ConvertedSecondaryStateEquations() {
853 this.combinedDimension = secondaryOffsets.get(SECONDARY_DIMENSION);
854 }
855
856
857 @Override
858 public int getDimension() {
859 return combinedDimension;
860 }
861
862
863 @Override
864 public void init(final T t0, final T[] primary0,
865 final T[] secondary0, final T finalTime) {
866
867 final FieldSpacecraftState<T> initialState = convert(t0, primary0, null, secondary0);
868
869 final FieldAbsoluteDate<T> target = stateMapper.mapDoubleToDate(finalTime);
870 for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
871 provider.init(initialState, target);
872 }
873
874 }
875
876
877 @Override
878 public T[] computeDerivatives(final T t, final T[] primary,
879 final T[] primaryDot, final T[] secondary) {
880
881
882
883
884 FieldSpacecraftState<T> updated = convert(t, primary, primaryDot, secondary);
885
886
887 final Queue<FieldAdditionalDerivativesProvider<T>> pending = new LinkedList<>(additionalDerivativesProviders);
888
889
890 final T[] secondaryDot = MathArrays.buildArray(t.getField(), combinedDimension);
891 int yieldCount = 0;
892 while (!pending.isEmpty()) {
893 final FieldAdditionalDerivativesProvider<T> equations = pending.remove();
894 if (equations.yields(updated)) {
895
896
897 pending.add(equations);
898 if (++yieldCount >= pending.size()) {
899
900
901
902 break;
903 }
904 } else {
905
906 final String name = equations.getName();
907 final int offset = secondaryOffsets.get(name);
908 final int dimension = equations.getDimension();
909 final FieldCombinedDerivatives<T> derivatives = equations.combinedDerivatives(updated);
910 final T[] additionalPart = derivatives.getAdditionalDerivatives();
911 final T[] mainPart = derivatives.getMainStateDerivativesIncrements();
912 System.arraycopy(additionalPart, 0, secondaryDot, offset, dimension);
913 updated = updated.addAdditionalStateDerivative(name, additionalPart);
914 if (mainPart != null) {
915
916 for (int i = 0; i < mainPart.length; ++i) {
917 primaryDot[i] = primaryDot[i].add(mainPart[i]);
918 }
919 }
920 yieldCount = 0;
921 }
922 }
923
924 return secondaryDot;
925
926 }
927
928
929
930
931
932
933
934
935 private FieldSpacecraftState<T> convert(final T t, final T[] primary,
936 final T[] primaryDot, final T[] secondary) {
937
938 FieldSpacecraftState<T> initialState = stateMapper.mapArrayToState(t, primary, primaryDot, PropagationType.MEAN);
939
940 for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
941 final String name = provider.getName();
942 final int offset = secondaryOffsets.get(name);
943 final int dimension = provider.getDimension();
944 initialState = initialState.addAdditionalData(name, Arrays.copyOfRange(secondary, offset, offset + dimension));
945 }
946
947 return updateAdditionalData(initialState);
948
949 }
950
951 }
952
953
954
955
956
957 private class FieldAdaptedEventDetector implements FieldODEEventDetector<T> {
958
959
960 private final FieldEventDetector<T> detector;
961
962
963
964
965 private final FieldEventHandler<T> handler;
966
967
968 private T lastT;
969
970
971 private T lastG;
972
973
974
975
976 FieldAdaptedEventDetector(final FieldEventDetector<T> detector) {
977 this.detector = detector;
978 this.handler = detector.getHandler();
979 this.lastT = getField().getZero().add(Double.NaN);
980 this.lastG = getField().getZero().add(Double.NaN);
981 }
982
983
984 @Override
985 public FieldAdaptableInterval<T> getMaxCheckInterval() {
986 return (state, isForward) -> detector.getMaxCheckInterval().currentInterval(convert(state), isForward);
987 }
988
989
990 @Override
991 public int getMaxIterationCount() {
992 return detector.getMaxIterationCount();
993 }
994
995
996 @Override
997 public FieldBracketingNthOrderBrentSolver<T> getSolver() {
998 final T zero = detector.getThreshold().getField().getZero();
999 return new FieldBracketingNthOrderBrentSolver<>(zero, detector.getThreshold(), zero, 5);
1000 }
1001
1002
1003 @Override
1004 public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
1005 detector.init(convert(s0), stateMapper.mapDoubleToDate(t));
1006 this.lastT = getField().getZero().add(Double.NaN);
1007 this.lastG = getField().getZero().add(Double.NaN);
1008 }
1009
1010
1011 @Override
1012 public void reset(final FieldODEStateAndDerivative<T> intermediateState, final T finalTime) {
1013 detector.reset(convert(intermediateState), stateMapper.mapDoubleToDate(finalTime));
1014 this.lastT = getField().getZero().add(Double.NaN);
1015 this.lastG = getField().getZero().add(Double.NaN);
1016 }
1017
1018
1019 public T g(final FieldODEStateAndDerivative<T> s) {
1020 if (!Precision.equals(lastT.getReal(), s.getTime().getReal(), 0)) {
1021 lastT = s.getTime();
1022 lastG = detector.g(convert(s));
1023 }
1024 return lastG;
1025 }
1026
1027
1028 public FieldODEEventHandler<T> getHandler() {
1029
1030 return new FieldODEEventHandler<T>() {
1031
1032
1033 public Action eventOccurred(final FieldODEStateAndDerivative<T> s,
1034 final FieldODEEventDetector<T> d,
1035 final boolean increasing) {
1036 return handler.eventOccurred(convert(s), detector, increasing);
1037 }
1038
1039
1040 @Override
1041 public FieldODEState<T> resetState(final FieldODEEventDetector<T> d,
1042 final FieldODEStateAndDerivative<T> s) {
1043
1044 final FieldSpacecraftState<T> oldState = convert(s);
1045 final FieldSpacecraftState<T> newState = handler.resetState(detector, oldState);
1046 stateChanged(newState);
1047
1048
1049 final T[] primary = MathArrays.buildArray(getField(), s.getPrimaryStateDimension());
1050 stateMapper.mapStateToArray(newState, primary, null);
1051
1052
1053 final T[][] secondary = MathArrays.buildArray(getField(), 1, additionalDerivativesProviders.size());
1054 for (final FieldAdditionalDerivativesProvider<T> provider : additionalDerivativesProviders) {
1055 final String name = provider.getName();
1056 final int offset = secondaryOffsets.get(name);
1057 final int dimension = provider.getDimension();
1058 System.arraycopy(newState.getAdditionalData(name), 0, secondary[0], offset, dimension);
1059 }
1060
1061 return new FieldODEState<>(newState.getDate().durationFrom(getStartDate()),
1062 primary, secondary);
1063 }
1064 };
1065
1066 }
1067
1068 }
1069
1070
1071
1072
1073
1074 private class FieldAdaptedStepHandler implements FieldODEStepHandler<T> {
1075
1076
1077 private final FieldOrekitStepHandler<T> handler;
1078
1079
1080
1081
1082 FieldAdaptedStepHandler(final FieldOrekitStepHandler<T> handler) {
1083 this.handler = handler;
1084 }
1085
1086
1087 @Override
1088 public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
1089 handler.init(convert(s0), stateMapper.mapDoubleToDate(t));
1090 }
1091
1092
1093 public void handleStep(final FieldODEStateInterpolator<T> interpolator) {
1094 handler.handleStep(new FieldAdaptedStepInterpolator(interpolator));
1095 }
1096
1097
1098 @Override
1099 public void finish(final FieldODEStateAndDerivative<T> finalState) {
1100 handler.finish(convert(finalState));
1101 }
1102
1103 }
1104
1105
1106
1107
1108
1109 private class FieldAdaptedStepInterpolator implements FieldOrekitStepInterpolator<T> {
1110
1111
1112 private final FieldODEStateInterpolator<T> mathInterpolator;
1113
1114
1115
1116
1117 FieldAdaptedStepInterpolator(final FieldODEStateInterpolator<T> mathInterpolator) {
1118 this.mathInterpolator = mathInterpolator;
1119 }
1120
1121
1122 @Override
1123 public FieldSpacecraftState<T> getPreviousState() {
1124 return convert(mathInterpolator.getPreviousState());
1125 }
1126
1127
1128 @Override
1129 public FieldSpacecraftState<T> getCurrentState() {
1130 return convert(mathInterpolator.getCurrentState());
1131 }
1132
1133
1134 @Override
1135 public FieldSpacecraftState<T> getInterpolatedState(final FieldAbsoluteDate<T> date) {
1136 return convert(mathInterpolator.getInterpolatedState(date.durationFrom(getStartDate())));
1137 }
1138
1139
1140
1141
1142 public boolean isForward() {
1143 return mathInterpolator.isForward();
1144 }
1145
1146
1147 @Override
1148 public FieldAdaptedStepInterpolator restrictStep(final FieldSpacecraftState<T> newPreviousState,
1149 final FieldSpacecraftState<T> newCurrentState) {
1150 try {
1151 final AbstractFieldODEStateInterpolator<T> aosi = (AbstractFieldODEStateInterpolator<T>) mathInterpolator;
1152 return new FieldAdaptedStepInterpolator(aosi.restrictStep(convert(newPreviousState),
1153 convert(newCurrentState)));
1154 } catch (ClassCastException cce) {
1155
1156 throw new OrekitInternalError(cce);
1157 }
1158 }
1159
1160 }
1161
1162
1163
1164
1165 private class FieldStoringStepHandler implements FieldODEStepHandler<T>, FieldEphemerisGenerator<T> {
1166
1167
1168 private FieldDenseOutputModel<T> model;
1169
1170
1171 private FieldAbsoluteDate<T> endDate;
1172
1173
1174 private FieldBoundedPropagator<T> ephemeris;
1175
1176
1177 private FieldODEStateInterpolator<T> lastInterpolator;
1178
1179
1180
1181
1182 public void setEndDate(final FieldAbsoluteDate<T> endDate) {
1183 this.endDate = endDate;
1184 }
1185
1186
1187 @Override
1188 public void init(final FieldODEStateAndDerivative<T> s0, final T t) {
1189 this.model = new FieldDenseOutputModel<>();
1190 model.init(s0, t);
1191
1192
1193 this.ephemeris = null;
1194
1195 this.lastInterpolator = null;
1196
1197 }
1198
1199
1200 @Override
1201 public FieldBoundedPropagator<T> getGeneratedEphemeris() {
1202
1203 buildEphemeris();
1204 return ephemeris;
1205 }
1206
1207
1208 @Override
1209 public void handleStep(final FieldODEStateInterpolator<T> interpolator) {
1210 model.handleStep(interpolator);
1211 lastInterpolator = interpolator;
1212 }
1213
1214
1215 @Override
1216 public void finish(final FieldODEStateAndDerivative<T> finalState) {
1217 buildEphemeris();
1218 }
1219
1220
1221
1222
1223
1224 private void buildEphemeris() {
1225
1226
1227
1228
1229
1230 model.finish(lastInterpolator.getCurrentState());
1231
1232
1233 final T tI = model.getInitialTime();
1234 final T tF = model.getFinalTime();
1235
1236 final FieldAbsoluteDate<T> startDate =
1237 stateMapper.mapDoubleToDate(tI);
1238 final FieldAbsoluteDate<T> finalDate =
1239 stateMapper.mapDoubleToDate(tF, this.endDate);
1240 final FieldAbsoluteDate<T> minDate;
1241 final FieldAbsoluteDate<T> maxDate;
1242 if (tF.getReal() < tI.getReal()) {
1243 minDate = finalDate;
1244 maxDate = startDate;
1245 } else {
1246 minDate = startDate;
1247 maxDate = finalDate;
1248 }
1249
1250
1251 final FieldDataDictionary<T> unmanaged = new FieldDataDictionary<>(startDate.getField());
1252 for (final FieldDataDictionary<T>.Entry initial : getInitialState().getAdditionalDataValues().getData()) {
1253 if (!FieldAbstractIntegratedPropagator.this.isAdditionalDataManaged(initial.getKey())) {
1254
1255
1256 unmanaged.put(initial.getKey(), initial.getValue());
1257 }
1258 }
1259
1260
1261 final String[] names = new String[additionalDerivativesProviders.size()];
1262 final int[] dimensions = new int[additionalDerivativesProviders.size()];
1263 for (int i = 0; i < names.length; ++i) {
1264 names[i] = additionalDerivativesProviders.get(i).getName();
1265 dimensions[i] = additionalDerivativesProviders.get(i).getDimension();
1266 }
1267
1268
1269 ephemeris = new FieldIntegratedEphemeris<>(startDate, minDate, maxDate,
1270 stateMapper, getAttitudeProvider(), propagationType, model,
1271 unmanaged, getAdditionalDataProviders(),
1272 names, dimensions);
1273
1274 }
1275
1276 }
1277
1278
1279
1280
1281
1282
1283
1284
1285
1286
1287
1288
1289 private static class IntegratorResetter<T extends CalculusFieldElement<T>> implements AutoCloseable {
1290
1291
1292 private final FieldODEIntegrator<T> integrator;
1293
1294
1295 private final List<FieldODEEventDetector<T>> detectors;
1296
1297
1298 private final List<FieldODEStepHandler<T>> stepHandlers;
1299
1300
1301
1302
1303 IntegratorResetter(final FieldODEIntegrator<T> integrator) {
1304 this.integrator = integrator;
1305 this.detectors = new ArrayList<>(integrator.getEventDetectors());
1306 this.stepHandlers = new ArrayList<>(integrator.getStepHandlers());
1307 }
1308
1309
1310
1311
1312
1313
1314 @Override
1315 public void close() {
1316
1317
1318 integrator.clearEventDetectors();
1319 detectors.forEach(integrator::addEventDetector);
1320
1321
1322 integrator.clearStepHandlers();
1323 stepHandlers.forEach(integrator::addStepHandler);
1324
1325 }
1326
1327 }
1328
1329 }