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