1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.numerical;
18
19 import org.hipparchus.analysis.differentiation.Gradient;
20 import org.hipparchus.analysis.differentiation.GradientField;
21 import org.hipparchus.exception.LocalizedCoreFormats;
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.linear.DecompositionSolver;
24 import org.hipparchus.linear.MatrixUtils;
25 import org.hipparchus.linear.QRDecomposition;
26 import org.hipparchus.linear.RealMatrix;
27 import org.hipparchus.ode.ODEIntegrator;
28 import org.hipparchus.util.Precision;
29 import org.orekit.annotation.DefaultDataContext;
30 import org.orekit.attitudes.Attitude;
31 import org.orekit.attitudes.AttitudeProvider;
32 import org.orekit.data.DataContext;
33 import org.orekit.errors.OrekitException;
34 import org.orekit.errors.OrekitInternalError;
35 import org.orekit.errors.OrekitMessages;
36 import org.orekit.forces.ForceModel;
37 import org.orekit.forces.drag.AbstractDragForceModel;
38 import org.orekit.forces.gravity.NewtonianAttraction;
39 import org.orekit.forces.inertia.InertialForces;
40 import org.orekit.forces.maneuvers.Maneuver;
41 import org.orekit.forces.maneuvers.jacobians.Duration;
42 import org.orekit.forces.maneuvers.jacobians.MassDepletionDelay;
43 import org.orekit.forces.maneuvers.jacobians.MedianDate;
44 import org.orekit.forces.maneuvers.jacobians.TriggerDate;
45 import org.orekit.forces.maneuvers.trigger.ManeuverTriggerDetector;
46 import org.orekit.forces.maneuvers.trigger.ResettableManeuverTriggers;
47 import org.orekit.forces.radiation.RadiationForceModel;
48 import org.orekit.frames.Frame;
49 import org.orekit.orbits.Orbit;
50 import org.orekit.orbits.OrbitType;
51 import org.orekit.orbits.PositionAngleType;
52 import org.orekit.propagation.AbstractMatricesHarvester;
53 import org.orekit.propagation.AdditionalDataProvider;
54 import org.orekit.propagation.MatricesHarvester;
55 import org.orekit.propagation.PropagationType;
56 import org.orekit.propagation.Propagator;
57 import org.orekit.propagation.SpacecraftState;
58 import org.orekit.propagation.events.DetectorModifier;
59 import org.orekit.propagation.events.EventDetector;
60 import org.orekit.propagation.events.FieldEventDetector;
61 import org.orekit.propagation.events.ParameterDrivenDateIntervalDetector;
62 import org.orekit.propagation.events.handlers.EventHandler;
63 import org.orekit.propagation.integration.AbstractIntegratedPropagator;
64 import org.orekit.propagation.integration.AdditionalDerivativesProvider;
65 import org.orekit.propagation.integration.StateMapper;
66 import org.orekit.time.AbsoluteDate;
67 import org.orekit.utils.AbsolutePVCoordinates;
68 import org.orekit.utils.DoubleArrayDictionary;
69 import org.orekit.utils.ParameterDriver;
70 import org.orekit.utils.ParameterDriversList;
71 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
72 import org.orekit.utils.ParameterObserver;
73 import org.orekit.utils.TimeSpanMap;
74 import org.orekit.utils.TimeSpanMap.Span;
75 import org.orekit.utils.TimeStampedPVCoordinates;
76
77 import java.util.ArrayList;
78 import java.util.Collection;
79 import java.util.Collections;
80 import java.util.List;
81 import java.util.Optional;
82 import java.util.stream.Collectors;
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176 public class NumericalPropagator extends AbstractIntegratedPropagator {
177
178
179 public static final OrbitType DEFAULT_ORBIT_TYPE = OrbitType.EQUINOCTIAL;
180
181
182 public static final PositionAngleType DEFAULT_POSITION_ANGLE_TYPE = PositionAngleType.ECCENTRIC;
183
184
185 private static final double THRESHOLD = Precision.SAFE_MIN;
186
187
188 private final List<ForceModel> forceModels;
189
190
191 private boolean ignoreCentralAttraction;
192
193
194
195
196
197 private boolean needFullAttitudeForDerivatives = true;
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214 @DefaultDataContext
215 public NumericalPropagator(final ODEIntegrator integrator) {
216 this(integrator,
217 Propagator.getDefaultLaw(DataContext.getDefault().getFrames()));
218 }
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233 public NumericalPropagator(final ODEIntegrator integrator,
234 final AttitudeProvider attitudeProvider) {
235 super(integrator, PropagationType.OSCULATING);
236 forceModels = new ArrayList<>();
237 ignoreCentralAttraction = false;
238 initMapper();
239 setAttitudeProvider(attitudeProvider);
240 clearStepHandlers();
241 setOrbitType(DEFAULT_ORBIT_TYPE);
242 setPositionAngleType(DEFAULT_POSITION_ANGLE_TYPE);
243 }
244
245
246
247
248
249 public void setIgnoreCentralAttraction(final boolean ignoreCentralAttraction) {
250 this.ignoreCentralAttraction = ignoreCentralAttraction;
251 }
252
253
254
255
256
257
258
259
260
261
262
263 @Override
264 public void setMu(final double mu) {
265 if (ignoreCentralAttraction) {
266 superSetMu(mu);
267 } else {
268 addForceModel(new NewtonianAttraction(mu));
269 superSetMu(mu);
270 }
271 }
272
273
274
275
276 private void superSetMu(final double mu) {
277 super.setMu(mu);
278 }
279
280
281
282
283
284
285
286 private boolean hasNewtonianAttraction() {
287 final int last = forceModels.size() - 1;
288 return last >= 0 && forceModels.get(last) instanceof NewtonianAttraction;
289 }
290
291
292
293
294
295
296
297
298
299 public void addForceModel(final ForceModel model) {
300
301 if (model instanceof NewtonianAttraction) {
302
303
304 try {
305
306 model.getParametersDrivers().getFirst().addObserver(new ParameterObserver() {
307
308 @Override
309 public void valueSpanMapChanged(final TimeSpanMap<Double> previousValue, final ParameterDriver driver) {
310 superSetMu(driver.getValue());
311 }
312
313 @Override
314 public void valueChanged(final double previousValue, final ParameterDriver driver, final AbsoluteDate date) {
315 superSetMu(driver.getValue());
316 }
317 });
318 } catch (OrekitException oe) {
319
320 throw new OrekitInternalError(oe);
321 }
322
323 if (hasNewtonianAttraction()) {
324
325 forceModels.set(forceModels.size() - 1, model);
326 } else {
327
328 forceModels.add(model);
329 }
330 } else {
331
332 if (hasNewtonianAttraction()) {
333
334
335 forceModels.add(forceModels.size() - 1, model);
336 } else {
337
338 forceModels.add(model);
339 }
340 }
341
342 }
343
344
345
346
347
348
349
350 public void removeForceModels() {
351 final int last = forceModels.size() - 1;
352 if (hasNewtonianAttraction()) {
353
354 final ForceModel newton = forceModels.get(last);
355 forceModels.clear();
356 forceModels.add(newton);
357 } else {
358 forceModels.clear();
359 }
360 }
361
362
363
364
365
366
367
368 public List<ForceModel> getAllForceModels() {
369 return Collections.unmodifiableList(forceModels);
370 }
371
372
373
374
375
376 @Override
377 public void setOrbitType(final OrbitType orbitType) {
378 super.setOrbitType(orbitType);
379 }
380
381
382
383
384
385 @Override
386 public OrbitType getOrbitType() {
387 return super.getOrbitType();
388 }
389
390
391
392
393
394
395
396
397
398
399 @Override
400 public void setPositionAngleType(final PositionAngleType positionAngleType) {
401 super.setPositionAngleType(positionAngleType);
402 }
403
404
405
406
407 @Override
408 public PositionAngleType getPositionAngleType() {
409 return super.getPositionAngleType();
410 }
411
412
413
414
415 public void setInitialState(final SpacecraftState initialState) {
416 resetInitialState(initialState);
417 }
418
419
420 @Override
421 public void resetInitialState(final SpacecraftState state) {
422 super.resetInitialState(state);
423 if (!hasNewtonianAttraction()) {
424
425 setMu(state.isOrbitDefined() ? state.getOrbit().getMu() : Double.NaN);
426 }
427 setStartDate(state.getDate());
428 }
429
430
431
432
433 List<String> getJacobiansColumnsNames() {
434 final List<String> columnsNames = new ArrayList<>();
435 for (final ForceModel forceModel : getAllForceModels()) {
436 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
437 if (driver.isSelected() && !columnsNames.contains(driver.getNamesSpanMap().getFirstSpan().getData())) {
438
439
440 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
441 columnsNames.add(span.getData());
442 }
443 }
444 }
445 }
446 Collections.sort(columnsNames);
447 return columnsNames;
448 }
449
450
451
452
453
454
455
456 @Override
457 protected AbstractMatricesHarvester createHarvester(final String stmName, final RealMatrix initialStm,
458 final DoubleArrayDictionary initialJacobianColumns) {
459 return new NumericalPropagationHarvester(this, stmName, initialStm, initialJacobianColumns);
460 }
461
462
463 @Override
464 public void clearMatricesComputation() {
465 final List<AdditionalDerivativesProvider> copiedDerivativesProviders = new ArrayList<>(getAdditionalDerivativesProviders());
466 copiedDerivativesProviders.stream().filter(AbstractStateTransitionMatrixGenerator.class::isInstance)
467 .forEach(provider -> removeAdditionalDerivativesProvider(provider.getName()));
468 final List<AdditionalDataProvider<?>> copiedDataProviders = new ArrayList<>(getAdditionalDataProviders());
469 for (final AdditionalDataProvider<?> additionalDataProvider: copiedDataProviders) {
470 if (additionalDataProvider instanceof TriggerDate triggerDate) {
471 if (triggerDate.getMassDepletionDelay() != null) {
472 removeAdditionalDerivativesProvider(triggerDate.getMassDepletionDelay().getName());
473 }
474 removeAdditionalDataProvider(additionalDataProvider.getName());
475 } else if (additionalDataProvider instanceof MedianDate || additionalDataProvider instanceof Duration) {
476 removeAdditionalDataProvider(additionalDataProvider.getName());
477 }
478 }
479 super.clearMatricesComputation();
480 }
481
482
483 @Override
484 protected void setUpStmAndJacobianGenerators() {
485
486 final AbstractMatricesHarvester harvester = getHarvester();
487 if (harvester != null) {
488
489
490 final AbstractStateTransitionMatrixGenerator stmGenerator = setUpStmGenerator();
491 final List<String> triggersDates = setUpTriggerDatesJacobiansColumns(stmGenerator);
492 setUpRegularParametersJacobiansColumns(stmGenerator, triggersDates);
493
494
495
496 harvester.freezeColumnsNames();
497
498 }
499
500 }
501
502
503
504
505
506 private AbstractStateTransitionMatrixGenerator setUpStmGenerator() {
507
508 final AbstractMatricesHarvester harvester = getHarvester();
509
510
511 AbstractStateTransitionMatrixGenerator stmGenerator = null;
512 for (final AdditionalDerivativesProvider equations : getAdditionalDerivativesProviders()) {
513 if (equations instanceof AbstractStateTransitionMatrixGenerator generator &&
514 equations.getName().equals(harvester.getStmName())) {
515
516 stmGenerator = generator;
517 break;
518 }
519 }
520 if (stmGenerator == null) {
521
522 if (harvester.getStateDimension() > 6) {
523 stmGenerator = new ExtendedStateTransitionMatrixGenerator(harvester.getStmName(), getAllForceModels(),
524 getAttitudeProvider());
525 } else {
526 stmGenerator = new StateTransitionMatrixGenerator(harvester.getStmName(), getAllForceModels(),
527 getAttitudeProvider());
528 }
529 addAdditionalDerivativesProvider(stmGenerator);
530 }
531
532 if (!getInitialIntegrationState().hasAdditionalData(harvester.getStmName())) {
533
534
535 setInitialState(stmGenerator.setInitialStateTransitionMatrix(getInitialState(),
536 harvester.getInitialStateTransitionMatrix(),
537 getOrbitType(),
538 getPositionAngleType()));
539 }
540
541 return stmGenerator;
542
543 }
544
545
546
547
548
549
550 private List<String> setUpTriggerDatesJacobiansColumns(final AbstractStateTransitionMatrixGenerator stmGenerator) {
551
552 final String stmName = stmGenerator.getName();
553 final boolean isMassInStm = stmGenerator instanceof ExtendedStateTransitionMatrixGenerator;
554 final List<String> names = new ArrayList<>();
555 for (final ForceModel forceModel : getAllForceModels()) {
556 if (forceModel instanceof Maneuver maneuver && ((Maneuver) forceModel).getManeuverTriggers() instanceof ResettableManeuverTriggers) {
557 final ResettableManeuverTriggers maneuverTriggers = (ResettableManeuverTriggers) maneuver.getManeuverTriggers();
558
559 final Collection<EventDetector> selectedDetectors = maneuverTriggers.getEventDetectors().
560 filter(ManeuverTriggerDetector.class::isInstance).
561 map(triggerDetector -> ((ManeuverTriggerDetector<?>) triggerDetector).getDetector())
562 .collect(Collectors.toList());
563 for (final EventDetector detector: selectedDetectors) {
564 if (detector instanceof ParameterDrivenDateIntervalDetector d) {
565 TriggerDate start;
566 TriggerDate stop;
567
568 if (d.getStartDriver().isSelected() || d.getMedianDriver().isSelected() || d.getDurationDriver().isSelected()) {
569
570
571 for (Span<String> span = d.getStartDriver().getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
572 start = manageTriggerDate(stmName, maneuver, maneuverTriggers, span.getData(), true,
573 d.getThreshold(), isMassInStm);
574 names.add(start.getName());
575 start = null;
576 }
577 }
578 if (d.getStopDriver().isSelected() || d.getMedianDriver().isSelected() || d.getDurationDriver().isSelected()) {
579
580
581 for (Span<String> span = d.getStopDriver().getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
582 stop = manageTriggerDate(stmName, maneuver, maneuverTriggers, span.getData(), false,
583 d.getThreshold(), isMassInStm);
584 names.add(stop.getName());
585 stop = null;
586 }
587 }
588 if (d.getMedianDriver().isSelected()) {
589
590 Span<String> currentMedianNameSpan = d.getMedianDriver().getNamesSpanMap().getFirstSpan();
591 MedianDate median =
592 manageMedianDate(d.getStartDriver().getNamesSpanMap().getFirstSpan().getData(),
593 d.getStopDriver().getNamesSpanMap().getFirstSpan().getData(), currentMedianNameSpan.getData());
594 names.add(median.getName());
595
596
597
598 for (int spanNumber = 1; spanNumber < d.getMedianDriver().getNamesSpanMap().getSpansNumber(); ++spanNumber) {
599 currentMedianNameSpan = d.getMedianDriver().getNamesSpanMap().getSpan(currentMedianNameSpan.getEnd());
600 median =
601 manageMedianDate(d.getStartDriver().getNamesSpanMap().getSpan(currentMedianNameSpan.getStart()).getData(),
602 d.getStopDriver().getNamesSpanMap().getSpan(currentMedianNameSpan.getStart()).getData(),
603 currentMedianNameSpan.getData());
604 names.add(median.getName());
605
606 }
607
608 }
609 if (d.getDurationDriver().isSelected()) {
610
611 Span<String> currentDurationNameSpan = d.getDurationDriver().getNamesSpanMap().getFirstSpan();
612 Duration duration =
613 manageManeuverDuration(d.getStartDriver().getNamesSpanMap().getFirstSpan().getData(),
614 d.getStopDriver().getNamesSpanMap().getFirstSpan().getData(), currentDurationNameSpan.getData());
615 names.add(duration.getName());
616
617 for (int spanNumber = 1; spanNumber < d.getDurationDriver().getNamesSpanMap().getSpansNumber(); ++spanNumber) {
618 currentDurationNameSpan = d.getDurationDriver().getNamesSpanMap().getSpan(currentDurationNameSpan.getEnd());
619 duration =
620 manageManeuverDuration(d.getStartDriver().getNamesSpanMap().getSpan(currentDurationNameSpan.getStart()).getData(),
621 d.getStopDriver().getNamesSpanMap().getSpan(currentDurationNameSpan.getStart()).getData(),
622 currentDurationNameSpan.getData());
623 names.add(duration.getName());
624
625 }
626 }
627 }
628 }
629 }
630 }
631
632 return names;
633
634 }
635
636
637
638
639
640
641
642
643
644
645
646
647 private TriggerDate manageTriggerDate(final String stmName,
648 final Maneuver maneuver,
649 final ResettableManeuverTriggers mt,
650 final String driverName,
651 final boolean start,
652 final double threshold,
653 final boolean isMassInStm) {
654
655 TriggerDate triggerGenerator = null;
656
657
658 for (final AdditionalDataProvider<?> provider : getAdditionalDataProviders()) {
659 if (provider instanceof TriggerDate date &&
660 provider.getName().equals(driverName)) {
661
662 triggerGenerator = date;
663 break;
664 }
665 }
666
667 if (triggerGenerator == null) {
668
669 if (isMassInStm) {
670 triggerGenerator = new TriggerDate(stmName, driverName, start, maneuver, threshold, true);
671 } else {
672 final Optional<ForceModel> dragForce = getAllForceModels().stream().filter(AbstractDragForceModel.class::isInstance).findFirst();
673 final Optional<ForceModel> srpForce = getAllForceModels().stream().filter(RadiationForceModel.class::isInstance).findFirst();
674 final List<ForceModel> nonGravitationalForces = new ArrayList<>();
675 dragForce.ifPresent(nonGravitationalForces::add);
676 srpForce.ifPresent(nonGravitationalForces::add);
677 triggerGenerator = new TriggerDate(stmName, driverName, start, maneuver, threshold, false,
678 nonGravitationalForces.toArray(new ForceModel[0]));
679 }
680 mt.addResetter(triggerGenerator);
681 final MassDepletionDelay depletionDelay = triggerGenerator.getMassDepletionDelay();
682 if (depletionDelay != null) {
683 addAdditionalDerivativesProvider(depletionDelay);
684 }
685 addAdditionalDataProvider(triggerGenerator);
686 }
687
688 if (!getInitialIntegrationState().hasAdditionalData(driverName)) {
689
690
691 final MassDepletionDelay depletionDelay = triggerGenerator.getMassDepletionDelay();
692 final double[] zeroes = new double[depletionDelay == null ? 7 : 6];
693 if (depletionDelay != null) {
694 setInitialColumn(depletionDelay.getName(), zeroes);
695 }
696 setInitialColumn(driverName, getHarvester().getInitialJacobianColumn(driverName));
697 }
698
699 return triggerGenerator;
700
701 }
702
703
704
705
706
707
708
709
710 private MedianDate manageMedianDate(final String startName, final String stopName, final String medianName) {
711
712 MedianDate medianGenerator = null;
713
714
715 for (final AdditionalDataProvider<?> provider : getAdditionalDataProviders()) {
716 if (provider instanceof MedianDate date &&
717 provider.getName().equals(medianName)) {
718
719 medianGenerator = date;
720 break;
721 }
722 }
723
724 if (medianGenerator == null) {
725
726 medianGenerator = new MedianDate(startName, stopName, medianName);
727 addAdditionalDataProvider(medianGenerator);
728 }
729
730 if (!getInitialIntegrationState().hasAdditionalData(medianName)) {
731
732
733 setInitialColumn(medianName, getHarvester().getInitialJacobianColumn(medianName));
734 }
735
736 return medianGenerator;
737
738 }
739
740
741
742
743
744
745
746
747 private Duration manageManeuverDuration(final String startName, final String stopName, final String durationName) {
748
749 Duration durationGenerator = null;
750
751
752 for (final AdditionalDataProvider<?> provider : getAdditionalDataProviders()) {
753 if (provider instanceof Duration duration &&
754 provider.getName().equals(durationName)) {
755
756 durationGenerator = duration;
757 break;
758 }
759 }
760
761 if (durationGenerator == null) {
762
763 durationGenerator = new Duration(startName, stopName, durationName);
764 addAdditionalDataProvider(durationGenerator);
765 }
766
767 if (!getInitialIntegrationState().hasAdditionalData(durationName)) {
768
769
770 setInitialColumn(durationName, getHarvester().getInitialJacobianColumn(durationName));
771 }
772
773 return durationGenerator;
774
775 }
776
777
778
779
780
781
782 private void setUpRegularParametersJacobiansColumns(final AbstractStateTransitionMatrixGenerator stmGenerator,
783 final List<String> triggerDates) {
784
785
786 final ParameterDriversList selected = new ParameterDriversList();
787 for (final ForceModel forceModel : getAllForceModels()) {
788 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
789 if (!triggerDates.contains(driver.getNamesSpanMap().getFirstSpan().getData())) {
790
791
792 selected.add(driver);
793 }
794 }
795 }
796
797
798
799 selected.filter(true);
800
801
802 selected.sort();
803
804
805
806 for (final DelegatingDriver driver : selected.getDrivers()) {
807
808 for (Span<String> currentNameSpan = driver.getNamesSpanMap().getFirstSpan(); currentNameSpan != null; currentNameSpan = currentNameSpan.next()) {
809
810 IntegrableJacobianColumnGenerator generator = null;
811
812 for (final AdditionalDerivativesProvider provider : getAdditionalDerivativesProviders()) {
813 if (provider instanceof IntegrableJacobianColumnGenerator columnGenerator &&
814 provider.getName().equals(currentNameSpan.getData())) {
815
816 generator = columnGenerator;
817 break;
818 }
819
820 }
821
822 if (generator == null) {
823
824 final boolean isMassIncluded = stmGenerator.getStateDimension() == 7;
825 generator = new IntegrableJacobianColumnGenerator(stmGenerator, currentNameSpan.getData(), isMassIncluded);
826 addAdditionalDerivativesProvider(generator);
827 }
828
829 if (!getInitialIntegrationState().hasAdditionalData(currentNameSpan.getData())) {
830
831
832 setInitialColumn(currentNameSpan.getData(), getHarvester().getInitialJacobianColumn(currentNameSpan.getData()));
833 }
834
835 }
836
837 }
838
839 }
840
841
842
843
844
845
846
847
848
849
850 private void setInitialColumn(final String columnName, final double[] dYdQ) {
851
852 final SpacecraftState state = getInitialState();
853
854 final AbstractStateTransitionMatrixGenerator generator = (AbstractStateTransitionMatrixGenerator)
855 getAdditionalDerivativesProviders().stream()
856 .filter(AbstractStateTransitionMatrixGenerator.class::isInstance)
857 .collect(Collectors.toList()).getFirst();
858 final int expectedSize = generator.getStateDimension();
859 if (dYdQ.length != expectedSize) {
860 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, dYdQ.length, expectedSize);
861 }
862
863
864 final RealMatrix dYdC = MatrixUtils.createRealIdentityMatrix(expectedSize);
865 final double[][] jacobian = new double[6][6];
866 getOrbitType().convertType(state.getOrbit()).getJacobianWrtCartesian(getPositionAngleType(), jacobian);
867 dYdC.setSubMatrix(jacobian, 0, 0);
868 final DecompositionSolver solver = getSolver(dYdC);
869 final double[] column = solver.solve(MatrixUtils.createRealVector(dYdQ)).toArray();
870
871
872 setInitialState(state.addAdditionalData(columnName, column));
873
874 }
875
876
877
878
879
880
881
882 private DecompositionSolver getSolver(final RealMatrix matrix) {
883 return new QRDecomposition(matrix, THRESHOLD).getSolver();
884 }
885
886
887 @Override
888 protected AttitudeProvider initializeAttitudeProviderForDerivatives() {
889 return needFullAttitudeForDerivatives ? getAttitudeProvider() : getFrozenAttitudeProvider();
890 }
891
892
893 @Override
894 protected StateMapper createMapper(final AbsoluteDate referenceDate, final double mu,
895 final OrbitType orbitType, final PositionAngleType positionAngleType,
896 final AttitudeProvider attitudeProvider, final Frame frame) {
897 return new OsculatingMapper(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
898 }
899
900
901 private static class OsculatingMapper extends StateMapper {
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917 OsculatingMapper(final AbsoluteDate referenceDate, final double mu,
918 final OrbitType orbitType, final PositionAngleType positionAngleType,
919 final AttitudeProvider attitudeProvider, final Frame frame) {
920 super(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
921 }
922
923
924 @Override
925 public SpacecraftState mapArrayToState(final AbsoluteDate date, final double[] y, final double[] yDot,
926 final PropagationType type) {
927
928
929 final double mass = y[6];
930 final double massRate = yDot == null ? 0. : yDot[6];
931 if (mass <= 0.0) {
932 throw new OrekitException(OrekitMessages.NOT_POSITIVE_SPACECRAFT_MASS, mass);
933 }
934
935 if (super.getOrbitType() == null) {
936
937 final Vector3D p = new Vector3D(y[0], y[1], y[2]);
938 final Vector3D v = new Vector3D(y[3], y[4], y[5]);
939 final Vector3D a;
940 final AbsolutePVCoordinates absPva;
941 if (yDot == null) {
942 absPva = new AbsolutePVCoordinates(getFrame(), new TimeStampedPVCoordinates(date, p, v));
943 } else {
944 a = new Vector3D(yDot[3], yDot[4], yDot[5]);
945 absPva = new AbsolutePVCoordinates(getFrame(), new TimeStampedPVCoordinates(date, p, v, a));
946 }
947
948 final Attitude attitude = getAttitudeProvider().getAttitude(absPva, date, getFrame());
949 return new SpacecraftState(absPva, attitude).withMassRate(massRate).withMass(mass);
950 } else {
951
952 final Orbit orbit = super.getOrbitType().mapArrayToOrbit(y, yDot, super.getPositionAngleType(), date, getMu(), getFrame());
953 final Attitude attitude = getAttitudeProvider().getAttitude(orbit, date, getFrame());
954
955 return new SpacecraftState(orbit, attitude).withMassRate(massRate).withMass(mass);
956 }
957
958 }
959
960
961 public void mapStateToArray(final SpacecraftState state, final double[] y, final double[] yDot) {
962 if (super.getOrbitType() == null) {
963
964 final Vector3D p = state.getAbsPVA().getPosition();
965 final Vector3D v = state.getAbsPVA().getVelocity();
966 y[0] = p.getX();
967 y[1] = p.getY();
968 y[2] = p.getZ();
969 y[3] = v.getX();
970 y[4] = v.getY();
971 y[5] = v.getZ();
972 y[6] = state.getMass();
973 }
974 else {
975 super.getOrbitType().mapOrbitToArray(state.getOrbit(), super.getPositionAngleType(), y, yDot);
976 y[6] = state.getMass();
977 }
978 }
979
980 }
981
982
983 protected MainStateEquations getMainStateEquations(final ODEIntegrator integrator) {
984 return new Main(integrator, getOrbitType(), getPositionAngleType(), getAllForceModels());
985 }
986
987
988 private class Main extends NumericalTimeDerivativesEquations implements MainStateEquations {
989
990
991 private final boolean recomputingJacobian;
992
993
994
995
996
997
998
999 Main(final ODEIntegrator integrator, final OrbitType orbitType, final PositionAngleType positionAngleType,
1000 final List<ForceModel> forceModelList) {
1001
1002 super(orbitType, positionAngleType, forceModelList);
1003 final int numberOfForces = forceModelList.size();
1004 if (orbitType != null && orbitType != OrbitType.CARTESIAN && numberOfForces > 0) {
1005 if (numberOfForces > 1) {
1006 recomputingJacobian = true;
1007 } else {
1008 recomputingJacobian = !(forceModelList.getFirst() instanceof NewtonianAttraction);
1009 }
1010 } else {
1011 recomputingJacobian = false;
1012 }
1013
1014
1015 setUpInternalDetectors(integrator);
1016
1017 }
1018
1019
1020
1021
1022 private void setUpInternalDetectors(final ODEIntegrator integrator) {
1023 final NumericalTimeDerivativesEquations cartesianEquations = new NumericalTimeDerivativesEquations(OrbitType.CARTESIAN,
1024 null, forceModels);
1025 final List<FieldEventDetector<Gradient>> fieldDetectors = new ArrayList<>();
1026 if (getHarvester() != null) {
1027 final GradientField field = GradientField.getField(getHarvester().getStateDimension() + 1);
1028 getForceModels().stream().flatMap(forceModel -> forceModel.getFieldEventDetectors(field))
1029 .filter(fieldEventDetector -> !fieldEventDetector.getEventFunction().dependsOnTimeOnly())
1030 .forEach(fieldDetectors::add);
1031 getAttitudeProvider().getFieldEventDetectors(field)
1032 .filter(fieldEventDetector -> !fieldEventDetector.getEventFunction().dependsOnTimeOnly())
1033 .forEach(fieldDetectors::add);
1034 }
1035 for (final ForceModel forceModel : getForceModels()) {
1036 forceModel.getEventDetectors().forEach(detector -> setUpInternalEventDetector(integrator, detector,
1037 cartesianEquations, fieldDetectors));
1038 }
1039 getAttitudeProvider().getEventDetectors().forEach(detector -> setUpInternalEventDetector(integrator,
1040 detector, cartesianEquations, fieldDetectors));
1041 }
1042
1043
1044
1045
1046
1047
1048
1049 private void setUpInternalEventDetector(final ODEIntegrator integrator,
1050 final EventDetector eventDetector,
1051 final NumericalTimeDerivativesEquations cartesianEquations,
1052 final List<FieldEventDetector<Gradient>> fieldDetectors) {
1053 final EventDetector internalDetector;
1054 if (!fieldDetectors.isEmpty() && !eventDetector.getEventFunction().dependsOnTimeOnly()) {
1055
1056 final NumericalPropagationHarvester harvester = (NumericalPropagationHarvester) getHarvester();
1057 final SwitchEventHandler handler = new SwitchEventHandler(eventDetector.getHandler(), harvester,
1058 cartesianEquations, getAttitudeProvider(), fieldDetectors);
1059 internalDetector = getLocalDetector(eventDetector, handler);
1060 } else {
1061 internalDetector = eventDetector;
1062 }
1063 setUpEventDetector(integrator, internalDetector);
1064 }
1065
1066
1067 @Override
1068 public void init(final SpacecraftState initialState, final AbsoluteDate target) {
1069 final List<ForceModel> forceModelList = getForceModels();
1070 needFullAttitudeForDerivatives = forceModelList.stream().anyMatch(ForceModel::dependsOnAttitudeRate);
1071
1072 forceModelList.forEach(fm -> fm.init(initialState, target));
1073
1074 }
1075
1076
1077 @Override
1078 public double[] computeDerivatives(final SpacecraftState state) {
1079 setCurrentState(state);
1080 if (recomputingJacobian) {
1081
1082 final double[][] jacobian = new double[6][6];
1083 state.getOrbit().getJacobianWrtCartesian(getPositionAngleType(), jacobian);
1084 setCoordinatesJacobian(jacobian);
1085 }
1086 return computeTimeDerivatives(state);
1087
1088 }
1089
1090 }
1091
1092
1093 @Override
1094 protected void beforeIntegration(final SpacecraftState initialState, final AbsoluteDate tEnd) {
1095
1096 if (!getFrame().isPseudoInertial()) {
1097
1098
1099 for (ForceModel force : forceModels) {
1100 if (force instanceof InertialForces) {
1101 return;
1102 }
1103 }
1104
1105
1106 throw new OrekitException(OrekitMessages.INERTIAL_FORCE_MODEL_MISSING, getFrame().getName());
1107
1108 }
1109
1110 }
1111
1112
1113
1114
1115
1116
1117
1118 private static EventDetector getLocalDetector(final EventDetector eventDetector,
1119 final SwitchEventHandler switchEventHandler) {
1120 return new DetectorModifier() {
1121 @Override
1122 public EventDetector getDetector() {
1123 return eventDetector;
1124 }
1125
1126 @Override
1127 public EventHandler getHandler() {
1128 return switchEventHandler;
1129 }
1130 };
1131 }
1132 }