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().get(0).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) {
471 final TriggerDate triggerDate = (TriggerDate) additionalDataProvider;
472 if (triggerDate.getMassDepletionDelay() != null) {
473 removeAdditionalDerivativesProvider(triggerDate.getMassDepletionDelay().getName());
474 }
475 removeAdditionalDataProvider(additionalDataProvider.getName());
476 } else if (additionalDataProvider instanceof MedianDate || additionalDataProvider instanceof Duration) {
477 removeAdditionalDataProvider(additionalDataProvider.getName());
478 }
479 }
480 super.clearMatricesComputation();
481 }
482
483
484 @Override
485 protected void setUpStmAndJacobianGenerators() {
486
487 final AbstractMatricesHarvester harvester = getHarvester();
488 if (harvester != null) {
489
490
491 final AbstractStateTransitionMatrixGenerator stmGenerator = setUpStmGenerator();
492 final List<String> triggersDates = setUpTriggerDatesJacobiansColumns(stmGenerator);
493 setUpRegularParametersJacobiansColumns(stmGenerator, triggersDates);
494
495
496
497 harvester.freezeColumnsNames();
498
499 }
500
501 }
502
503
504
505
506
507 private AbstractStateTransitionMatrixGenerator setUpStmGenerator() {
508
509 final AbstractMatricesHarvester harvester = getHarvester();
510
511
512 AbstractStateTransitionMatrixGenerator stmGenerator = null;
513 for (final AdditionalDerivativesProvider equations : getAdditionalDerivativesProviders()) {
514 if (equations instanceof AbstractStateTransitionMatrixGenerator &&
515 equations.getName().equals(harvester.getStmName())) {
516
517 stmGenerator = (AbstractStateTransitionMatrixGenerator) equations;
518 break;
519 }
520 }
521 if (stmGenerator == null) {
522
523 if (harvester.getStateDimension() > 6) {
524 stmGenerator = new ExtendedStateTransitionMatrixGenerator(harvester.getStmName(), getAllForceModels(),
525 getAttitudeProvider());
526 } else {
527 stmGenerator = new StateTransitionMatrixGenerator(harvester.getStmName(), getAllForceModels(),
528 getAttitudeProvider());
529 }
530 addAdditionalDerivativesProvider(stmGenerator);
531 }
532
533 if (!getInitialIntegrationState().hasAdditionalData(harvester.getStmName())) {
534
535
536 setInitialState(stmGenerator.setInitialStateTransitionMatrix(getInitialState(),
537 harvester.getInitialStateTransitionMatrix(),
538 getOrbitType(),
539 getPositionAngleType()));
540 }
541
542 return stmGenerator;
543
544 }
545
546
547
548
549
550
551 private List<String> setUpTriggerDatesJacobiansColumns(final AbstractStateTransitionMatrixGenerator stmGenerator) {
552
553 final String stmName = stmGenerator.getName();
554 final boolean isMassInStm = stmGenerator instanceof ExtendedStateTransitionMatrixGenerator;
555 final List<String> names = new ArrayList<>();
556 for (final ForceModel forceModel : getAllForceModels()) {
557 if (forceModel instanceof Maneuver && ((Maneuver) forceModel).getManeuverTriggers() instanceof ResettableManeuverTriggers) {
558 final Maneuver maneuver = (Maneuver) forceModel;
559 final ResettableManeuverTriggers maneuverTriggers = (ResettableManeuverTriggers) maneuver.getManeuverTriggers();
560
561 final Collection<EventDetector> selectedDetectors = maneuverTriggers.getEventDetectors().
562 filter(ManeuverTriggerDetector.class::isInstance).
563 map(triggerDetector -> ((ManeuverTriggerDetector<?>) triggerDetector).getDetector())
564 .collect(Collectors.toList());
565 for (final EventDetector detector: selectedDetectors) {
566 if (detector instanceof ParameterDrivenDateIntervalDetector) {
567 final ParameterDrivenDateIntervalDetector d = (ParameterDrivenDateIntervalDetector) detector;
568 TriggerDate start;
569 TriggerDate stop;
570
571 if (d.getStartDriver().isSelected() || d.getMedianDriver().isSelected() || d.getDurationDriver().isSelected()) {
572
573
574 for (Span<String> span = d.getStartDriver().getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
575 start = manageTriggerDate(stmName, maneuver, maneuverTriggers, span.getData(), true,
576 d.getThreshold(), isMassInStm);
577 names.add(start.getName());
578 start = null;
579 }
580 }
581 if (d.getStopDriver().isSelected() || d.getMedianDriver().isSelected() || d.getDurationDriver().isSelected()) {
582
583
584 for (Span<String> span = d.getStopDriver().getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
585 stop = manageTriggerDate(stmName, maneuver, maneuverTriggers, span.getData(), false,
586 d.getThreshold(), isMassInStm);
587 names.add(stop.getName());
588 stop = null;
589 }
590 }
591 if (d.getMedianDriver().isSelected()) {
592
593 Span<String> currentMedianNameSpan = d.getMedianDriver().getNamesSpanMap().getFirstSpan();
594 MedianDate median =
595 manageMedianDate(d.getStartDriver().getNamesSpanMap().getFirstSpan().getData(),
596 d.getStopDriver().getNamesSpanMap().getFirstSpan().getData(), currentMedianNameSpan.getData());
597 names.add(median.getName());
598
599
600
601 for (int spanNumber = 1; spanNumber < d.getMedianDriver().getNamesSpanMap().getSpansNumber(); ++spanNumber) {
602 currentMedianNameSpan = d.getMedianDriver().getNamesSpanMap().getSpan(currentMedianNameSpan.getEnd());
603 median =
604 manageMedianDate(d.getStartDriver().getNamesSpanMap().getSpan(currentMedianNameSpan.getStart()).getData(),
605 d.getStopDriver().getNamesSpanMap().getSpan(currentMedianNameSpan.getStart()).getData(),
606 currentMedianNameSpan.getData());
607 names.add(median.getName());
608
609 }
610
611 }
612 if (d.getDurationDriver().isSelected()) {
613
614 Span<String> currentDurationNameSpan = d.getDurationDriver().getNamesSpanMap().getFirstSpan();
615 Duration duration =
616 manageManeuverDuration(d.getStartDriver().getNamesSpanMap().getFirstSpan().getData(),
617 d.getStopDriver().getNamesSpanMap().getFirstSpan().getData(), currentDurationNameSpan.getData());
618 names.add(duration.getName());
619
620 for (int spanNumber = 1; spanNumber < d.getDurationDriver().getNamesSpanMap().getSpansNumber(); ++spanNumber) {
621 currentDurationNameSpan = d.getDurationDriver().getNamesSpanMap().getSpan(currentDurationNameSpan.getEnd());
622 duration =
623 manageManeuverDuration(d.getStartDriver().getNamesSpanMap().getSpan(currentDurationNameSpan.getStart()).getData(),
624 d.getStopDriver().getNamesSpanMap().getSpan(currentDurationNameSpan.getStart()).getData(),
625 currentDurationNameSpan.getData());
626 names.add(duration.getName());
627
628 }
629 }
630 }
631 }
632 }
633 }
634
635 return names;
636
637 }
638
639
640
641
642
643
644
645
646
647
648
649
650 private TriggerDate manageTriggerDate(final String stmName,
651 final Maneuver maneuver,
652 final ResettableManeuverTriggers mt,
653 final String driverName,
654 final boolean start,
655 final double threshold,
656 final boolean isMassInStm) {
657
658 TriggerDate triggerGenerator = null;
659
660
661 for (final AdditionalDataProvider<?> provider : getAdditionalDataProviders()) {
662 if (provider instanceof TriggerDate &&
663 provider.getName().equals(driverName)) {
664
665 triggerGenerator = (TriggerDate) provider;
666 break;
667 }
668 }
669
670 if (triggerGenerator == null) {
671
672 if (isMassInStm) {
673 triggerGenerator = new TriggerDate(stmName, driverName, start, maneuver, threshold, true);
674 } else {
675 final Optional<ForceModel> dragForce = getAllForceModels().stream().filter(AbstractDragForceModel.class::isInstance).findFirst();
676 final Optional<ForceModel> srpForce = getAllForceModels().stream().filter(RadiationForceModel.class::isInstance).findFirst();
677 final List<ForceModel> nonGravitationalForces = new ArrayList<>();
678 dragForce.ifPresent(nonGravitationalForces::add);
679 srpForce.ifPresent(nonGravitationalForces::add);
680 triggerGenerator = new TriggerDate(stmName, driverName, start, maneuver, threshold, false,
681 nonGravitationalForces.toArray(new ForceModel[0]));
682 }
683 mt.addResetter(triggerGenerator);
684 final MassDepletionDelay depletionDelay = triggerGenerator.getMassDepletionDelay();
685 if (depletionDelay != null) {
686 addAdditionalDerivativesProvider(depletionDelay);
687 }
688 addAdditionalDataProvider(triggerGenerator);
689 }
690
691 if (!getInitialIntegrationState().hasAdditionalData(driverName)) {
692
693
694 final MassDepletionDelay depletionDelay = triggerGenerator.getMassDepletionDelay();
695 final double[] zeroes = new double[depletionDelay == null ? 7 : 6];
696 if (depletionDelay != null) {
697 setInitialColumn(depletionDelay.getName(), zeroes);
698 }
699 setInitialColumn(driverName, getHarvester().getInitialJacobianColumn(driverName));
700 }
701
702 return triggerGenerator;
703
704 }
705
706
707
708
709
710
711
712
713 private MedianDate manageMedianDate(final String startName, final String stopName, final String medianName) {
714
715 MedianDate medianGenerator = null;
716
717
718 for (final AdditionalDataProvider<?> provider : getAdditionalDataProviders()) {
719 if (provider instanceof MedianDate &&
720 provider.getName().equals(medianName)) {
721
722 medianGenerator = (MedianDate) provider;
723 break;
724 }
725 }
726
727 if (medianGenerator == null) {
728
729 medianGenerator = new MedianDate(startName, stopName, medianName);
730 addAdditionalDataProvider(medianGenerator);
731 }
732
733 if (!getInitialIntegrationState().hasAdditionalData(medianName)) {
734
735
736 setInitialColumn(medianName, getHarvester().getInitialJacobianColumn(medianName));
737 }
738
739 return medianGenerator;
740
741 }
742
743
744
745
746
747
748
749
750 private Duration manageManeuverDuration(final String startName, final String stopName, final String durationName) {
751
752 Duration durationGenerator = null;
753
754
755 for (final AdditionalDataProvider<?> provider : getAdditionalDataProviders()) {
756 if (provider instanceof Duration &&
757 provider.getName().equals(durationName)) {
758
759 durationGenerator = (Duration) provider;
760 break;
761 }
762 }
763
764 if (durationGenerator == null) {
765
766 durationGenerator = new Duration(startName, stopName, durationName);
767 addAdditionalDataProvider(durationGenerator);
768 }
769
770 if (!getInitialIntegrationState().hasAdditionalData(durationName)) {
771
772
773 setInitialColumn(durationName, getHarvester().getInitialJacobianColumn(durationName));
774 }
775
776 return durationGenerator;
777
778 }
779
780
781
782
783
784
785 private void setUpRegularParametersJacobiansColumns(final AbstractStateTransitionMatrixGenerator stmGenerator,
786 final List<String> triggerDates) {
787
788
789 final ParameterDriversList selected = new ParameterDriversList();
790 for (final ForceModel forceModel : getAllForceModels()) {
791 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
792 if (!triggerDates.contains(driver.getNamesSpanMap().getFirstSpan().getData())) {
793
794
795 selected.add(driver);
796 }
797 }
798 }
799
800
801
802 selected.filter(true);
803
804
805 selected.sort();
806
807
808
809 for (final DelegatingDriver driver : selected.getDrivers()) {
810
811 for (Span<String> currentNameSpan = driver.getNamesSpanMap().getFirstSpan(); currentNameSpan != null; currentNameSpan = currentNameSpan.next()) {
812
813 IntegrableJacobianColumnGenerator generator = null;
814
815 for (final AdditionalDerivativesProvider provider : getAdditionalDerivativesProviders()) {
816 if (provider instanceof IntegrableJacobianColumnGenerator &&
817 provider.getName().equals(currentNameSpan.getData())) {
818
819 generator = (IntegrableJacobianColumnGenerator) provider;
820 break;
821 }
822
823 }
824
825 if (generator == null) {
826
827 final boolean isMassIncluded = stmGenerator.getStateDimension() == 7;
828 generator = new IntegrableJacobianColumnGenerator(stmGenerator, currentNameSpan.getData(), isMassIncluded);
829 addAdditionalDerivativesProvider(generator);
830 }
831
832 if (!getInitialIntegrationState().hasAdditionalData(currentNameSpan.getData())) {
833
834
835 setInitialColumn(currentNameSpan.getData(), getHarvester().getInitialJacobianColumn(currentNameSpan.getData()));
836 }
837
838 }
839
840 }
841
842 }
843
844
845
846
847
848
849
850
851
852
853 private void setInitialColumn(final String columnName, final double[] dYdQ) {
854
855 final SpacecraftState state = getInitialState();
856
857 final AbstractStateTransitionMatrixGenerator generator = (AbstractStateTransitionMatrixGenerator)
858 getAdditionalDerivativesProviders().stream()
859 .filter(AbstractStateTransitionMatrixGenerator.class::isInstance)
860 .collect(Collectors.toList()).get(0);
861 final int expectedSize = generator.getStateDimension();
862 if (dYdQ.length != expectedSize) {
863 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH, dYdQ.length, expectedSize);
864 }
865
866
867 final RealMatrix dYdC = MatrixUtils.createRealIdentityMatrix(expectedSize);
868 final double[][] jacobian = new double[6][6];
869 getOrbitType().convertType(state.getOrbit()).getJacobianWrtCartesian(getPositionAngleType(), jacobian);
870 dYdC.setSubMatrix(jacobian, 0, 0);
871 final DecompositionSolver solver = getSolver(dYdC);
872 final double[] column = solver.solve(MatrixUtils.createRealVector(dYdQ)).toArray();
873
874
875 setInitialState(state.addAdditionalData(columnName, column));
876
877 }
878
879
880
881
882
883
884
885 private DecompositionSolver getSolver(final RealMatrix matrix) {
886 return new QRDecomposition(matrix, THRESHOLD).getSolver();
887 }
888
889
890 @Override
891 protected AttitudeProvider initializeAttitudeProviderForDerivatives() {
892 return needFullAttitudeForDerivatives ? getAttitudeProvider() : getFrozenAttitudeProvider();
893 }
894
895
896 @Override
897 protected StateMapper createMapper(final AbsoluteDate referenceDate, final double mu,
898 final OrbitType orbitType, final PositionAngleType positionAngleType,
899 final AttitudeProvider attitudeProvider, final Frame frame) {
900 return new OsculatingMapper(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
901 }
902
903
904 private static class OsculatingMapper extends StateMapper {
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920 OsculatingMapper(final AbsoluteDate referenceDate, final double mu,
921 final OrbitType orbitType, final PositionAngleType positionAngleType,
922 final AttitudeProvider attitudeProvider, final Frame frame) {
923 super(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
924 }
925
926
927 @Override
928 public SpacecraftState mapArrayToState(final AbsoluteDate date, final double[] y, final double[] yDot,
929 final PropagationType type) {
930
931
932 final double mass = y[6];
933 final double massRate = yDot == null ? 0. : yDot[6];
934 if (mass <= 0.0) {
935 throw new OrekitException(OrekitMessages.NOT_POSITIVE_SPACECRAFT_MASS, mass);
936 }
937
938 if (super.getOrbitType() == null) {
939
940 final Vector3D p = new Vector3D(y[0], y[1], y[2]);
941 final Vector3D v = new Vector3D(y[3], y[4], y[5]);
942 final Vector3D a;
943 final AbsolutePVCoordinates absPva;
944 if (yDot == null) {
945 absPva = new AbsolutePVCoordinates(getFrame(), new TimeStampedPVCoordinates(date, p, v));
946 } else {
947 a = new Vector3D(yDot[3], yDot[4], yDot[5]);
948 absPva = new AbsolutePVCoordinates(getFrame(), new TimeStampedPVCoordinates(date, p, v, a));
949 }
950
951 final Attitude attitude = getAttitudeProvider().getAttitude(absPva, date, getFrame());
952 return new SpacecraftState(absPva, attitude).withMassRate(massRate).withMass(mass);
953 } else {
954
955 final Orbit orbit = super.getOrbitType().mapArrayToOrbit(y, yDot, super.getPositionAngleType(), date, getMu(), getFrame());
956 final Attitude attitude = getAttitudeProvider().getAttitude(orbit, date, getFrame());
957
958 return new SpacecraftState(orbit, attitude).withMassRate(massRate).withMass(mass);
959 }
960
961 }
962
963
964 public void mapStateToArray(final SpacecraftState state, final double[] y, final double[] yDot) {
965 if (super.getOrbitType() == null) {
966
967 final Vector3D p = state.getAbsPVA().getPosition();
968 final Vector3D v = state.getAbsPVA().getVelocity();
969 y[0] = p.getX();
970 y[1] = p.getY();
971 y[2] = p.getZ();
972 y[3] = v.getX();
973 y[4] = v.getY();
974 y[5] = v.getZ();
975 y[6] = state.getMass();
976 }
977 else {
978 super.getOrbitType().mapOrbitToArray(state.getOrbit(), super.getPositionAngleType(), y, yDot);
979 y[6] = state.getMass();
980 }
981 }
982
983 }
984
985
986 protected MainStateEquations getMainStateEquations(final ODEIntegrator integrator) {
987 return new Main(integrator, getOrbitType(), getPositionAngleType(), getAllForceModels());
988 }
989
990
991 private class Main extends NumericalTimeDerivativesEquations implements MainStateEquations {
992
993
994 private final boolean recomputingJacobian;
995
996
997
998
999
1000
1001
1002 Main(final ODEIntegrator integrator, final OrbitType orbitType, final PositionAngleType positionAngleType,
1003 final List<ForceModel> forceModelList) {
1004
1005 super(orbitType, positionAngleType, forceModelList);
1006 final int numberOfForces = forceModelList.size();
1007 if (orbitType != null && orbitType != OrbitType.CARTESIAN && numberOfForces > 0) {
1008 if (numberOfForces > 1) {
1009 recomputingJacobian = true;
1010 } else {
1011 recomputingJacobian = !(forceModelList.get(0) instanceof NewtonianAttraction);
1012 }
1013 } else {
1014 recomputingJacobian = false;
1015 }
1016
1017
1018 setUpInternalDetectors(integrator);
1019
1020 }
1021
1022
1023
1024
1025 private void setUpInternalDetectors(final ODEIntegrator integrator) {
1026 final NumericalTimeDerivativesEquations cartesianEquations = new NumericalTimeDerivativesEquations(OrbitType.CARTESIAN,
1027 null, forceModels);
1028 final List<FieldEventDetector<Gradient>> fieldDetectors = new ArrayList<>();
1029 if (getHarvester() != null) {
1030 final GradientField field = GradientField.getField(getHarvester().getStateDimension() + 1);
1031 getForceModels().stream().flatMap(forceModel -> forceModel.getFieldEventDetectors(field))
1032 .filter(fieldEventDetector -> !fieldEventDetector.getEventFunction().dependsOnTimeOnly())
1033 .forEach(fieldDetectors::add);
1034 getAttitudeProvider().getFieldEventDetectors(field)
1035 .filter(fieldEventDetector -> !fieldEventDetector.getEventFunction().dependsOnTimeOnly())
1036 .forEach(fieldDetectors::add);
1037 }
1038 for (final ForceModel forceModel : getForceModels()) {
1039 forceModel.getEventDetectors().forEach(detector -> setUpInternalEventDetector(integrator, detector,
1040 cartesianEquations, fieldDetectors));
1041 }
1042 getAttitudeProvider().getEventDetectors().forEach(detector -> setUpInternalEventDetector(integrator,
1043 detector, cartesianEquations, fieldDetectors));
1044 }
1045
1046
1047
1048
1049
1050
1051
1052 private void setUpInternalEventDetector(final ODEIntegrator integrator,
1053 final EventDetector eventDetector,
1054 final NumericalTimeDerivativesEquations cartesianEquations,
1055 final List<FieldEventDetector<Gradient>> fieldDetectors) {
1056 final EventDetector internalDetector;
1057 if (!fieldDetectors.isEmpty() && !eventDetector.getEventFunction().dependsOnTimeOnly()) {
1058
1059 final NumericalPropagationHarvester harvester = (NumericalPropagationHarvester) getHarvester();
1060 final SwitchEventHandler handler = new SwitchEventHandler(eventDetector.getHandler(), harvester,
1061 cartesianEquations, getAttitudeProvider(), fieldDetectors);
1062 internalDetector = getLocalDetector(eventDetector, handler);
1063 } else {
1064 internalDetector = eventDetector;
1065 }
1066 setUpEventDetector(integrator, internalDetector);
1067 }
1068
1069
1070 @Override
1071 public void init(final SpacecraftState initialState, final AbsoluteDate target) {
1072 final List<ForceModel> forceModelList = getForceModels();
1073 needFullAttitudeForDerivatives = forceModelList.stream().anyMatch(ForceModel::dependsOnAttitudeRate);
1074
1075 forceModelList.forEach(fm -> fm.init(initialState, target));
1076
1077 }
1078
1079
1080 @Override
1081 public double[] computeDerivatives(final SpacecraftState state) {
1082 setCurrentState(state);
1083 if (recomputingJacobian) {
1084
1085 final double[][] jacobian = new double[6][6];
1086 state.getOrbit().getJacobianWrtCartesian(getPositionAngleType(), jacobian);
1087 setCoordinatesJacobian(jacobian);
1088 }
1089 return computeTimeDerivatives(state);
1090
1091 }
1092
1093 }
1094
1095
1096 @Override
1097 protected void beforeIntegration(final SpacecraftState initialState, final AbsoluteDate tEnd) {
1098
1099 if (!getFrame().isPseudoInertial()) {
1100
1101
1102 for (ForceModel force : forceModels) {
1103 if (force instanceof InertialForces) {
1104 return;
1105 }
1106 }
1107
1108
1109 throw new OrekitException(OrekitMessages.INERTIAL_FORCE_MODEL_MISSING, getFrame().getName());
1110
1111 }
1112
1113 }
1114
1115
1116
1117
1118
1119
1120
1121 private static EventDetector getLocalDetector(final EventDetector eventDetector,
1122 final SwitchEventHandler switchEventHandler) {
1123 return new DetectorModifier() {
1124 @Override
1125 public EventDetector getDetector() {
1126 return eventDetector;
1127 }
1128
1129 @Override
1130 public EventHandler getHandler() {
1131 return switchEventHandler;
1132 }
1133 };
1134 }
1135 }