1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import static org.orekit.OrekitMatchers.relativelyCloseTo;
20
21 import java.lang.reflect.InvocationTargetException;
22 import java.lang.reflect.Method;
23 import java.util.ArrayList;
24 import java.util.List;
25 import java.util.function.Function;
26
27 import org.hamcrest.MatcherAssert;
28 import org.hipparchus.CalculusFieldElement;
29 import org.hipparchus.Field;
30 import org.hipparchus.analysis.UnivariateFunction;
31 import org.hipparchus.analysis.differentiation.DSFactory;
32 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
33 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
34 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
35 import org.hipparchus.linear.FieldMatrixPreservingVisitor;
36 import org.hipparchus.linear.MatrixUtils;
37 import org.hipparchus.util.Decimal64Field;
38 import org.hipparchus.util.FastMath;
39 import org.hipparchus.util.MathArrays;
40 import org.hipparchus.util.MathUtils;
41 import org.junit.Assert;
42 import org.junit.Before;
43 import org.junit.Test;
44 import org.orekit.Utils;
45 import org.orekit.frames.Frame;
46 import org.orekit.frames.FramesFactory;
47 import org.orekit.frames.Transform;
48 import org.orekit.propagation.analytical.FieldEcksteinHechlerPropagator;
49 import org.orekit.time.FieldAbsoluteDate;
50 import org.orekit.time.TimeScalesFactory;
51 import org.orekit.utils.Constants;
52 import org.orekit.utils.FieldPVCoordinates;
53 import org.orekit.utils.TimeStampedFieldPVCoordinates;
54
55
56 public class FieldCartesianOrbitTest {
57
58
59
60 private double mu;
61
62 @Before
63 public void setUp() {
64 Utils.setDataRoot("regular-data");
65
66 mu = 3.9860047e14;
67 }
68
69 @Test
70 public void testCartesianToCartesian()
71 throws NoSuchMethodException, SecurityException, IllegalAccessException, IllegalArgumentException, InvocationTargetException {
72 doTestCartesianToCartesian(Decimal64Field.getInstance());
73 }
74
75 @Test
76 public void testCartesianToEquinoctial() {
77 doTestCartesianToEquinoctial(Decimal64Field.getInstance());
78 }
79
80 @Test
81 public void testCartesianToKeplerian() {
82 doTestCartesianToKeplerian(Decimal64Field.getInstance());
83 }
84
85 @Test
86 public void testPositionVelocityNorms() {
87 doTestPositionVelocityNorms(Decimal64Field.getInstance());
88 }
89
90 @Test
91 public void testGeometry() {
92 doTestGeometry(Decimal64Field.getInstance());
93 }
94
95 @Test
96 public void testHyperbola1() {
97 doTestHyperbola1(Decimal64Field.getInstance());
98 }
99
100 @Test
101 public void testHyperbola2() {
102 doTestHyperbola2(Decimal64Field.getInstance());
103 }
104
105 @Test
106 public void testNumericalIssue25() {
107 doTestNumericalIssue25(Decimal64Field.getInstance());
108 }
109
110 @Test
111 public void testDerivativesConversionSymmetry() {
112 doTestDerivativesConversionSymmetry(Decimal64Field.getInstance());
113 }
114
115 @Test
116 public void testDerivativesConversionSymmetryHyperbolic() {
117 doTestDerivativesConversionSymmetryHyperbolic(Decimal64Field.getInstance());
118 }
119
120 @Test
121 public void testShiftElliptic() {
122 doTestShiftElliptic(Decimal64Field.getInstance());
123 }
124
125 @Test
126 public void testShiftCircular() {
127 doTestShiftCircular(Decimal64Field.getInstance());
128 }
129
130 @Test
131 public void testShiftEquinoctial() {
132 doTestShiftEquinoctial(Decimal64Field.getInstance());
133 }
134
135 @Test
136 public void testShiftHyperbolic() {
137 doTestShiftHyperbolic(Decimal64Field.getInstance());
138 }
139
140 @Test
141 public void testNumericalIssue135() {
142 doTestNumericalIssue135(Decimal64Field.getInstance());
143 }
144
145 @Test
146 public void testJacobianReference() {
147 doTestJacobianReference(Decimal64Field.getInstance());
148 }
149
150 @Test
151 public void testInterpolationWithDerivatives() {
152 doTestInterpolation(Decimal64Field.getInstance(), true,
153 394, 2.28e-8, 3.21, 1.39e-9,
154 2474, 6842, 6.55, 186);
155 }
156
157 @Test
158 public void testInterpolationWithoutDerivatives() {
159 doTestInterpolation(Decimal64Field.getInstance(), false,
160 394, 2.61, 3.21, 0.154,
161 2474, 2.28e12, 6.55, 6.22e10);
162 }
163
164 @Test(expected=IllegalArgumentException.class)
165 public void testErr1(){
166 doTestErr1(Decimal64Field.getInstance());
167 }
168
169 @Test
170 public void testToOrbitWithoutDerivatives() {
171 doTestToOrbitWithoutDerivatives(Decimal64Field.getInstance());
172 }
173
174 @Test
175 public void testToOrbitWithDerivatives() {
176 doTestToOrbitWithDerivatives(Decimal64Field.getInstance());
177 }
178
179 @Test
180 public void testToString() {
181 doTestToString(Decimal64Field.getInstance());
182 }
183
184 @Test
185 public void testNonKeplerianDerivatives() {
186 doTestNonKeplerianDerivatives(Decimal64Field.getInstance());
187 }
188
189 @Test
190 public void testEquatorialRetrograde() {
191 doTestEquatorialRetrograde(Decimal64Field.getInstance());
192 }
193
194 @Test
195 public void testCopyNonKeplerianAcceleration() {
196 doTestCopyNonKeplerianAcceleration(Decimal64Field.getInstance());
197 }
198
199 @Test
200 public void testNormalize() {
201 doTestNormalize(Decimal64Field.getInstance());
202 }
203
204 private <T extends CalculusFieldElement<T>> void doTestCartesianToCartesian(Field<T> field)
205 throws NoSuchMethodException, SecurityException, IllegalAccessException, IllegalArgumentException, InvocationTargetException {
206
207 T zero = field.getZero();
208 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
209
210 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-29536113.0), zero.add(30329259.0), zero.add(-100125.0));
211 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-2194.0), zero.add(-2141.0), zero.add(-8.0));
212 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
213 double mu = 3.9860047e14;
214
215 FieldCartesianOrbit<T> p = new FieldCartesianOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(), date, zero.add(mu));
216
217 Assert.assertEquals(p.getPVCoordinates().getPosition().getX().getReal(), FieldPVCoordinates.getPosition().getX().getReal(), Utils.epsilonTest * FastMath.abs(FieldPVCoordinates.getPosition().getX().getReal()));
218 Assert.assertEquals(p.getPVCoordinates().getPosition().getY().getReal(), FieldPVCoordinates.getPosition().getY().getReal(), Utils.epsilonTest * FastMath.abs(FieldPVCoordinates.getPosition().getY().getReal()));
219 Assert.assertEquals(p.getPVCoordinates().getPosition().getZ().getReal(), FieldPVCoordinates.getPosition().getZ().getReal(), Utils.epsilonTest * FastMath.abs(FieldPVCoordinates.getPosition().getZ().getReal()));
220 Assert.assertEquals(p.getPVCoordinates().getVelocity().getX().getReal(), FieldPVCoordinates.getVelocity().getX().getReal(), Utils.epsilonTest * FastMath.abs(FieldPVCoordinates.getVelocity().getX().getReal()));
221 Assert.assertEquals(p.getPVCoordinates().getVelocity().getY().getReal(), FieldPVCoordinates.getVelocity().getY().getReal(), Utils.epsilonTest * FastMath.abs(FieldPVCoordinates.getVelocity().getY().getReal()));
222 Assert.assertEquals(p.getPVCoordinates().getVelocity().getZ().getReal(), FieldPVCoordinates.getVelocity().getZ().getReal(), Utils.epsilonTest * FastMath.abs(FieldPVCoordinates.getVelocity().getZ().getReal()));
223
224 Method initPV = FieldCartesianOrbit.class.getDeclaredMethod("initPVCoordinates", new Class[0]);
225 initPV.setAccessible(true);
226 Assert.assertSame(p.getPVCoordinates(), initPV.invoke(p, new Object[0]));
227
228 }
229
230 private <T extends CalculusFieldElement<T>> void doTestCartesianToEquinoctial(Field<T> field) {
231 T zero = field.getZero();
232
233 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-29536113.0), zero.add(30329259.0), zero.add(-100125.0));
234 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-2194.0), zero.add(-2141.0), zero.add(-8.0));
235 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
236
237 FieldCartesianOrbit<T> p = new FieldCartesianOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(),
238 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
239
240 Assert.assertEquals(42255170.0028257, p.getA().getReal(), Utils.epsilonTest * p.getA().getReal());
241 Assert.assertEquals(0.592732497856475e-03, p.getEquinoctialEx().getReal(), Utils.epsilonE * FastMath.abs(p.getE().getReal()));
242 Assert.assertEquals(-0.206274396964359e-02, p.getEquinoctialEy().getReal(), Utils.epsilonE * FastMath.abs(p.getE().getReal()));
243 Assert.assertEquals(FastMath.sqrt(FastMath.pow(0.592732497856475e-03, 2)+FastMath.pow(-0.206274396964359e-02, 2)), p.getE().getReal(), Utils.epsilonAngle * FastMath.abs(p.getE().getReal()));
244 Assert.assertEquals(MathUtils.normalizeAngle(2*FastMath.asin(FastMath.sqrt((FastMath.pow(0.128021863908325e-03, 2)+FastMath.pow(-0.352136186881817e-02, 2))/4.)), p.getI().getReal()), p.getI().getReal(), Utils.epsilonAngle * FastMath.abs(p.getI().getReal()));
245 Assert.assertEquals(MathUtils.normalizeAngle(0.234498139679291e+01, p.getLM().getReal()), p.getLM().getReal(), Utils.epsilonAngle * FastMath.abs(p.getLM().getReal()));
246
247
248 FieldCartesianOrbit<T> q = new FieldCartesianOrbit<>(p);
249
250 Assert.assertEquals(42255170.0028257, q.getA().getReal(), Utils.epsilonTest * q.getA().getReal());
251 Assert.assertEquals(0.592732497856475e-03, q.getEquinoctialEx().getReal(), Utils.epsilonE * FastMath.abs(q.getE().getReal()));
252 Assert.assertEquals(-0.206274396964359e-02, q.getEquinoctialEy().getReal(), Utils.epsilonE * FastMath.abs(q.getE().getReal()));
253 Assert.assertEquals(FastMath.sqrt(FastMath.pow(0.592732497856475e-03, 2)+FastMath.pow(-0.206274396964359e-02, 2)), q.getE().getReal(), Utils.epsilonAngle * FastMath.abs(q.getE().getReal()));
254 Assert.assertEquals(MathUtils.normalizeAngle(2*FastMath.asin(FastMath.sqrt((FastMath.pow(0.128021863908325e-03, 2)+FastMath.pow(-0.352136186881817e-02, 2))/4.)), q.getI().getReal()), q.getI().getReal(), Utils.epsilonAngle * FastMath.abs(q.getI().getReal()));
255 Assert.assertEquals(MathUtils.normalizeAngle(0.234498139679291e+01, q.getLM().getReal()), q.getLM().getReal(), Utils.epsilonAngle * FastMath.abs(q.getLM().getReal()));
256
257 Assert.assertNull(q.getADot());
258 Assert.assertNull(q.getEquinoctialExDot());
259 Assert.assertNull(q.getEquinoctialEyDot());
260 Assert.assertNull(q.getHxDot());
261 Assert.assertNull(q.getHyDot());
262 Assert.assertNull(q.getLvDot());
263 Assert.assertNull(q.getEDot());
264 Assert.assertNull(q.getIDot());
265
266 }
267
268 private <T extends CalculusFieldElement<T>> void doTestCartesianToKeplerian(Field<T> field){
269 T zero = field.getZero();
270
271 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-26655470.0), zero.add(29881667.0), zero.add(-113657.0));
272 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-1125.0), zero.add(-1122.0), zero.add(195.0));
273 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
274 double mu = 3.9860047e14;
275
276 FieldCartesianOrbit<T> p = new FieldCartesianOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(),
277 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
278 FieldKeplerianOrbit<T> kep = new FieldKeplerianOrbit<>(p);
279
280 Assert.assertEquals(22979265.3030773, p.getA().getReal(), Utils.epsilonTest * p.getA().getReal());
281 Assert.assertEquals(0.743502611664700, p.getE().getReal(), Utils.epsilonE * FastMath.abs(p.getE().getReal()));
282 Assert.assertEquals(0.122182096220906, p.getI().getReal(), Utils.epsilonAngle * FastMath.abs(p.getI().getReal()));
283 T pa = kep.getPerigeeArgument();
284 Assert.assertEquals(MathUtils.normalizeAngle(3.09909041016672, pa.getReal()), pa.getReal(),
285 Utils.epsilonAngle * FastMath.abs(pa.getReal()));
286 T raan = kep.getRightAscensionOfAscendingNode();
287 Assert.assertEquals(MathUtils.normalizeAngle(2.32231010979999, raan.getReal()), raan.getReal(),
288 Utils.epsilonAngle * FastMath.abs(raan.getReal()));
289 T m = kep.getMeanAnomaly();
290 Assert.assertEquals(MathUtils.normalizeAngle(3.22888977629034, m.getReal()), m.getReal(),
291 Utils.epsilonAngle * FastMath.abs(FastMath.abs(m.getReal())));
292 }
293
294 private <T extends CalculusFieldElement<T>> void doTestPositionVelocityNorms(Field<T> field){ T zero=field.getZero();T one=field.getOne(); FieldAbsoluteDate<T> date=new FieldAbsoluteDate<>(field);
295
296 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-29536113.0), zero.add(30329259.0), zero.add(-100125.0));
297 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-2194.0), zero.add(-2141.0), zero.add(-8.0));
298 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
299
300 FieldCartesianOrbit<T> p = new FieldCartesianOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(), date, zero.add(mu));
301
302 T e = p.getE();
303 T v = new FieldKeplerianOrbit<>(p).getTrueAnomaly();
304 T ksi = e.multiply(v.cos()).add(1);
305 T nu = e.multiply(v.sin());
306 T epsilon = one.subtract(e).multiply(e.add(1)).sqrt();
307
308 T a = p.getA();
309 T na = a.reciprocal().multiply(mu).sqrt();
310
311
312 Assert.assertEquals(a.getReal() * epsilon.getReal() * epsilon.getReal() / ksi.getReal(),
313 p.getPVCoordinates().getPosition().getNorm().getReal(),
314 Utils.epsilonTest * FastMath.abs(p.getPVCoordinates().getPosition().getNorm().getReal()));
315
316
317 Assert.assertEquals(na.getReal() * FastMath.sqrt(ksi.getReal() * ksi.getReal() + nu .getReal()* nu.getReal()) / epsilon.getReal(),
318 p.getPVCoordinates().getVelocity().getNorm().getReal(),
319 Utils.epsilonTest * FastMath.abs(p.getPVCoordinates().getVelocity().getNorm().getReal()));
320
321 }
322
323 private <T extends CalculusFieldElement<T>> void doTestGeometry(Field<T> field) {
324 T zero = field.getZero();
325
326 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-29536113.0), zero.add(30329259.0), zero.add(-100125.0));
327 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-2194.0), zero.add(-2141.0), zero.add(-8.0));
328 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
329
330 FieldVector3D<T> momentum = FieldPVCoordinates.getMomentum().normalize();
331
332 FieldEquinoctialOrbit<T> p = new FieldEquinoctialOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(),
333 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
334
335 T apogeeRadius = p.getA().multiply( p.getE().add(1.0));
336 T perigeeRadius = p.getA().multiply( p.getE().negate().add(1.0));
337
338 for (T lv = zero; lv.getReal() <= 2 * FastMath.PI; lv = lv.add(2 * FastMath.PI/100.)) {
339 p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(), p.getEquinoctialEy(),
340 p.getHx(), p.getHy(), lv, PositionAngle.TRUE, p.getFrame(),
341 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
342 position = p.getPVCoordinates().getPosition();
343
344
345
346 Assert.assertTrue((position.getNorm().getReal() - apogeeRadius.getReal()) <= ( apogeeRadius.getReal() * Utils.epsilonTest));
347 Assert.assertTrue((position.getNorm().getReal() - perigeeRadius.getReal()) >= (- perigeeRadius.getReal() * Utils.epsilonTest));
348
349
350
351 position= position.normalize();
352 velocity = p.getPVCoordinates().getVelocity().normalize();
353
354
355
356
357 Assert.assertTrue(FastMath.abs(FieldVector3D.dotProduct(position, momentum).getReal()) < Utils.epsilonTest);
358
359 Assert.assertTrue(FastMath.abs(FieldVector3D.dotProduct(velocity, momentum).getReal()) < Utils.epsilonTest);
360 }
361 }
362
363 private <T extends CalculusFieldElement<T>> void doTestHyperbola1(final Field<T> field) {
364 T zero = field.getZero();
365 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(new FieldKeplerianOrbit<>(zero.add(-10000000.0), zero.add(2.5), zero.add(0.3),
366 zero, zero, zero,
367 PositionAngle.TRUE,
368 FramesFactory.getEME2000(), new FieldAbsoluteDate<>(field),
369 zero.add(mu)));
370 FieldVector3D<T> perigeeP = orbit.getPVCoordinates().getPosition();
371 FieldVector3D<T> u = perigeeP.normalize();
372 FieldVector3D<T> focus1 = new FieldVector3D<>(zero, zero, zero);
373 FieldVector3D<T> focus2 = new FieldVector3D<>(orbit.getA().multiply(-2).multiply(orbit.getE()), u);
374 for (T dt = zero.add(-5000); dt.getReal() < 5000; dt = dt.add(60)) {
375 FieldPVCoordinates<T> pv = orbit.shiftedBy(dt).getPVCoordinates();
376 T d1 = FieldVector3D.distance(pv.getPosition(), focus1);
377 T d2 = FieldVector3D.distance(pv.getPosition(), focus2);
378 Assert.assertEquals(orbit.getA().multiply(-2).getReal(), d1.subtract(d2).abs().getReal(), 1.0e-6);
379 FieldCartesianOrbit<T> rebuilt =
380 new FieldCartesianOrbit<>(pv, orbit.getFrame(), orbit.getDate().shiftedBy(dt), zero.add(mu));
381 Assert.assertEquals(-10000000.0, rebuilt.getA().getReal(), 1.0e-6);
382 Assert.assertEquals(2.5, rebuilt.getE().getReal(), 1.0e-13);
383 }
384 }
385
386 private <T extends CalculusFieldElement<T>> void doTestHyperbola2(final Field<T> field) {
387 T zero = field.getZero();
388 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(new FieldKeplerianOrbit<>(zero.add(-10000000.0), zero.add(1.2), zero.add(0.3),
389 zero, zero, zero.add(-1.75),
390 PositionAngle.MEAN,
391 FramesFactory.getEME2000(), new FieldAbsoluteDate<>(field),
392 zero.add(mu)));
393 FieldVector3D<T> perigeeP = new FieldKeplerianOrbit<>(zero.add(-10000000.0), zero.add(1.2), zero.add(0.3),
394 zero, zero, zero,
395 PositionAngle.TRUE,
396 orbit.getFrame(), orbit.getDate(), orbit.getMu()).getPVCoordinates().getPosition();
397 FieldVector3D<T> u = perigeeP.normalize();
398 FieldVector3D<T> focus1 = new FieldVector3D<>(zero, zero, zero);
399 FieldVector3D<T> focus2 = new FieldVector3D<>(orbit.getA().multiply(-2).multiply(orbit.getE()), u);
400 for (T dt = zero.add(-5000); dt.getReal() < 5000; dt = dt.add(60)) {
401 FieldPVCoordinates<T> pv = orbit.shiftedBy(dt).getPVCoordinates();
402 T d1 = FieldVector3D.distance(pv.getPosition(), focus1);
403 T d2 = FieldVector3D.distance(pv.getPosition(), focus2);
404 Assert.assertEquals(orbit.getA().multiply(-2).getReal(), d1.subtract(d2).abs().getReal(), 1.0e-6);
405 FieldCartesianOrbit<T> rebuilt =
406 new FieldCartesianOrbit<>(pv, orbit.getFrame(), orbit.getDate().shiftedBy(dt), zero.add(mu));
407 Assert.assertEquals(-10000000.0, rebuilt.getA().getReal(), 1.0e-6);
408 Assert.assertEquals(1.2, rebuilt.getE().getReal(), 1.0e-13);
409 }
410 }
411
412 private <T extends CalculusFieldElement<T>> void doTestNumericalIssue25(Field<T> field) {
413 T zero = field.getZero();
414 FieldVector3D<T> position = new FieldVector3D<>(zero.add(3782116.14107698), zero.add(416663.11924914), zero.add(5875541.62103057));
415 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-6349.7848910501), zero.add(288.4061811651), zero.add(4066.9366759691));
416 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(new FieldPVCoordinates<>(position, velocity),
417 FramesFactory.getEME2000(),
418 new FieldAbsoluteDate<>(field, "2004-01-01T23:00:00.000",
419 TimeScalesFactory.getUTC()),
420 zero.add(3.986004415E14));
421 Assert.assertEquals(0.0, orbit.getE().getReal(), 2.0e-14);
422 }
423
424 private <T extends CalculusFieldElement<T>> void doTestDerivativesConversionSymmetry(Field<T> field) {
425 T zero = field.getZero();
426 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:01:20.000", TimeScalesFactory.getUTC());
427 FieldVector3D<T> position = new FieldVector3D<>(zero.add(6893443.400234382),
428 zero.add(1886406.1073757345),
429 zero.add(-589265.1150359757));
430 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-281.1261461082365),
431 zero.add(-1231.6165642450928),
432 zero.add(-7348.756363469432));
433 FieldVector3D<T> acceleration = new FieldVector3D<>(zero.add(-7.460341170581685),
434 zero.add(-2.0415957334584527),
435 zero.add(0.6393322823627762));
436 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>( position, velocity, acceleration);
437 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(pvCoordinates, FramesFactory.getEME2000(),
438 date, zero.add(Constants.EIGEN5C_EARTH_MU));
439 Assert.assertTrue(orbit.hasDerivatives());
440 T r2 = position.getNormSq();
441 T r = r2.sqrt();
442 FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(orbit.getMu().negate()),
443 position);
444 Assert.assertEquals(0.0101, FieldVector3D.distance(keplerianAcceleration, acceleration).getReal(), 1.0e-4);
445
446 for (OrbitType type : OrbitType.values()) {
447 FieldOrbit<T> converted = type.convertType(orbit);
448 Assert.assertTrue(converted.hasDerivatives());
449 FieldCartesianOrbit<T> rebuilt = (FieldCartesianOrbit<T>) OrbitType.CARTESIAN.convertType(converted);
450 Assert.assertTrue(rebuilt.hasDerivatives());
451 Assert.assertEquals(0, FieldVector3D.distance(rebuilt.getPVCoordinates().getPosition(), position).getReal(), 2.0e-9);
452 Assert.assertEquals(0, FieldVector3D.distance(rebuilt.getPVCoordinates().getVelocity(), velocity).getReal(), 7.0e-12);
453 Assert.assertEquals(0, FieldVector3D.distance(rebuilt.getPVCoordinates().getAcceleration(), acceleration).getReal(), 4.9e-15);
454 }
455
456 }
457
458 private <T extends CalculusFieldElement<T>> void doTestDerivativesConversionSymmetryHyperbolic(Field<T> field) {
459 T zero = field.getZero();
460 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
461 FieldVector3D<T> position = new FieldVector3D<>(zero.add(224267911.905821),
462 zero.add(290251613.109399),
463 zero.add(45534292.777492));
464 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-1494.068165293),
465 zero.add(1124.771027677),
466 zero.add(526.915286134));
467 FieldVector3D<T> acceleration = new FieldVector3D<>(zero.add(-0.001295920501),
468 zero.add(-0.002233045187),
469 zero.add(-0.000349906292));
470 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>( position, velocity, acceleration);
471 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(pvCoordinates, FramesFactory.getEME2000(),
472 date, zero.add(Constants.EIGEN5C_EARTH_MU));
473 Assert.assertTrue(orbit.hasDerivatives());
474 T r2 = position.getNormSq();
475 T r = r2.sqrt();
476 FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(orbit.getMu().negate()),
477 position);
478 Assert.assertEquals(4.78e-4, FieldVector3D.distance(keplerianAcceleration, acceleration).getReal(), 1.0e-6);
479
480 OrbitType type = OrbitType.KEPLERIAN;
481 FieldOrbit<T> converted = type.convertType(orbit);
482 Assert.assertTrue(converted.hasDerivatives());
483 FieldCartesianOrbit<T> rebuilt = (FieldCartesianOrbit<T>) OrbitType.CARTESIAN.convertType(converted);
484 Assert.assertTrue(rebuilt.hasDerivatives());
485 Assert.assertEquals(0, FieldVector3D.distance(rebuilt.getPVCoordinates().getPosition(), position).getReal(), 1.0e-15);
486 Assert.assertEquals(0, FieldVector3D.distance(rebuilt.getPVCoordinates().getVelocity(), velocity).getReal(), 1.0e-15);
487 Assert.assertEquals(0, FieldVector3D.distance(rebuilt.getPVCoordinates().getAcceleration(), acceleration).getReal(), 1.0e-15);
488
489 }
490
491 private <T extends CalculusFieldElement<T>> void doTestShiftElliptic(Field<T> field) {
492 T zero = field.getZero();
493 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-29536113.0), zero.add(30329259.0), zero.add(-100125.0));
494 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-2194.0), zero.add(-2141.0), zero.add(-8.0));
495 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
496 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(),
497 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
498 testShift(orbit, new FieldKeplerianOrbit<>(orbit), 2e-15);
499 }
500
501 private <T extends CalculusFieldElement<T>> void doTestShiftCircular(Field<T> field) {
502 T zero = field.getZero();
503 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-29536113.0), zero.add(30329259.0), zero.add(-100125.0));
504 FieldVector3D<T> velocity = new FieldVector3D<>(position.getNorm().reciprocal().multiply(mu).sqrt(), position.orthogonal());
505 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
506 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(),
507 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
508 testShift(orbit, new FieldCircularOrbit<>(orbit), 2.0e-15);
509 }
510
511 private <T extends CalculusFieldElement<T>> void doTestShiftEquinoctial(Field<T> field) {
512 T zero = field.getZero();
513 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-29536113.0), zero.add(30329259.0), zero.add(-100125.0));
514 FieldVector3D<T> velocity = new FieldVector3D<>(position.getNorm().reciprocal().multiply(mu).sqrt(), position.orthogonal());
515 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
516 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(),
517 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
518 testShift(orbit, new FieldEquinoctialOrbit<>(orbit), 5.0e-14);
519 }
520
521 private <T extends CalculusFieldElement<T>> void doTestShiftHyperbolic(Field<T> field) {
522 T zero = field.getZero();
523 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-29536113.0), zero.add(30329259.0), zero.add(-100125.0));
524 FieldVector3D<T> velocity = new FieldVector3D<>(position.getNorm().reciprocal().multiply(mu).sqrt().multiply(3.0), position.orthogonal());
525 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>(position, velocity);
526 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(),
527 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
528 testShift(orbit, new FieldKeplerianOrbit<>(orbit), 2.0e-15);
529 }
530
531 private <T extends CalculusFieldElement<T>> void doTestNumericalIssue135(Field<T> field) {
532 T zero = field.getZero();
533 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-6.7884943832e7), zero.add(-2.1423006112e7), zero.add(-3.1603915377e7));
534 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-4732.55), zero.add(-2472.086), zero.add(-3022.177));
535 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>(position, velocity);
536 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(),
537 FieldAbsoluteDate.getJ2000Epoch(field),
538 zero.add(324858598826460.));
539 testShift(orbit, new FieldKeplerianOrbit<>(orbit), 6.0e-15);
540 }
541
542 private <T extends CalculusFieldElement<T>> void testShift(FieldCartesianOrbit<T> tested, FieldOrbit<T> reference, double threshold) {
543 Field<T> field = tested.getA().getField();
544 T zero = field.getZero();
545 for (T dt = zero.add(- 1000); dt.getReal() < 1000; dt = dt.add(10.0)) {
546
547 FieldPVCoordinates<T> pvTested = tested.shiftedBy(dt).getPVCoordinates();
548 FieldVector3D<T> pTested = pvTested.getPosition();
549 FieldVector3D<T> vTested = pvTested.getVelocity();
550
551 FieldPVCoordinates<T> pvReference = reference.shiftedBy(dt).getPVCoordinates();
552 FieldVector3D<T> pReference = pvReference.getPosition();
553 FieldVector3D<T> vReference = pvReference.getVelocity();
554 Assert.assertEquals(0.0, pTested.subtract(pReference).getNorm().getReal(), threshold * pReference.getNorm().getReal());
555 Assert.assertEquals(0.0, vTested.subtract(vReference).getNorm().getReal(), threshold * vReference.getNorm().getReal());
556
557 }
558 }
559
560 private <T extends CalculusFieldElement<T> >void doTestErr1(Field<T> field) throws IllegalArgumentException {
561 T zero = field.getZero();
562 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
563 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-26655470.0), zero.add(29881667.0), zero.add(-113657.0));
564 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-1125.0), zero.add(-1122.0), zero.add(195.0));
565 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
566 double mu = 3.9860047e14;
567 new FieldCartesianOrbit<>(FieldPVCoordinates,
568 new Frame(FramesFactory.getEME2000(), Transform.IDENTITY, "non-inertial", false),
569 date, zero.add(mu));
570 }
571
572 private <T extends CalculusFieldElement<T>> void doTestToOrbitWithoutDerivatives(Field<T> field) {
573 T zero = field.getZero();
574 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
575
576 FieldVector3D<T> position = new FieldVector3D<>(zero.add(7.0e6), zero.add(1.0e6), zero.add(4.0e6));
577 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-500.0), zero.add(8000.0), zero.add(1000.0));
578 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity);
579 FieldCartesianOrbit<T> fieldOrbit = new FieldCartesianOrbit<>(pvCoordinates, FramesFactory.getEME2000(), date, zero.add(mu));
580 CartesianOrbit orbit = fieldOrbit.toOrbit();
581 Assert.assertFalse(orbit.hasDerivatives());
582 MatcherAssert.assertThat(orbit.getPVCoordinates().getPosition().getX(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getPosition().getX().getReal(), 0));
583 MatcherAssert.assertThat(orbit.getPVCoordinates().getPosition().getY(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getPosition().getY().getReal(), 0));
584 MatcherAssert.assertThat(orbit.getPVCoordinates().getPosition().getZ(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getPosition().getZ().getReal(), 0));
585 MatcherAssert.assertThat(orbit.getPVCoordinates().getVelocity().getX(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getVelocity().getX().getReal(), 0));
586 MatcherAssert.assertThat(orbit.getPVCoordinates().getVelocity().getY(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getVelocity().getY().getReal(), 0));
587 MatcherAssert.assertThat(orbit.getPVCoordinates().getVelocity().getZ(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getVelocity().getZ().getReal(), 0));
588 MatcherAssert.assertThat(orbit.getPVCoordinates().getAcceleration().getX(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getAcceleration().getX().getReal(), 0));
589 MatcherAssert.assertThat(orbit.getPVCoordinates().getAcceleration().getY(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getAcceleration().getY().getReal(), 0));
590 MatcherAssert.assertThat(orbit.getPVCoordinates().getAcceleration().getZ(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getAcceleration().getZ().getReal(), 0));
591
592 }
593
594 private <T extends CalculusFieldElement<T>> void doTestToOrbitWithDerivatives(Field<T> field) {
595 T zero = field.getZero();
596 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
597
598 FieldVector3D<T> position = new FieldVector3D<>(zero.add(7.0e6), zero.add(1.0e6), zero.add(4.0e6));
599 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-500.0), zero.add(8000.0), zero.add(1000.0));
600 T r2 = position.getNormSq();
601 T r = r2.sqrt();
602 FieldVector3D<T> acceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(-mu).add(0.1), position);
603 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity,acceleration);
604 FieldCartesianOrbit<T> fieldOrbit = new FieldCartesianOrbit<>(pvCoordinates, FramesFactory.getEME2000(), date, zero.add(mu));
605 CartesianOrbit orbit = fieldOrbit.toOrbit();
606 Assert.assertTrue(orbit.hasDerivatives());
607 MatcherAssert.assertThat(orbit.getPVCoordinates().getPosition().getX(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getPosition().getX().getReal(), 0));
608 MatcherAssert.assertThat(orbit.getPVCoordinates().getPosition().getY(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getPosition().getY().getReal(), 0));
609 MatcherAssert.assertThat(orbit.getPVCoordinates().getPosition().getZ(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getPosition().getZ().getReal(), 0));
610 MatcherAssert.assertThat(orbit.getPVCoordinates().getVelocity().getX(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getVelocity().getX().getReal(), 0));
611 MatcherAssert.assertThat(orbit.getPVCoordinates().getVelocity().getY(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getVelocity().getY().getReal(), 0));
612 MatcherAssert.assertThat(orbit.getPVCoordinates().getVelocity().getZ(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getVelocity().getZ().getReal(), 0));
613 MatcherAssert.assertThat(orbit.getPVCoordinates().getAcceleration().getX(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getAcceleration().getX().getReal(), 0));
614 MatcherAssert.assertThat(orbit.getPVCoordinates().getAcceleration().getY(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getAcceleration().getY().getReal(), 0));
615 MatcherAssert.assertThat(orbit.getPVCoordinates().getAcceleration().getZ(), relativelyCloseTo(fieldOrbit.getPVCoordinates().getAcceleration().getZ().getReal(), 0));
616 }
617
618 private <T extends CalculusFieldElement<T>> void doTestJacobianReference(Field<T> field) {
619 T zero = field.getZero();
620 FieldVector3D<T> position = new FieldVector3D<>(zero.add(-29536113.0), zero.add(30329259.0), zero.add(-100125.0));
621 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-2194.0), zero.add(-2141.0), zero.add(-8.0));
622 FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
623 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(),
624 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
625
626 T[][] jacobian = MathArrays.buildArray(field, 6, 6);
627 orbit.getJacobianWrtCartesian(PositionAngle.MEAN, jacobian);
628
629 for (int i = 0; i < jacobian.length; i++) {
630 T[] row = jacobian[i];
631 for (int j = 0; j < row.length; j++) {
632 Assert.assertEquals((i == j) ? 1 : 0, row[j].getReal(), 1.0e-15);
633 }
634 }
635
636 T[][] invJacobian = MathArrays.buildArray(field, 6, 6);
637 orbit.getJacobianWrtParameters(PositionAngle.MEAN, invJacobian);
638 MatrixUtils.createFieldMatrix(jacobian).
639 multiply(MatrixUtils.createFieldMatrix(invJacobian)).
640 walkInRowOrder(new FieldMatrixPreservingVisitor<T>() {
641 public void start(int rows, int columns,
642 int startRow, int endRow, int startColumn, int endColumn) {
643 }
644
645 public void visit(int row, int column, T value) {
646 Assert.assertEquals(row == column ? 1.0 : 0.0, value.getReal(), 1.0e-15);
647 }
648
649 public T end() {
650 return null;
651 }
652 });
653
654 }
655
656 private <T extends CalculusFieldElement<T>> void doTestInterpolation(Field<T> field, boolean useDerivatives,
657 double shiftPositionErrorWithin, double interpolationPositionErrorWithin,
658 double shiftVelocityErrorWithin, double interpolationVelocityErrorWithin,
659 double shiftPositionErrorFarPast, double interpolationPositionErrorFarPast,
660 double shiftVelocityErrorFarPast, double interpolationVelocityErrorFarPast)
661 {
662 T zero = field.getZero();
663 final T ehMu = zero.add(3.9860047e14);
664
665 final double ae = 6.378137e6;
666 final double c20 = -1.08263e-3;
667 final double c30 = 2.54e-6;
668 final double c40 = 1.62e-6;
669 final double c50 = 2.3e-7;
670 final double c60 = -5.5e-7;
671
672 final FieldAbsoluteDate<T> date = FieldAbsoluteDate.getJ2000Epoch(field).shiftedBy(584.);
673 final FieldVector3D<T> position = new FieldVector3D<>(zero.add(3220103.), zero.add(69623.), zero.add(6449822.));
674 final FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(6414.7), zero.add(-2006.), zero.add(-3180.));
675 final FieldCartesianOrbit<T> initialOrbit = new FieldCartesianOrbit<>(new FieldPVCoordinates<>(position, velocity),
676 FramesFactory.getEME2000(), date, ehMu);
677
678 FieldEcksteinHechlerPropagator<T> propagator =
679 new FieldEcksteinHechlerPropagator<>(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);
680
681
682 List<FieldOrbit<T>> sample = new ArrayList<FieldOrbit<T>>();
683 for (T dt = zero; dt.getReal() < 251.0; dt = dt.add(60.0)) {
684 FieldOrbit<T> orbit = propagator.propagate(date.shiftedBy(dt)).getOrbit();
685 if (!useDerivatives) {
686
687 T[] stateVector = MathArrays.buildArray(field, 6);
688 orbit.getType().mapOrbitToArray(orbit, PositionAngle.TRUE, stateVector, null);
689 orbit = orbit.getType().mapArrayToOrbit(stateVector, null, PositionAngle.TRUE,
690 orbit.getDate(), orbit.getMu(), orbit.getFrame());
691 }
692 sample.add(orbit);
693 }
694
695
696
697
698
699 double maxShiftPError = 0;
700 double maxInterpolationPError = 0;
701 double maxShiftVError = 0;
702 double maxInterpolationVError = 0;
703 for (T dt = zero; dt.getReal() < 240.0; dt = dt.add(1.0)) {
704 FieldAbsoluteDate<T> t = initialOrbit.getDate().shiftedBy(dt);
705 FieldPVCoordinates<T> propagated = propagator.propagate(t).getPVCoordinates();
706 FieldPVCoordinates<T> shiftError = new FieldPVCoordinates<>(propagated,
707 initialOrbit.shiftedBy(dt.getReal()).getPVCoordinates());
708 FieldPVCoordinates<T> interpolationError = new FieldPVCoordinates<>(propagated,
709 initialOrbit.interpolate(t, sample).getPVCoordinates());
710 maxShiftPError = FastMath.max(maxShiftPError,
711 shiftError.getPosition().getNorm().getReal());
712 maxInterpolationPError = FastMath.max(maxInterpolationPError,
713 interpolationError.getPosition().getNorm().getReal());
714 maxShiftVError = FastMath.max(maxShiftVError,
715 shiftError.getVelocity().getNorm().getReal());
716 maxInterpolationVError = FastMath.max(maxInterpolationVError,
717 interpolationError.getVelocity().getNorm().getReal());
718 }
719 Assert.assertEquals(shiftPositionErrorWithin, maxShiftPError, 0.01 * shiftPositionErrorWithin);
720 Assert.assertEquals(interpolationPositionErrorWithin, maxInterpolationPError, 0.01 * interpolationPositionErrorWithin);
721 Assert.assertEquals(shiftVelocityErrorWithin, maxShiftVError, 0.01 * shiftVelocityErrorWithin);
722 Assert.assertEquals(interpolationVelocityErrorWithin, maxInterpolationVError, 0.01 * interpolationVelocityErrorWithin);
723
724
725 maxShiftPError = 0;
726 maxInterpolationPError = 0;
727 maxShiftVError = 0;
728 maxInterpolationVError = 0;
729 for (T dt = zero.add(500.0); dt.getReal() < 650.0; dt = dt.add(1.0)) {
730 FieldAbsoluteDate<T> t = initialOrbit.getDate().shiftedBy(dt);
731 FieldPVCoordinates<T> propagated = propagator.propagate(t).getPVCoordinates();
732 FieldPVCoordinates<T> shiftError = new FieldPVCoordinates<>(propagated,
733 initialOrbit.shiftedBy(dt).getPVCoordinates());
734 FieldPVCoordinates<T> interpolationError = new FieldPVCoordinates<>(propagated,
735 initialOrbit.interpolate(t, sample).getPVCoordinates());
736 maxShiftPError = FastMath.max(maxShiftPError,
737 shiftError.getPosition().getNorm().getReal());
738 maxInterpolationPError = FastMath.max(maxInterpolationPError,
739 interpolationError.getPosition().getNorm().getReal());
740 maxShiftVError = FastMath.max(maxShiftVError,
741 shiftError.getVelocity().getNorm().getReal());
742 maxInterpolationVError = FastMath.max(maxInterpolationVError,
743 interpolationError.getVelocity().getNorm().getReal());
744 }
745 Assert.assertEquals(shiftPositionErrorFarPast, maxShiftPError, 0.01 * shiftPositionErrorFarPast);
746 Assert.assertEquals(interpolationPositionErrorFarPast, maxInterpolationPError, 0.01 * interpolationPositionErrorFarPast);
747 Assert.assertEquals(shiftVelocityErrorFarPast, maxShiftVError, 0.01 * shiftVelocityErrorFarPast);
748 Assert.assertEquals(interpolationVelocityErrorFarPast, maxInterpolationVError, 0.01 * interpolationVelocityErrorFarPast);
749
750 }
751
752 private <T extends CalculusFieldElement<T>> void doTestNonKeplerianDerivatives(Field<T> field) {
753 final T zero = field.getZero();
754 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
755 final FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(6896874.444705), field.getZero().add(1956581.072644), field.getZero().add(-147476.245054));
756 final FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(166.816407662), field.getZero().add(-1106.783301861), field.getZero().add(-7372.745712770));
757 final FieldVector3D <T> acceleration = new FieldVector3D<>(field.getZero().add(-7.466182457944), field.getZero().add(-2.118153357345), field.getZero().add(0.160004048437));
758 final TimeStampedFieldPVCoordinates<T> pv = new TimeStampedFieldPVCoordinates<>(date, position, velocity, acceleration);
759 final Frame frame = FramesFactory.getEME2000();
760 final T mu = zero.add(Constants.EIGEN5C_EARTH_MU);
761 final FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(pv, frame, zero.add(mu));
762
763 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getA()),
764 orbit.getADot().getReal(),
765 4.3e-8);
766 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEx()),
767 orbit.getEquinoctialExDot().getReal(),
768 2.1e-15);
769 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEy()),
770 orbit.getEquinoctialEyDot().getReal(),
771 5.3e-16);
772 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHx()),
773 orbit.getHxDot().getReal(),
774 4.4e-15);
775 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHy()),
776 orbit.getHyDot().getReal(),
777 8.0e-16);
778 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLv()),
779 orbit.getLvDot().getReal(),
780 1.2e-15);
781 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLE()),
782 orbit.getLEDot().getReal(),
783 7.8e-16);
784 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLM()),
785 orbit.getLMDot().getReal(),
786 8.8e-16);
787 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getE()),
788 orbit.getEDot().getReal(),
789 7.0e-16);
790 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getI()),
791 orbit.getIDot().getReal(),
792 5.7e-16);
793
794 }
795
796 private <T extends CalculusFieldElement<T>, S extends Function<FieldCartesianOrbit<T>, T>>
797 double differentiate(TimeStampedFieldPVCoordinates<T> pv, Frame frame, T mu, S picker) {
798 final DSFactory factory = new DSFactory(1, 1);
799 FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
800 UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {
801 public double value(double dt) {
802 return picker.apply(new FieldCartesianOrbit<>(pv.shiftedBy(dt), frame, mu)).getReal();
803 }
804 });
805 return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
806 }
807
808 private <T extends CalculusFieldElement<T>> void doTestEquatorialRetrograde(Field<T> field) {
809 final T zero = field.getZero();
810 FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(10000000.0), field.getZero(), field.getZero());
811 FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero(), field.getZero().add(-6500.0), field.getZero());
812 T r2 = position.getNormSq();
813 T r = r2.sqrt();
814 FieldVector3D<T> acceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(zero.add(mu).negate()), position,
815 field.getOne(), new FieldVector3D<>(field.getZero().add(-0.1),
816 field.getZero().add(0.2),
817 field.getZero().add(0.3)));
818 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity, acceleration);
819 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(pvCoordinates, FramesFactory.getEME2000(),
820 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
821 Assert.assertEquals(10637829.465, orbit.getA().getReal(), 1.0e-3);
822 Assert.assertEquals(-738.145, orbit.getADot().getReal(), 1.0e-3);
823 Assert.assertEquals(0.05995861, orbit.getE().getReal(), 1.0e-8);
824 Assert.assertEquals(-6.523e-5, orbit.getEDot().getReal(), 1.0e-8);
825 Assert.assertEquals(FastMath.PI, orbit.getI().getReal(), 1.0e-15);
826 Assert.assertTrue(Double.isNaN(orbit.getIDot().getReal()));
827 Assert.assertTrue(Double.isNaN(orbit.getHx().getReal()));
828 Assert.assertTrue(Double.isNaN(orbit.getHxDot().getReal()));
829 Assert.assertTrue(Double.isNaN(orbit.getHy().getReal()));
830 Assert.assertTrue(Double.isNaN(orbit.getHyDot().getReal()));
831 }
832
833 private <T extends CalculusFieldElement<T>> void doTestToString(Field<T> field) {
834 final T zero = field.getZero();
835 FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(-29536113.0),
836 field.getZero().add(30329259.0),
837 field.getZero().add(-100125.0));
838 FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(-2194.0),
839 field.getZero().add(-2141.0),
840 field.getZero().add(-8.0));
841 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity);
842 FieldCartesianOrbit<T> orbit = new FieldCartesianOrbit<>(pvCoordinates, FramesFactory.getEME2000(),
843 FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
844 Assert.assertEquals("Cartesian parameters: {P(-2.9536113E7, 3.0329259E7, -100125.0), V(-2194.0, -2141.0, -8.0)}",
845 orbit.toString());
846 }
847
848 private <T extends CalculusFieldElement<T>> void doTestCopyNonKeplerianAcceleration(Field<T> field) {
849
850 final T zero = field.getZero();
851 final Frame eme2000 = FramesFactory.getEME2000();
852
853
854 final FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(42164140),
855 field.getZero(),
856 field.getZero());
857
858 final FieldPVCoordinates<T> pv =
859 new FieldPVCoordinates<>(position,
860 new FieldVector3D<>(field.getZero(),
861 position.getNorm().reciprocal().multiply(mu).sqrt(),
862 field.getZero()));
863
864 final FieldOrbit<T> orbit = new FieldCartesianOrbit<>(pv, eme2000, FieldAbsoluteDate.getJ2000Epoch(field), zero.add(mu));
865
866
867 final FieldOrbit<T> orbitCopy = new FieldCartesianOrbit<>(orbit);
868
869
870 final FieldOrbit<T> shiftedOrbit = orbit.shiftedBy(10);
871 final FieldOrbit<T> shiftedOrbitCopy = orbitCopy.shiftedBy(10);
872
873 Assert.assertEquals(0.0,
874 FieldVector3D.distance(shiftedOrbit.getPVCoordinates().getPosition(),
875 shiftedOrbitCopy.getPVCoordinates().getPosition()).getReal(),
876 1.0e-10);
877 Assert.assertEquals(0.0,
878 FieldVector3D.distance(shiftedOrbit.getPVCoordinates().getVelocity(),
879 shiftedOrbitCopy.getPVCoordinates().getVelocity()).getReal(),
880 1.0e-10);
881
882 }
883
884 private <T extends CalculusFieldElement<T>> void doTestNormalize(Field<T> field) {
885 final T zero = field.getZero();
886 final FieldVector3D<T> position = new FieldVector3D<>(zero.newInstance(42164140.0), zero, zero);
887 final FieldPVCoordinates<T> pv = new FieldPVCoordinates<>(position,
888 new FieldVector3D<>(zero,
889 FastMath.sqrt(position.getNorm().reciprocal().multiply(mu)),
890 zero));
891 final FieldOrbit<T> orbit = new FieldCartesianOrbit<>(pv,
892 FramesFactory.getEME2000(),
893 FieldAbsoluteDate.getJ2000Epoch(field),
894 field.getZero().newInstance(mu));
895 Assert.assertSame(orbit, orbit.getType().normalize(orbit, null));
896 }
897
898 }
899