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 org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator;
23 import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
24 import org.hipparchus.util.Binary64Field;
25 import org.hipparchus.util.MathArrays;
26 import org.junit.jupiter.api.AfterEach;
27 import org.junit.jupiter.api.Assertions;
28 import org.junit.jupiter.api.BeforeEach;
29 import org.junit.jupiter.api.Test;
30 import org.orekit.Utils;
31 import org.orekit.forces.gravity.potential.GravityFieldFactory;
32 import org.orekit.forces.gravity.potential.SHMFormatReader;
33 import org.orekit.frames.FramesFactory;
34 import org.orekit.orbits.EquinoctialOrbit;
35 import org.orekit.orbits.Orbit;
36 import org.orekit.orbits.OrbitType;
37 import org.orekit.propagation.FieldSpacecraftState;
38 import org.orekit.propagation.SpacecraftState;
39 import org.orekit.propagation.ToleranceProvider;
40 import org.orekit.propagation.numerical.FieldNumericalPropagator;
41 import org.orekit.propagation.semianalytical.dsst.FieldDSSTPropagator;
42 import org.orekit.time.AbsoluteDate;
43 import org.orekit.time.FieldAbsoluteDate;
44 import org.orekit.utils.PVCoordinates;
45
46 public class FieldAdditionalDerivativesProvidersTest {
47
48 private double mu;
49 private AbsoluteDate initDate;
50 private SpacecraftState initialState;
51 private double[][] tolerance;
52
53
54
55 @Test
56 public void testInitNumerical() {
57 doTestInitNumerical(Binary64Field.getInstance());
58 }
59
60
61
62 @Test
63 public void testInitDSST() {
64 doTestInitDSST(Binary64Field.getInstance());
65 }
66
67 @Test
68 public void testResetState() {
69 doTestResetState(Binary64Field.getInstance());
70 }
71
72 @Test
73 public void testYield() {
74 doTestYield(Binary64Field.getInstance());
75 }
76
77 @Test
78 public void testCoupling() {
79 doTestCoupling(Binary64Field.getInstance());
80 }
81
82 private <T extends CalculusFieldElement<T>> void doTestInitNumerical(Field<T> field) {
83
84 final double reference = 1.25;
85 final double rate = 1.5;
86 final double dt = 600.0;
87 Linear<T> linear = new Linear<>("linear", reference, rate);
88 Assertions.assertFalse(linear.wasCalled());
89
90
91 AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200,
92 tolerance[0], tolerance[1]);
93 integrator.setInitialStepSize(60);
94 FieldNumericalPropagator<T> propagatorNumerical = new FieldNumericalPropagator<>(field, integrator);
95 propagatorNumerical.setInitialState(new FieldSpacecraftState<>(field, initialState).
96 addAdditionalData(linear.getName(), field.getZero().newInstance(reference)));
97 propagatorNumerical.addAdditionalDerivativesProvider(linear);
98 FieldSpacecraftState<T> finalState = propagatorNumerical.propagate(new FieldAbsoluteDate<>(field, initDate).shiftedBy(dt));
99
100
101 Assertions.assertTrue(linear.wasCalled());
102 Assertions.assertEquals(reference + dt * rate, finalState.getAdditionalState(linear.getName())[0].getReal(), 1.0e-10);
103
104 }
105
106 private <T extends CalculusFieldElement<T>> void doTestInitDSST(Field<T> field) {
107
108 final double reference = 3.5;
109 final double rate = 1.5;
110 final double dt = 600.0;
111 Linear<T> linear = new Linear<>("linear", reference, rate);
112 Assertions.assertFalse(linear.wasCalled());
113
114
115 AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200,
116 tolerance[0], tolerance[1]);
117 integrator.setInitialStepSize(60);
118 FieldDSSTPropagator<T> propagatorDSST = new FieldDSSTPropagator<>(field, integrator);
119 propagatorDSST.setInitialState(new FieldSpacecraftState<>(field, initialState).
120 addAdditionalData(linear.getName(), field.getZero().newInstance(reference)));
121 propagatorDSST.addAdditionalDerivativesProvider(linear);
122 FieldSpacecraftState<T> finalState = propagatorDSST.propagate(new FieldAbsoluteDate<>(field, initDate).shiftedBy(dt));
123
124
125 Assertions.assertTrue(linear.wasCalled());
126 Assertions.assertEquals(reference + dt * rate, finalState.getAdditionalState(linear.getName())[0].getReal(), 1.0e-10);
127
128 }
129
130 private <T extends CalculusFieldElement<T>> void doTestResetState(Field<T> field) {
131
132 final double reference1 = 3.5;
133 final double rate1 = 1.5;
134 Linear<T> linear1 = new Linear<>("linear-1", reference1, rate1);
135 Assertions.assertFalse(linear1.wasCalled());
136 final double reference2 = 4.5;
137 final double rate2 = 1.25;
138 Linear<T> linear2 = new Linear<>("linear-2", reference2, rate2);
139 Assertions.assertFalse(linear2.wasCalled());
140 final double dt = 600;
141
142
143 AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200,
144 tolerance[0], tolerance[1]);
145 integrator.setInitialStepSize(60);
146 FieldNumericalPropagator<T> propagatorNumerical = new FieldNumericalPropagator<>(field, integrator);
147 propagatorNumerical.setInitialState(new FieldSpacecraftState<>(field, initialState).
148 addAdditionalData(linear1.getName(), field.getZero().newInstance(reference1)).
149 addAdditionalData(linear2.getName(), field.getZero().newInstance(reference2)));
150 propagatorNumerical.addAdditionalDerivativesProvider(linear1);
151 propagatorNumerical.addAdditionalDerivativesProvider(linear2);
152 FieldSpacecraftState<T> finalState = propagatorNumerical.propagate(new FieldAbsoluteDate<>(field, initDate).shiftedBy(dt));
153
154
155 Assertions.assertTrue(linear1.wasCalled());
156 Assertions.assertTrue(linear2.wasCalled());
157 Assertions.assertEquals(reference1 + dt * rate1, finalState.getAdditionalState(linear1.getName())[0].getReal(), 1.0e-10);
158 Assertions.assertEquals(reference2 + dt * rate2, finalState.getAdditionalState(linear2.getName())[0].getReal(), 1.0e-10);
159
160 }
161
162 private <T extends CalculusFieldElement<T>> void doTestYield(Field<T> field) {
163
164
165 final double init1 = 1.0;
166 final double init2 = 2.0;
167 final double rate = 0.5;
168 final double dt = 600;
169 Yield<T> yield1 = new Yield<>(null, "yield-1", rate);
170 Yield<T> yield2 = new Yield<>(yield1.getName(), "yield-2", Double.NaN);
171
172
173 AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200,
174 tolerance[0], tolerance[1]);
175 integrator.setInitialStepSize(60);
176 FieldNumericalPropagator<T> propagatorNumerical = new FieldNumericalPropagator<>(field, integrator);
177 propagatorNumerical.setInitialState(new FieldSpacecraftState<>(field, initialState).
178 addAdditionalData(yield1.getName(), field.getZero().newInstance(init1)).
179 addAdditionalData(yield2.getName(), field.getZero().newInstance(init2)));
180 propagatorNumerical.addAdditionalDerivativesProvider(yield2);
181 propagatorNumerical.addAdditionalDerivativesProvider(yield1);
182 FieldSpacecraftState<T> finalState = propagatorNumerical.propagate(new FieldAbsoluteDate<>(field, initDate).shiftedBy(dt));
183
184
185 Assertions.assertEquals(init1 + dt * rate, finalState.getAdditionalState(yield1.getName())[0].getReal(), 1.0e-10);
186 Assertions.assertEquals(init2 + dt * rate, finalState.getAdditionalState(yield2.getName())[0].getReal(), 1.0e-10);
187 Assertions.assertEquals(rate, finalState.getAdditionalStateDerivative(yield1.getName())[0].getReal(), 1.0e-10);
188 Assertions.assertEquals(rate, finalState.getAdditionalStateDerivative(yield2.getName())[0].getReal(), 1.0e-10);
189
190 }
191
192 private <T extends CalculusFieldElement<T>> void doTestCoupling(Field<T> field) {
193
194
195 final T dt = field.getZero().newInstance(600.0);
196 final Coupling<T> coupling = new Coupling<>("coupling", 3.5, -2.0, 1.0);
197
198
199 AdaptiveStepsizeFieldIntegrator<T> integrator =
200 new DormandPrince853FieldIntegrator<>(field, 0.001, 200,
201 tolerance[0], tolerance[1]);
202 integrator.setInitialStepSize(60);
203 FieldNumericalPropagator<T> propagatorNumerical = new FieldNumericalPropagator<>(field, integrator);
204 propagatorNumerical.setInitialState(new FieldSpacecraftState<>(field, initialState).
205 addAdditionalData(coupling.getName(),
206 field.getZero().newInstance(coupling.secondaryInit)));
207 propagatorNumerical.addAdditionalDerivativesProvider(coupling);
208 FieldSpacecraftState<T> finalState = propagatorNumerical.propagate(new FieldAbsoluteDate<>(field, initDate).shiftedBy(dt));
209
210
211 Assertions.assertEquals(coupling.secondaryInit + dt.getReal() * coupling.secondaryRate,
212 finalState.getAdditionalState(coupling.getName())[0].getReal(),
213 1.0e-10);
214 Assertions.assertEquals(initialState.getOrbit().getA() + dt.getReal() * coupling.smaRate,
215 finalState.getOrbit().getA().getReal(),
216 1.0e-10);
217
218 }
219
220 @BeforeEach
221 public void setUp() {
222 Utils.setDataRoot("regular-data:potential/shm-format");
223 GravityFieldFactory.addPotentialCoefficientsReader(new SHMFormatReader("^eigen_cg03c_coef$", false));
224 mu = GravityFieldFactory.getUnnormalizedProvider(0, 0).getMu();
225 final Vector3D position = new Vector3D(7.0e6, 1.0e6, 4.0e6);
226 final Vector3D velocity = new Vector3D(-500.0, 8000.0, 1000.0);
227 initDate = AbsoluteDate.J2000_EPOCH;
228 final Orbit orbit = new EquinoctialOrbit(new PVCoordinates(position, velocity),
229 FramesFactory.getEME2000(), initDate, mu);
230 initialState = new SpacecraftState(orbit);
231 tolerance = ToleranceProvider.getDefaultToleranceProvider(0.001).getTolerances(orbit, OrbitType.EQUINOCTIAL);
232 }
233
234 @AfterEach
235 public void tearDown() {
236 initDate = null;
237 initialState = null;
238 tolerance = null;
239 }
240
241 private static class Linear<T extends CalculusFieldElement<T>> implements FieldAdditionalDerivativesProvider<T> {
242
243 private String name;
244 private double expectedAtInit;
245 private double rate;
246 private boolean called;
247
248 public Linear(final String name, final double expectedAtInit, final double rate) {
249 this.name = name;
250 this.expectedAtInit = expectedAtInit;
251 this.rate = rate;
252 this.called = false;
253 }
254
255 @Override
256 public void init(FieldSpacecraftState<T> initiaState, FieldAbsoluteDate<T> target) {
257 Assertions.assertEquals(expectedAtInit, initiaState.getAdditionalState(getName())[0].getReal(), 1.0e-15);
258 called = true;
259 }
260
261 @Override
262 public FieldCombinedDerivatives<T> combinedDerivatives(FieldSpacecraftState<T> s) {
263 final T[] pDot = MathArrays.buildArray(s.getDate().getField(), 1);
264 pDot[0] = s.getDate().getField().getZero().newInstance(rate);
265 return new FieldCombinedDerivatives<>(pDot, null);
266 }
267
268 @Override
269 public int getDimension() {
270 return 1;
271 }
272
273 @Override
274 public String getName() {
275 return name;
276 }
277
278 public boolean wasCalled() {
279 return called;
280 }
281
282 }
283
284 private static class Yield<T extends CalculusFieldElement<T>> implements FieldAdditionalDerivativesProvider<T> {
285
286 private String dependency;
287 private String name;
288 private double rate;
289
290 public Yield(final String dependency, final String name, final double rate) {
291 this.dependency = dependency;
292 this.name = name;
293 this.rate = rate;
294 }
295
296 @Override
297 public FieldCombinedDerivatives<T> combinedDerivatives(FieldSpacecraftState<T> s) {
298 final T[] pDot;
299 if (dependency == null) {
300 pDot = MathArrays.buildArray(s.getDate().getField(), 1);
301 pDot[0] = s.getDate().getField().getZero().newInstance(rate);
302 } else {
303 pDot = s.getAdditionalStateDerivative(dependency);
304 }
305 return new FieldCombinedDerivatives<>(pDot, null);
306 }
307
308 @Override
309 public boolean yields(final FieldSpacecraftState<T> state) {
310 return dependency != null && !state.hasAdditionalStateDerivative(dependency);
311 }
312
313 @Override
314 public int getDimension() {
315 return 1;
316 }
317
318 @Override
319 public String getName() {
320 return name;
321 }
322
323 }
324
325 private static class Coupling<T extends CalculusFieldElement<T>> implements FieldAdditionalDerivativesProvider<T> {
326
327 private final String name;
328 private final double secondaryInit;
329 private final double secondaryRate;
330 private final double smaRate;
331
332 public Coupling(final String name,
333 final double secondaryInit, final double secondaryRate,
334 final double smaRate) {
335 this.name = name;
336 this.secondaryInit = secondaryInit;
337 this.secondaryRate = secondaryRate;
338 this.smaRate = smaRate;
339 }
340
341 @Override
342 public FieldCombinedDerivatives<T> combinedDerivatives(FieldSpacecraftState<T> s) {
343 final Field<T> field = s.getDate().getField();
344 T[] pDot = MathArrays.buildArray(field, 1);
345 T[] mainIncrement = MathArrays.buildArray(field, 6);
346 mainIncrement[0] = field.getZero().newInstance(smaRate);
347 mainIncrement[1] = field.getZero();
348 mainIncrement[2] = field.getZero();
349 mainIncrement[3] = field.getZero();
350 mainIncrement[4] = field.getZero();
351 mainIncrement[5] = field.getZero();
352 pDot[0] = field.getZero().newInstance(secondaryRate);
353 return new FieldCombinedDerivatives<>(pDot, mainIncrement);
354 }
355
356 @Override
357 public int getDimension() {
358 return 1;
359 }
360
361 @Override
362 public String getName() {
363 return name;
364 }
365
366 }
367
368 }