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