1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.forces.maneuvers;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.differentiation.DSFactory;
22 import org.hipparchus.analysis.differentiation.Gradient;
23 import org.hipparchus.analysis.differentiation.GradientField;
24 import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
25 import org.hipparchus.analysis.differentiation.UnivariateDerivative1Field;
26 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
27 import org.hipparchus.analysis.differentiation.UnivariateDerivative2Field;
28 import org.hipparchus.complex.Complex;
29 import org.hipparchus.complex.ComplexField;
30 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
31 import org.hipparchus.geometry.euclidean.threed.Vector3D;
32 import org.hipparchus.linear.DecompositionSolver;
33 import org.hipparchus.linear.LUDecomposition;
34 import org.hipparchus.linear.RealMatrix;
35 import org.hipparchus.ode.events.Action;
36 import org.hipparchus.ode.nonstiff.ClassicalRungeKuttaFieldIntegrator;
37 import org.hipparchus.ode.nonstiff.ClassicalRungeKuttaIntegrator;
38 import org.hipparchus.util.Binary64;
39 import org.hipparchus.util.Binary64Field;
40 import org.hipparchus.util.MathArrays;
41 import org.junit.jupiter.api.Assertions;
42 import org.junit.jupiter.api.BeforeEach;
43 import org.junit.jupiter.api.Test;
44 import org.junit.jupiter.params.ParameterizedTest;
45 import org.junit.jupiter.params.provider.EnumSource;
46 import org.orekit.TestUtils;
47 import org.orekit.Utils;
48 import org.orekit.attitudes.AttitudeProvider;
49 import org.orekit.attitudes.FrameAlignedProvider;
50 import org.orekit.attitudes.LofOffset;
51 import org.orekit.bodies.CelestialBody;
52 import org.orekit.bodies.CelestialBodyFactory;
53 import org.orekit.bodies.OneAxisEllipsoid;
54 import org.orekit.errors.OrekitInternalError;
55 import org.orekit.frames.Frame;
56 import org.orekit.frames.FramesFactory;
57 import org.orekit.frames.LOFType;
58 import org.orekit.orbits.CartesianOrbit;
59 import org.orekit.orbits.FieldCartesianOrbit;
60 import org.orekit.orbits.FieldOrbit;
61 import org.orekit.orbits.Orbit;
62 import org.orekit.orbits.OrbitType;
63 import org.orekit.orbits.PositionAngleType;
64 import org.orekit.propagation.AbstractPropagator;
65 import org.orekit.propagation.FieldSpacecraftState;
66 import org.orekit.propagation.MatricesHarvester;
67 import org.orekit.propagation.SpacecraftState;
68 import org.orekit.propagation.analytical.FieldKeplerianPropagator;
69 import org.orekit.propagation.events.*;
70 import org.orekit.propagation.events.handlers.*;
71 import org.orekit.propagation.events.intervals.FieldAdaptableInterval;
72 import org.orekit.propagation.integration.FieldAdditionalDerivativesProvider;
73 import org.orekit.propagation.integration.FieldCombinedDerivatives;
74 import org.orekit.propagation.numerical.FieldNumericalPropagator;
75 import org.orekit.propagation.numerical.NumericalPropagator;
76 import org.orekit.time.AbsoluteDate;
77 import org.orekit.time.FieldAbsoluteDate;
78 import org.orekit.utils.AbsolutePVCoordinates;
79 import org.orekit.utils.Constants;
80 import org.orekit.utils.FieldPVCoordinates;
81 import org.orekit.utils.PVCoordinates;
82
83 class FieldImpulseManeuverTest {
84
85 private final double mu = Constants.WGS84_EARTH_MU;
86 private final Frame inertialFrame = FramesFactory.getGCRF();
87 private final Vector3D initialPosition = new Vector3D(Constants.EGM96_EARTH_EQUATORIAL_RADIUS + 2000e3,
88 100., -1000.);
89 private final Vector3D initialVelocity = new Vector3D(-100., 7.5e3, 1.);
90 private final double initialMass = 1000.;
91 private final double stepSize = 60.;
92 private final double isp = 500.;
93 private final Vector3D deltaV = new Vector3D(0.1, -0.5, 0.2);
94 private final double timeOfFlight = 10000.;
95 private final OrbitType orbitType = OrbitType.EQUINOCTIAL;
96 private final PositionAngleType positionAngleType = PositionAngleType.ECCENTRIC;
97 private final LofOffset attitudeOverride = new LofOffset(inertialFrame, LOFType.QSW);
98 private final GradientField gradientField = GradientField.getField(2);
99 private final UnivariateDerivative1Field univariateDerivative1Field = new UnivariateDerivative1(0., 0.).getField();
100 private final UnivariateDerivative2Field univariateDerivative2Field = new UnivariateDerivative2(0., 0., 0.).getField();
101 private final ComplexField complexField = ComplexField.getInstance();
102 private enum DetectorType {
103 DATE_DETECTOR,
104 LATITUDE_CROSSING_DETECTOR,
105 ECLIPSE_DETECTOR
106 }
107
108 private static class DummyFieldAdditionalDerivatives implements FieldAdditionalDerivativesProvider<Gradient> {
109
110 DummyFieldAdditionalDerivatives() {
111
112 }
113
114 @Override
115 public String getName() {
116 return "dummyDerivativesName";
117 }
118
119 @Override
120 public int getDimension() {
121 return 1;
122 }
123
124 @Override
125 public FieldCombinedDerivatives<Gradient> combinedDerivatives(FieldSpacecraftState<Gradient> s) {
126 final GradientField field = s.getMass().getField();
127 Gradient[] pDot = MathArrays.buildArray(field, 1);
128 pDot[0] = field.getZero();
129 return new FieldCombinedDerivatives<>(pDot, null);
130 }
131
132 }
133
134 @BeforeEach
135 public void setUp() {
136 Utils.setDataRoot("regular-data");
137 }
138
139 @Deprecated
140 @Test
141 void testDeprecatedConstructors() {
142
143 final Complex zero = complexField.getZero();
144 final Complex complexIsp = zero.add(200.);
145 final FieldVector3D<Complex> deltaVSat = new FieldVector3D<>(complexField, Vector3D.PLUS_I);
146 final FieldAbsoluteDate<Complex> fieldAbsoluteDate = new FieldAbsoluteDate<>(complexField,
147 AbsoluteDate.ARBITRARY_EPOCH);
148 final FieldDateDetector<Complex> dateDetector = new FieldDateDetector<>(complexField, fieldAbsoluteDate);
149
150
151 final FieldImpulseManeuver<Complex> fieldImpulseManeuver = new FieldImpulseManeuver<>(dateDetector, null, deltaVSat, complexIsp,
152 Control3DVectorCostType.TWO_NORM);
153
154
155 Assertions.assertEquals(complexIsp, fieldImpulseManeuver.getIsp());
156 }
157
158 @Test
159 void testComplexConstructors() {
160
161 final Complex zero = complexField.getZero();
162 final Complex complexIsp = zero.add(200.);
163 final FieldVector3D<Complex> deltaVSat = new FieldVector3D<>(complexField, Vector3D.PLUS_I);
164 final FieldAbsoluteDate<Complex> fieldAbsoluteDate = new FieldAbsoluteDate<>(complexField,
165 AbsoluteDate.ARBITRARY_EPOCH);
166 final FieldEventDetectionSettings<Complex> detectionSettings = new FieldEventDetectionSettings<>(ComplexField.getInstance(),
167 EventDetectionSettings.getDefaultEventDetectionSettings());
168 final FieldDateDetector<Complex> dateDetector = new FieldDateDetector<>(complexField, fieldAbsoluteDate).withDetectionSettings(detectionSettings);
169
170
171 final FieldImpulseManeuver<Complex> fieldImpulseManeuver1 = new FieldImpulseManeuver<>(dateDetector, deltaVSat, complexIsp);
172 final FieldImpulseManeuver<Complex> fieldImpulseManeuver2 = new FieldImpulseManeuver<>(dateDetector, null, deltaVSat, complexIsp);
173
174
175 Assertions.assertEquals(fieldImpulseManeuver1.getTrigger(), fieldImpulseManeuver2.getTrigger());
176 Assertions.assertEquals(fieldImpulseManeuver1.getControl3DVectorCostType(), fieldImpulseManeuver2.getControl3DVectorCostType());
177 Assertions.assertEquals(fieldImpulseManeuver1.getDetectionSettings(), dateDetector.getDetectionSettings());
178 Assertions.assertEquals(fieldImpulseManeuver1.getAttitudeOverride(), fieldImpulseManeuver2.getAttitudeOverride());
179 Assertions.assertEquals(fieldImpulseManeuver1.getIsp(), fieldImpulseManeuver2.getIsp());
180 }
181
182 @Test
183 void testEventOccurredEventSlopeFilter() {
184
185 final Orbit orbit = TestUtils.getDefaultOrbit(AbsoluteDate.ARBITRARY_EPOCH);
186 final Binary64Field field = Binary64Field.getInstance();
187 final FieldOrbit<Binary64> fieldOrbit = new FieldCartesianOrbit<>(field, orbit);
188 final FieldApsideDetector<Binary64> detector = new FieldApsideDetector<>(fieldOrbit);
189 final FieldImpulseManeuver<Binary64> maneuver = new FieldImpulseManeuver<>(new FieldEventSlopeFilter<>(detector,
190 FilterType.TRIGGER_ONLY_INCREASING_EVENTS), FieldVector3D.getZero(field), Binary64.ONE);
191 final FieldKeplerianPropagator<Binary64> propagator = new FieldKeplerianPropagator<>(fieldOrbit);
192
193 propagator.addEventDetector(maneuver);
194 Assertions.assertDoesNotThrow(() -> propagator.propagate(fieldOrbit.getDate().shiftedBy(1e5)));
195 }
196
197 @Test
198 void testWithDetectionSettings() {
199
200 final Binary64Field field = Binary64Field.getInstance();
201 final FieldDateDetector<Binary64> fieldDateDetector = new FieldDateDetector<>(field);
202 final FieldImpulseManeuver<Binary64> fieldImpulseManeuver = new FieldImpulseManeuver<>(fieldDateDetector,
203 FieldVector3D.getZero(field), Binary64.ONE);
204 final FieldEventDetectionSettings<Binary64> expectedSettings = new FieldEventDetectionSettings<>(
205 FieldAdaptableInterval.of(1), new Binary64(2), 3);
206
207 final FieldImpulseManeuver<Binary64> maneuverWithSettings = fieldImpulseManeuver.withDetectionSettings(expectedSettings);
208
209 Assertions.assertEquals(fieldImpulseManeuver.getAttitudeOverride(), maneuverWithSettings.getAttitudeOverride());
210 Assertions.assertEquals(fieldImpulseManeuver.getIsp(), maneuverWithSettings.getIsp());
211 Assertions.assertEquals(fieldImpulseManeuver.getFieldImpulseProvider(), maneuverWithSettings.getFieldImpulseProvider());
212 Assertions.assertEquals(expectedSettings.getThreshold(), maneuverWithSettings.getThreshold());
213 Assertions.assertEquals(expectedSettings.getMaxIterationCount(), maneuverWithSettings.getMaxIterationCount());
214 Assertions.assertEquals(expectedSettings.getMaxCheckInterval(), maneuverWithSettings.getMaxCheckInterval());
215 }
216
217 @ParameterizedTest
218 @EnumSource(Action.class)
219 void testEventOccurred(final Action action) {
220
221 final Binary64Field field = Binary64Field.getInstance();
222 final FieldDateDetector<Binary64> fieldDateDetector = new FieldDateDetector<>(field);
223 final Orbit orbit = createOrbit();
224 final FieldCountingHandler<Binary64> handler = new FieldCountingHandler<Binary64>(0, action) {
225 @Override
226 protected boolean doesCount(FieldSpacecraftState<Binary64> state, FieldEventDetector<Binary64> detector, boolean increasing) {
227 return true;
228 }
229 };
230 final FieldImpulseManeuver<Binary64> fieldImpulseManeuver = new FieldImpulseManeuver<>(fieldDateDetector.withHandler(handler),
231 FieldVector3D.getZero(field), Binary64.ONE);
232
233 fieldImpulseManeuver.getHandler().eventOccurred(new FieldSpacecraftState<>(new FieldCartesianOrbit<>(field, orbit)),
234 fieldImpulseManeuver, true);
235
236 Assertions.assertEquals(1, handler.getCount());
237 }
238
239 @Test
240 void testResetStateAttitudeOverride() {
241
242 final Binary64Field field = Binary64Field.getInstance();
243 final FieldDateDetector<Binary64> fieldDateDetector = new FieldDateDetector<>(field);
244 final AbsolutePVCoordinates pvCoordinates = new AbsolutePVCoordinates(FramesFactory.getEME2000(),
245 AbsoluteDate.ARBITRARY_EPOCH, new Vector3D(1., 2, 3), new Vector3D(4, 5, 6));
246 final FieldSpacecraftState<Binary64> expectedState = new FieldSpacecraftState<>(field, new SpacecraftState(pvCoordinates));
247 final FieldImpulseManeuver<Binary64> fieldImpulseManeuver = new FieldImpulseManeuver<>(fieldDateDetector,
248 new FrameAlignedProvider(pvCoordinates.getFrame()), FieldVector3D.getZero(field), Binary64.ONE);
249
250 final FieldSpacecraftState<Binary64> actualState = fieldImpulseManeuver.getHandler().resetState(fieldImpulseManeuver, expectedState);
251
252 compareStates(expectedState, actualState);
253 }
254
255 @Test
256 void testResetState() {
257
258 final Binary64Field field = Binary64Field.getInstance();
259 final FieldDateDetector<Binary64> fieldDateDetector = new FieldDateDetector<>(field);
260 final AbsolutePVCoordinates pvCoordinates = new AbsolutePVCoordinates(FramesFactory.getEME2000(),
261 AbsoluteDate.ARBITRARY_EPOCH, new Vector3D(1., 2, 3), new Vector3D(4, 5, 6));
262 final FieldSpacecraftState<Binary64> expectedState = new FieldSpacecraftState<>(field, new SpacecraftState(pvCoordinates));
263 final FieldImpulseManeuver<Binary64> fieldImpulseManeuver = new FieldImpulseManeuver<>(fieldDateDetector,
264 FieldVector3D.getZero(field), Binary64.ONE);
265
266 final FieldSpacecraftState<Binary64> actualState = fieldImpulseManeuver.getHandler().resetState(fieldImpulseManeuver, expectedState);
267
268 compareStates(expectedState, actualState);
269 }
270
271 private static <T extends CalculusFieldElement<T>> void compareStates(final FieldSpacecraftState<T> expectedState,
272 final FieldSpacecraftState<T> actualState) {
273 Assertions.assertEquals(expectedState.getDate(), actualState.getDate());
274 Assertions.assertEquals(expectedState.getMass(), actualState.getMass());
275 Assertions.assertEquals(expectedState.getAttitude(), actualState.getAttitude());
276 Assertions.assertEquals(expectedState.getPosition(), actualState.getPosition());
277 Assertions.assertEquals(expectedState.getPVCoordinates().getVelocity(),
278 actualState.getPVCoordinates().getVelocity());
279 }
280
281 @Test
282 void testDeltaVNorm() {
283
284 final Complex complexIsp = complexField.getOne().add(200.);
285 final FieldVector3D<Complex> deltaVSat = new FieldVector3D<>(complexField, Vector3D.PLUS_I);
286 final FieldImpulseProvider<Complex> fieldImpulseProvider = FieldImpulseProvider.of(deltaVSat);
287 final FieldAbsoluteDate<Complex> fieldAbsoluteDate = new FieldAbsoluteDate<>(complexField,
288 AbsoluteDate.ARBITRARY_EPOCH);
289 final FieldDateDetector<Complex> dateDetector = new FieldDateDetector<>(complexField, fieldAbsoluteDate);
290
291
292 final FieldImpulseManeuver<Complex> fieldImpulseManeuverNorm1 = new FieldImpulseManeuver<>
293 (dateDetector.withHandler(new FieldStopOnEvent<>()), null, fieldImpulseProvider, complexIsp, Control3DVectorCostType.ONE_NORM);
294 final FieldImpulseManeuver<Complex> fieldImpulseManeuverNorm2 = new FieldImpulseManeuver<>
295 (dateDetector.withHandler(new FieldStopOnEvent<>()), null, fieldImpulseProvider, complexIsp, Control3DVectorCostType.TWO_NORM);
296 final FieldImpulseManeuver<Complex> fieldImpulseManeuverNormInf = new FieldImpulseManeuver<>
297 (dateDetector.withHandler(new FieldStopOnEvent<>()), null, fieldImpulseProvider, complexIsp, Control3DVectorCostType.INF_NORM);
298
299
300 Assertions.assertEquals(Control3DVectorCostType.ONE_NORM, fieldImpulseManeuverNorm1.getControl3DVectorCostType());
301 Assertions.assertEquals(Control3DVectorCostType.TWO_NORM, fieldImpulseManeuverNorm2.getControl3DVectorCostType());
302 Assertions.assertEquals(Control3DVectorCostType.INF_NORM, fieldImpulseManeuverNormInf.getControl3DVectorCostType());
303 }
304
305 @Test
306 void testEclipseDetectorDerivativeStructure() {
307 templateDetector(new DSFactory(1, 1).getDerivativeField(),
308 DetectorType.ECLIPSE_DETECTOR, Control3DVectorCostType.TWO_NORM);
309 }
310
311 @Test
312 void testEclipseDetectorGradient() {
313 templateDetector(gradientField, DetectorType.ECLIPSE_DETECTOR, Control3DVectorCostType.TWO_NORM);
314 }
315
316 @Test
317 void testEclipseDetectorGradientNormInf() {
318 templateDetector(gradientField, DetectorType.ECLIPSE_DETECTOR, Control3DVectorCostType.INF_NORM);
319 }
320
321 @Test
322 void testDateDetectorComplex() {
323 templateDetector(complexField, DetectorType.DATE_DETECTOR, Control3DVectorCostType.TWO_NORM);
324 }
325
326 @Test
327 void testDateDetectorUnivariateDerivative2() {
328 templateDetector(univariateDerivative2Field, DetectorType.DATE_DETECTOR, Control3DVectorCostType.TWO_NORM);
329 }
330
331 @Test
332 void testDateDetectorGradientNorm1() {
333 templateDetector(gradientField, DetectorType.DATE_DETECTOR, Control3DVectorCostType.ONE_NORM);
334 }
335
336 @Test
337 void testLatitudeCrossingDetectorUnivariateDerivative1() {
338 templateDetector(univariateDerivative1Field, DetectorType.LATITUDE_CROSSING_DETECTOR, Control3DVectorCostType.TWO_NORM);
339 }
340
341 @Test
342 void testLatitudeCrossingDetectorDerivativeStructure() {
343 templateDetector(new DSFactory(1, 1).getDerivativeField(),
344 DetectorType.LATITUDE_CROSSING_DETECTOR, Control3DVectorCostType.TWO_NORM);
345 }
346
347 private <T extends CalculusFieldElement<T>> FieldImpulseManeuver<T> convertManeuver(
348 final Field<T> field, final ImpulseManeuver impulseManeuver, final FieldEventHandler<T> fieldHandler) {
349 final T fieldIsp = field.getZero().add(impulseManeuver.getIsp());
350 final EventDetector detector = impulseManeuver.getTrigger();
351 FieldAbstractDetector<?, T> fieldDetector;
352 if (detector instanceof DateDetector) {
353 fieldDetector = new FieldDateDetector<>(field, new FieldAbsoluteDate<>(field, ((DateDetector) detector).getDate()));
354 } else if (detector instanceof LatitudeCrossingDetector) {
355 fieldDetector = new FieldLatitudeCrossingDetector<>(field,
356 ((LatitudeCrossingDetector) detector).getBody(),
357 ((LatitudeCrossingDetector) detector).getLatitude());
358 } else if (detector instanceof EclipseDetector) {
359 fieldDetector = new FieldEclipseDetector<>(field,
360 ((EclipseDetector) detector).getOccultationEngine());
361 } else {
362 throw new OrekitInternalError(null);
363 }
364
365 return new FieldImpulseManeuver<>(fieldDetector.withDetectionSettings(new FieldEventDetectionSettings<>(field, detector.getDetectionSettings()))
366 .withHandler(fieldHandler),
367 impulseManeuver.getAttitudeOverride(), FieldImpulseProvider.of(impulseManeuver.getImpulseProvider()), fieldIsp, impulseManeuver.getControl3DVectorCostType());
368 }
369
370 private <T extends CalculusFieldElement<T>> void templateDetector(final Field<T> field,
371 final DetectorType detectorType,
372 final Control3DVectorCostType control3DVectorCostType) {
373
374 final Orbit initialOrbit = createOrbit();
375 final NumericalPropagator propagator = createUnperturbedPropagator(initialOrbit, initialMass);
376 final FieldNumericalPropagator<T> fieldPropagator = createUnperturbedFieldPropagator(field,
377 initialOrbit, propagator.getInitialState().getMass());
378 fieldPropagator.setOrbitType(propagator.getOrbitType());
379 final AbsoluteDate endOfPropagationDate = propagator.getInitialState().getDate().shiftedBy(timeOfFlight);
380 final ImpulseManeuver impulseManeuver = new ImpulseManeuver(
381 buildEventDetector(detectorType, propagator).withHandler(new StopOnEvent()),
382 attitudeOverride, ImpulseProvider.of(deltaV), isp, control3DVectorCostType);
383 propagator.addEventDetector(impulseManeuver);
384 fieldPropagator.addEventDetector(convertManeuver(field, impulseManeuver, new FieldStopOnEvent<>()));
385
386 final SpacecraftState
387 terminalState = propagator.propagate(endOfPropagationDate);
388 final FieldSpacecraftState<T> fieldTerminalState = fieldPropagator.propagate(new FieldAbsoluteDate<>(field, endOfPropagationDate));
389
390 compareStateToConstantOfFieldState(terminalState, fieldTerminalState);
391 }
392
393 private AbstractDetector<?> buildEventDetector(final DetectorType detectorType,
394 final AbstractPropagator propagator) {
395 final CelestialBody earth = CelestialBodyFactory.getEarth();
396 final AbstractDetector<?> eventDetector;
397 switch (detectorType) {
398 case DATE_DETECTOR:
399 eventDetector = new DateDetector(propagator.getInitialState().getDate().
400 shiftedBy(timeOfFlight / 2.));
401 break;
402
403 case LATITUDE_CROSSING_DETECTOR:
404
405 eventDetector = new LatitudeCrossingDetector(new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
406 Constants.WGS84_EARTH_FLATTENING, earth.getBodyOrientedFrame()),
407 0.);
408 break;
409
410 case ECLIPSE_DETECTOR:
411 eventDetector = new EclipseDetector(CelestialBodyFactory.getSun(),Constants.SUN_RADIUS,
412 new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
413 Constants.WGS84_EARTH_FLATTENING, earth.getBodyOrientedFrame()));
414 break;
415
416 default:
417 throw new OrekitInternalError(null);
418 }
419 return eventDetector;
420 }
421
422 private NumericalPropagator createUnperturbedPropagator(final Orbit initialOrbit,
423 final double initialMass) {
424 final ClassicalRungeKuttaIntegrator integrator = new ClassicalRungeKuttaIntegrator(stepSize);
425 final NumericalPropagator propagator = new NumericalPropagator(integrator);
426 propagator.setInitialState(new SpacecraftState(initialOrbit, initialMass));
427 propagator.setOrbitType(orbitType);
428 propagator.setPositionAngleType(positionAngleType);
429 return propagator;
430 }
431
432 private <T extends CalculusFieldElement<T>> FieldNumericalPropagator<T> createUnperturbedFieldPropagator(
433 final Field<T> field, final Orbit initialOrbit, final double initialMass) {
434 final T fieldStepSize = field.getOne().add(stepSize);
435 final ClassicalRungeKuttaFieldIntegrator<T> fieldIntegrator = new ClassicalRungeKuttaFieldIntegrator<>(field,
436 fieldStepSize);
437 final FieldNumericalPropagator<T> fieldPropagator = new FieldNumericalPropagator<>(field,
438 fieldIntegrator);
439 final FieldOrbit<T> fieldInitialOrbit = createConstantFieldOrbit(field, initialOrbit);
440 final T fieldInitialMass = field.getZero().add(initialMass);
441 fieldPropagator.setInitialState(new FieldSpacecraftState<>(fieldInitialOrbit, fieldInitialMass));
442 fieldPropagator.setOrbitType(orbitType);
443 fieldPropagator.setPositionAngleType(positionAngleType);
444 return fieldPropagator;
445 }
446
447 private <T extends CalculusFieldElement<T>> void compareStateToConstantOfFieldState(final SpacecraftState state,
448 final FieldSpacecraftState<T> fieldState) {
449 final Orbit orbit = state.getOrbit();
450 final FieldOrbit<T> fieldOrbit = fieldState.getOrbit();
451 final double[] orbitAsArray = new double[6];
452 final PositionAngleType positionAngle = PositionAngleType.TRUE;
453 final OrbitType type = OrbitType.CARTESIAN;
454 type.mapOrbitToArray(orbit, positionAngle, orbitAsArray, orbitAsArray.clone());
455 final double[] fieldRealOrbitAsArray = orbitAsArray.clone();
456 type.mapOrbitToArray(fieldOrbit.toOrbit(), positionAngle, fieldRealOrbitAsArray, fieldRealOrbitAsArray.clone());
457 final double tolPos = 5e-2;
458 final double tolVel = 3e-5;
459 for (int i = 0; i < 3; i++) {
460 Assertions.assertEquals(orbitAsArray[i], fieldRealOrbitAsArray[i], tolPos);
461 Assertions.assertEquals(orbitAsArray[i + 3], fieldRealOrbitAsArray[i + 3], tolVel);
462 }
463 Assertions.assertEquals(state.getMass(), fieldState.getMass().getReal(), 1e-3);
464
465 }
466
467 private CartesianOrbit createOrbit() {
468 final PVCoordinates pvCoordinates = new PVCoordinates(initialPosition, initialVelocity);
469 return new CartesianOrbit(pvCoordinates, inertialFrame, AbsoluteDate.ARBITRARY_EPOCH, mu);
470 }
471
472 private <T extends CalculusFieldElement<T>> FieldCartesianOrbit<T> createConstantFieldOrbit(final Field<T> field,
473 final Orbit orbit) {
474 final PVCoordinates pvCoordinates = orbit.getPVCoordinates();
475 final FieldVector3D<T> fieldPosition = new FieldVector3D<>(field, pvCoordinates.getPosition());
476 final FieldVector3D<T> fieldVelocity = new FieldVector3D<>(field, pvCoordinates.getVelocity());
477 final FieldPVCoordinates<T> fieldPVCoordinates = new FieldPVCoordinates<>(fieldPosition,
478 fieldVelocity);
479 return new FieldCartesianOrbit<>(fieldPVCoordinates, inertialFrame,
480 new FieldAbsoluteDate<>(field, orbit.getDate()), field.getZero().add(mu));
481 }
482
483 @Test
484 void testAdditionalStatePropagation() {
485
486 final UnivariateDerivative1 zero = univariateDerivative1Field.getZero();
487 final FieldNumericalPropagator<UnivariateDerivative1> fieldPropagator = createFieldPropagatorForAdditionalStatesAndDerivatives(
488 univariateDerivative1Field);
489 FieldSpacecraftState<UnivariateDerivative1> initialState = fieldPropagator.getInitialState();
490 final String name = "dummy";
491 initialState = initialState.addAdditionalData(name, zero);
492 fieldPropagator.resetInitialState(initialState);
493
494 final FieldAbsoluteDate<UnivariateDerivative1> targetDate = initialState.getDate().shiftedBy(zero.add(10000.));
495 final FieldSpacecraftState<UnivariateDerivative1> terminalState = fieldPropagator.propagate(targetDate);
496
497 final UnivariateDerivative1 actualValue = terminalState.getAdditionalState(name)[0];
498 Assertions.assertEquals(zero, actualValue);
499 }
500
501 @Test
502 void testAdditionalStateDerivativesPropagation() {
503
504 final Gradient zero = gradientField.getZero();
505 final FieldNumericalPropagator<Gradient> fieldPropagator = createFieldPropagatorForAdditionalStatesAndDerivatives(
506 gradientField);
507 FieldSpacecraftState<Gradient> initialState = fieldPropagator.getInitialState();
508 final DummyFieldAdditionalDerivatives additionalDerivatives = new DummyFieldAdditionalDerivatives();
509 final String name = additionalDerivatives.getName();
510 final Gradient[] dummyState = new Gradient[] { zero };
511 fieldPropagator.addAdditionalDerivativesProvider(additionalDerivatives);
512 initialState = initialState.addAdditionalData(name, dummyState);
513 fieldPropagator.resetInitialState(initialState);
514
515 final FieldAbsoluteDate<Gradient> targetDate = initialState.getDate().shiftedBy(zero.add(10000.));
516 final FieldSpacecraftState<Gradient> terminalState = fieldPropagator.propagate(targetDate);
517
518 final Gradient actualValue = terminalState.getAdditionalStatesDerivatives().get(name)[0];
519 Assertions.assertEquals(dummyState[0], actualValue);
520 }
521
522 private <T extends CalculusFieldElement<T>> FieldNumericalPropagator<T> createFieldPropagatorForAdditionalStatesAndDerivatives(
523 final Field<T> field) {
524 final Orbit initialOrbit = createOrbit();
525 final NumericalPropagator propagator = createUnperturbedPropagator(initialOrbit, initialMass);
526 final DateDetector dateDetector = new DateDetector(propagator.getInitialState().getDate().shiftedBy(100.));
527 final ImpulseManeuver impulseManeuver = new ImpulseManeuver(dateDetector, Vector3D.PLUS_I, isp);
528 final FieldNumericalPropagator<T> fieldPropagator = createUnperturbedFieldPropagator(field,
529 initialOrbit, propagator.getInitialState().getMass());
530 fieldPropagator.addEventDetector(convertManeuver(field, impulseManeuver, new FieldStopOnEvent<>()));
531 return fieldPropagator;
532 }
533
534 @Test
535 void testBackAndForthPropagation() {
536
537 final Orbit initialOrbit = createOrbit();
538 final NumericalPropagator propagator = createUnperturbedPropagator(initialOrbit, initialMass);
539 final DateDetector dateDetector = new DateDetector(propagator.getInitialState().getDate().shiftedBy(-100.));
540 final ImpulseManeuver impulseManeuver = new ImpulseManeuver(dateDetector, Vector3D.PLUS_I, isp);
541 final UnivariateDerivative1 zero = univariateDerivative1Field.getZero();
542 final FieldNumericalPropagator<UnivariateDerivative1> fieldPropagator = createUnperturbedFieldPropagator(univariateDerivative1Field,
543 initialOrbit, initialMass);
544 fieldPropagator.setAttitudeProvider(propagator.getAttitudeProvider());
545 fieldPropagator.setResetAtEnd(true);
546 fieldPropagator.addEventDetector(convertManeuver(univariateDerivative1Field, impulseManeuver, new FieldContinueOnEvent<>()));
547
548 final UnivariateDerivative1 backwardDuration = zero.add(-10000.);
549 final FieldAbsoluteDate<UnivariateDerivative1> fieldEpoch = fieldPropagator.getInitialState().getDate();
550 fieldPropagator.propagate(fieldEpoch.shiftedBy(backwardDuration));
551 final FieldSpacecraftState<UnivariateDerivative1> actualFieldState = fieldPropagator.propagate(fieldEpoch);
552
553 final SpacecraftState expectedState = propagator.getInitialState();
554 compareStateToConstantOfFieldState(expectedState, actualFieldState);
555 }
556
557 @Test
558 void testVersusCartesianStateTransitionMatrix() {
559
560 final int freeParameters = 3;
561 final GradientField field = GradientField.getField(freeParameters);
562 final Orbit initialOrbit = createOrbit();
563 final NumericalPropagator propagator = createUnperturbedPropagator(initialOrbit, initialMass);
564 final FieldNumericalPropagator<Gradient> fieldPropagator = createUnperturbedFieldPropagator(field,
565 initialOrbit, initialMass);
566 fieldPropagator.setOrbitType(propagator.getOrbitType());
567 final AbsoluteDate endOfPropagationDate = propagator.getInitialState().getDate().shiftedBy(timeOfFlight);
568 final DateDetector dateDetector = (DateDetector) buildEventDetector(DetectorType.DATE_DETECTOR, propagator);
569 final AttitudeProvider attitudeProvider = new FrameAlignedProvider(propagator.getFrame());
570 propagator.addEventDetector(dateDetector);
571 propagator.setOrbitType(OrbitType.CARTESIAN);
572 final Gradient zero = field.getZero();
573 final FieldDateDetector<Gradient> fieldDateDetector =
574 new FieldDateDetector<>(field, new FieldAbsoluteDate<>(field, dateDetector.getDate()));
575 final FieldVector3D<Gradient> fieldDeltaV = new FieldVector3D<>(
576 Gradient.variable(freeParameters, 0, 0.),
577 Gradient.variable(freeParameters, 1, 0.),
578 Gradient.variable(freeParameters, 2, 0.));
579 fieldPropagator.addEventDetector(new FieldImpulseManeuver<>(fieldDateDetector, attitudeProvider,
580 fieldDeltaV, zero.add(isp)));
581 final String stmAdditionalName = "stm";
582 final MatricesHarvester harvester = propagator.setupMatricesComputation(stmAdditionalName, null, null);
583
584 final SpacecraftState intermediateState = propagator.propagate(dateDetector.getDate());
585 final RealMatrix stm1 = harvester.getStateTransitionMatrix(intermediateState);
586 final SpacecraftState terminalState = propagator.propagate(endOfPropagationDate);
587 final RealMatrix stm2 = harvester.getStateTransitionMatrix(terminalState);
588 final DecompositionSolver decompositionSolver = new LUDecomposition(stm1).getSolver();
589 final RealMatrix stm = stm2.multiply(decompositionSolver.getInverse());
590 final FieldSpacecraftState<Gradient> fieldTerminalState = fieldPropagator.
591 propagate(new FieldAbsoluteDate<>(field, endOfPropagationDate));
592
593 final FieldVector3D<Gradient> fieldTerminalPosition = fieldTerminalState.getPosition();
594 final FieldVector3D<Gradient> fieldTerminalVelocity = fieldTerminalState.getPVCoordinates().getVelocity();
595 final double tolerance = 1e0;
596 for (int i = 0; i < 3; i++) {
597 Assertions.assertEquals(stm.getEntry(0, 3 + i),
598 fieldTerminalPosition.getX().getPartialDerivative(i), tolerance);
599 Assertions.assertEquals(stm.getEntry(1, 3 + i),
600 fieldTerminalPosition.getY().getPartialDerivative(i), tolerance);
601 Assertions.assertEquals(stm.getEntry(2, 3 + i),
602 fieldTerminalPosition.getZ().getPartialDerivative(i), tolerance);
603 Assertions.assertEquals(stm.getEntry(3, 3 + i),
604 fieldTerminalVelocity.getX().getPartialDerivative(i), tolerance);
605 Assertions.assertEquals(stm.getEntry(4, 3 + i),
606 fieldTerminalVelocity.getY().getPartialDerivative(i), tolerance);
607 Assertions.assertEquals(stm.getEntry(5, 3 + i),
608 fieldTerminalVelocity.getZ().getPartialDerivative(i), tolerance);
609 }
610 }
611
612 }