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