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