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