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.Decimal64Field;
25 import org.hipparchus.util.MathArrays;
26 import org.junit.After;
27 import org.junit.Assert;
28 import org.junit.Before;
29 import org.junit.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.numerical.FieldNumericalPropagator;
40 import org.orekit.propagation.numerical.NumericalPropagator;
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(Decimal64Field.getInstance());
58 }
59
60
61
62 @Test
63 public void testInitDSST() {
64 doTestInitDSST(Decimal64Field.getInstance());
65 }
66
67 @Test
68 public void testResetState() {
69 doTestResetState(Decimal64Field.getInstance());
70 }
71
72 @Test
73 public void testYield() {
74 doTestYield(Decimal64Field.getInstance());
75 }
76
77 private <T extends CalculusFieldElement<T>> void doTestInitNumerical(Field<T> field) {
78
79 final double reference = 1.25;
80 final double rate = 1.5;
81 final double dt = 600.0;
82 Linear<T> linear = new Linear<>("linear", reference, rate);
83 Assert.assertFalse(linear.wasCalled());
84
85
86 AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200,
87 tolerance[0], tolerance[1]);
88 integrator.setInitialStepSize(60);
89 FieldNumericalPropagator<T> propagatorNumerical = new FieldNumericalPropagator<>(field, integrator);
90 propagatorNumerical.setInitialState(new FieldSpacecraftState<>(field, initialState).
91 addAdditionalState(linear.getName(), field.getZero().newInstance(reference)));
92 propagatorNumerical.addAdditionalDerivativesProvider(linear);
93 FieldSpacecraftState<T> finalState = propagatorNumerical.propagate(new FieldAbsoluteDate<>(field, initDate).shiftedBy(dt));
94
95
96 Assert.assertTrue(linear.wasCalled());
97 Assert.assertEquals(reference + dt * rate, finalState.getAdditionalState(linear.getName())[0].getReal(), 1.0e-10);
98
99 }
100
101 private <T extends CalculusFieldElement<T>> void doTestInitDSST(Field<T> field) {
102
103 final double reference = 3.5;
104 final double rate = 1.5;
105 final double dt = 600.0;
106 Linear<T> linear = new Linear<>("linear", reference, rate);
107 Assert.assertFalse(linear.wasCalled());
108
109
110 AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200,
111 tolerance[0], tolerance[1]);
112 integrator.setInitialStepSize(60);
113 FieldDSSTPropagator<T> propagatorDSST = new FieldDSSTPropagator<>(field, integrator);
114 propagatorDSST.setInitialState(new FieldSpacecraftState<>(field, initialState).
115 addAdditionalState(linear.getName(), field.getZero().newInstance(reference)));
116 propagatorDSST.addAdditionalDerivativesProvider(linear);
117 FieldSpacecraftState<T> finalState = propagatorDSST.propagate(new FieldAbsoluteDate<>(field, initDate).shiftedBy(dt));
118
119
120 Assert.assertTrue(linear.wasCalled());
121 Assert.assertEquals(reference + dt * rate, finalState.getAdditionalState(linear.getName())[0].getReal(), 1.0e-10);
122
123 }
124
125 private <T extends CalculusFieldElement<T>> void doTestResetState(Field<T> field) {
126
127 final double reference1 = 3.5;
128 final double rate1 = 1.5;
129 Linear<T> linear1 = new Linear<>("linear-1", reference1, rate1);
130 Assert.assertFalse(linear1.wasCalled());
131 final double reference2 = 4.5;
132 final double rate2 = 1.25;
133 Linear<T> linear2 = new Linear<>("linear-2", reference2, rate2);
134 Assert.assertFalse(linear2.wasCalled());
135 final double dt = 600;
136
137
138 AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200,
139 tolerance[0], tolerance[1]);
140 integrator.setInitialStepSize(60);
141 FieldNumericalPropagator<T> propagatorNumerical = new FieldNumericalPropagator<>(field, integrator);
142 propagatorNumerical.setInitialState(new FieldSpacecraftState<>(field, initialState).
143 addAdditionalState(linear1.getName(), field.getZero().newInstance(reference1)).
144 addAdditionalState(linear2.getName(), field.getZero().newInstance(reference2)));
145 propagatorNumerical.addAdditionalDerivativesProvider(linear1);
146 propagatorNumerical.addAdditionalDerivativesProvider(linear2);
147 FieldSpacecraftState<T> finalState = propagatorNumerical.propagate(new FieldAbsoluteDate<>(field, initDate).shiftedBy(dt));
148
149
150 Assert.assertTrue(linear1.wasCalled());
151 Assert.assertTrue(linear2.wasCalled());
152 Assert.assertEquals(reference1 + dt * rate1, finalState.getAdditionalState(linear1.getName())[0].getReal(), 1.0e-10);
153 Assert.assertEquals(reference2 + dt * rate2, finalState.getAdditionalState(linear2.getName())[0].getReal(), 1.0e-10);
154
155 }
156
157 private <T extends CalculusFieldElement<T>> void doTestYield(Field<T> field) {
158
159
160 final double init1 = 1.0;
161 final double init2 = 2.0;
162 final double rate = 0.5;
163 final double dt = 600;
164 Yield<T> yield1 = new Yield<>(null, "yield-1", rate);
165 Yield<T> yield2 = new Yield<>(yield1.getName(), "yield-2", Double.NaN);
166
167
168 AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200,
169 tolerance[0], tolerance[1]);
170 integrator.setInitialStepSize(60);
171 FieldNumericalPropagator<T> propagatorNumerical = new FieldNumericalPropagator<>(field, integrator);
172 propagatorNumerical.setInitialState(new FieldSpacecraftState<>(field, initialState).
173 addAdditionalState(yield1.getName(), field.getZero().newInstance(init1)).
174 addAdditionalState(yield2.getName(), field.getZero().newInstance(init2)));
175 propagatorNumerical.addAdditionalDerivativesProvider(yield2);
176 propagatorNumerical.addAdditionalDerivativesProvider(yield1);
177 FieldSpacecraftState<T> finalState = propagatorNumerical.propagate(new FieldAbsoluteDate<>(field, initDate).shiftedBy(dt));
178
179
180 Assert.assertEquals(init1 + dt * rate, finalState.getAdditionalState(yield1.getName())[0].getReal(), 1.0e-10);
181 Assert.assertEquals(init2 + dt * rate, finalState.getAdditionalState(yield2.getName())[0].getReal(), 1.0e-10);
182 Assert.assertEquals(rate, finalState.getAdditionalStateDerivative(yield1.getName())[0].getReal(), 1.0e-10);
183 Assert.assertEquals(rate, finalState.getAdditionalStateDerivative(yield2.getName())[0].getReal(), 1.0e-10);
184
185 }
186
187 @Before
188 public void setUp() {
189 Utils.setDataRoot("regular-data:potential/shm-format");
190 GravityFieldFactory.addPotentialCoefficientsReader(new SHMFormatReader("^eigen_cg03c_coef$", false));
191 mu = GravityFieldFactory.getUnnormalizedProvider(0, 0).getMu();
192 final Vector3D position = new Vector3D(7.0e6, 1.0e6, 4.0e6);
193 final Vector3D velocity = new Vector3D(-500.0, 8000.0, 1000.0);
194 initDate = AbsoluteDate.J2000_EPOCH;
195 final Orbit orbit = new EquinoctialOrbit(new PVCoordinates(position, velocity),
196 FramesFactory.getEME2000(), initDate, mu);
197 initialState = new SpacecraftState(orbit);
198 tolerance = NumericalPropagator.tolerances(0.001, orbit, OrbitType.EQUINOCTIAL);
199 }
200
201 @After
202 public void tearDown() {
203 initDate = null;
204 initialState = null;
205 tolerance = null;
206 }
207
208 private static class Linear<T extends CalculusFieldElement<T>> implements FieldAdditionalDerivativesProvider<T> {
209
210 private String name;
211 private double expectedAtInit;
212 private double rate;
213 private boolean called;
214
215 public Linear(final String name, final double expectedAtInit, final double rate) {
216 this.name = name;
217 this.expectedAtInit = expectedAtInit;
218 this.rate = rate;
219 this.called = false;
220 }
221
222 @Override
223 public void init(FieldSpacecraftState<T> initiaState, FieldAbsoluteDate<T> target) {
224 Assert.assertEquals(expectedAtInit, initiaState.getAdditionalState(getName())[0].getReal(), 1.0e-15);
225 called = true;
226 }
227
228 @Override
229 public T[] derivatives(FieldSpacecraftState<T> s) {
230 final T[] pDot = MathArrays.buildArray(s.getDate().getField(), 1);
231 pDot[0] = s.getDate().getField().getZero().newInstance(rate);
232 return pDot;
233 }
234
235 @Override
236 public int getDimension() {
237 return 1;
238 }
239
240 @Override
241 public String getName() {
242 return name;
243 }
244
245 public boolean wasCalled() {
246 return called;
247 }
248
249 }
250
251 private static class Yield<T extends CalculusFieldElement<T>> implements FieldAdditionalDerivativesProvider<T> {
252
253 private String dependency;
254 private String name;
255 private double rate;
256
257 public Yield(final String dependency, final String name, final double rate) {
258 this.dependency = dependency;
259 this.name = name;
260 this.rate = rate;
261 }
262
263 @Override
264 public T[] derivatives(final FieldSpacecraftState<T> s) {
265 final T[] pDot;
266 if (dependency == null) {
267 pDot = MathArrays.buildArray(s.getDate().getField(), 1);
268 pDot[0] = s.getDate().getField().getZero().newInstance(rate);
269 } else {
270 pDot = s.getAdditionalStateDerivative(dependency);
271 }
272 return pDot;
273 }
274
275 @Override
276 public boolean yield(final FieldSpacecraftState<T> state) {
277 return dependency != null && !state.hasAdditionalStateDerivative(dependency);
278 }
279
280 @Override
281 public int getDimension() {
282 return 1;
283 }
284
285 @Override
286 public String getName() {
287 return name;
288 }
289
290 }
291
292 }