1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import org.hamcrest.MatcherAssert;
20 import org.hipparchus.CalculusFieldElement;
21 import org.hipparchus.Field;
22 import org.hipparchus.analysis.UnivariateFunction;
23 import org.hipparchus.analysis.differentiation.DSFactory;
24 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
25 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
26 import org.hipparchus.complex.Complex;
27 import org.hipparchus.complex.ComplexField;
28 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
29 import org.hipparchus.geometry.euclidean.threed.Vector3D;
30 import org.hipparchus.linear.FieldMatrixPreservingVisitor;
31 import org.hipparchus.linear.MatrixUtils;
32 import org.hipparchus.util.Binary64;
33 import org.hipparchus.util.Binary64Field;
34 import org.hipparchus.util.FastMath;
35 import org.hipparchus.util.MathArrays;
36 import org.hipparchus.util.MathUtils;
37 import org.junit.jupiter.api.Assertions;
38 import org.junit.jupiter.api.BeforeEach;
39 import org.junit.jupiter.api.Test;
40 import org.junit.jupiter.params.ParameterizedTest;
41 import org.junit.jupiter.params.provider.EnumSource;
42 import org.orekit.Utils;
43 import org.orekit.errors.OrekitException;
44 import org.orekit.errors.OrekitIllegalArgumentException;
45 import org.orekit.errors.OrekitMessages;
46 import org.orekit.frames.Frame;
47 import org.orekit.frames.FramesFactory;
48 import org.orekit.frames.Transform;
49 import org.orekit.time.AbsoluteDate;
50 import org.orekit.time.FieldAbsoluteDate;
51 import org.orekit.time.TimeScalesFactory;
52 import org.orekit.utils.Constants;
53 import org.orekit.utils.FieldPVCoordinates;
54 import org.orekit.utils.PVCoordinates;
55 import org.orekit.utils.TimeStampedFieldPVCoordinates;
56
57 import java.util.function.Function;
58
59 import static org.orekit.OrekitMatchers.relativelyCloseTo;
60
61 public class FieldKeplerianOrbitTest {
62
63
64 public double mu;
65
66 @BeforeEach
67 public void setUp() {
68
69 Utils.setDataRoot("regular-data");
70
71
72 mu = 3.9860047e14;
73
74 }
75
76 @ParameterizedTest
77 @EnumSource(PositionAngleType.class)
78 void testWithCachedPositionAngleType(final PositionAngleType positionAngleType) {
79
80 final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
81 final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
82 final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
83 final double muEarth = 3.9860047e14;
84 final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), AbsoluteDate.ARBITRARY_EPOCH, muEarth);
85 final KeplerianOrbit keplerianOrbit = new KeplerianOrbit(cartesianOrbit);
86 final Binary64Field field = Binary64Field.getInstance();
87 final FieldKeplerianOrbit<Binary64> fieldOrbit = new FieldKeplerianOrbit<>(field, keplerianOrbit);
88
89 final FieldKeplerianOrbit<Binary64> orbit = fieldOrbit.withCachedPositionAngleType(positionAngleType);
90
91 Assertions.assertEquals(fieldOrbit.getFrame(), orbit.getFrame());
92 Assertions.assertEquals(fieldOrbit.getDate(), orbit.getDate());
93 Assertions.assertEquals(fieldOrbit.getMu(), orbit.getMu());
94 final Vector3D relativePosition = fieldOrbit.getPosition(orbit.getFrame()).subtract(
95 orbit.getPosition()).toVector3D();
96 Assertions.assertEquals(0., relativePosition.getNorm(), 1e-6);
97 Assertions.assertEquals(fieldOrbit.hasNonKeplerianAcceleration(),
98 orbit.hasNonKeplerianAcceleration());
99 }
100
101 @Test
102 void testNonKeplerianAcceleration() {
103
104 final PVCoordinates pvCoordinates = new PVCoordinates(new Vector3D(1, 2, 3),
105 Vector3D.MINUS_K.scalarMultiply(0.1), Vector3D.MINUS_I);
106 final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(),
107 AbsoluteDate.ARBITRARY_EPOCH, 1.);
108 final KeplerianOrbit keplerianOrbit = (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(cartesianOrbit);
109 final Binary64Field field = Binary64Field.getInstance();
110 final FieldCartesianOrbit<Binary64> fieldCartesianOrbit = new FieldCartesianOrbit<>(field, cartesianOrbit);
111 final FieldKeplerianOrbit<Binary64> fieldKeplerianOrbit = new FieldKeplerianOrbit<>(field, keplerianOrbit);
112
113 final FieldVector3D<Binary64> nonKeplerianAcceleration = fieldKeplerianOrbit.nonKeplerianAcceleration();
114
115 final FieldVector3D<Binary64> difference = nonKeplerianAcceleration.subtract(fieldCartesianOrbit.nonKeplerianAcceleration());
116 Assertions.assertEquals(0., difference.getNorm().getReal(), 1e-10);
117 }
118
119 @Test
120 void testInFrameNonKeplerian() {
121 testTemplateInFrame(Vector3D.MINUS_J, PositionAngleType.TRUE);
122 }
123
124 @ParameterizedTest
125 @EnumSource(PositionAngleType.class)
126 void testInFrameKeplerian(final PositionAngleType positionAngleType) {
127 testTemplateInFrame(Vector3D.ZERO, positionAngleType);
128 }
129
130 private void testTemplateInFrame(final Vector3D acceleration, final PositionAngleType positionAngleType) {
131
132 final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
133 final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
134 final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
135 final double muEarth = 3.9860047e14;
136 final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(),
137 AbsoluteDate.ARBITRARY_EPOCH, muEarth);
138 final KeplerianOrbit keplerianOrbit = new KeplerianOrbit(cartesianOrbit);
139 final FieldKeplerianOrbit<Binary64> fieldOrbit = new FieldKeplerianOrbit<>(Binary64Field.getInstance(),
140 keplerianOrbit).withCachedPositionAngleType(positionAngleType);
141
142 final FieldKeplerianOrbit<Binary64> fieldOrbitWithOtherFrame = fieldOrbit.inFrame(FramesFactory.getGCRF());
143
144 Assertions.assertNotEquals(fieldOrbit.getFrame(), fieldOrbitWithOtherFrame.getFrame());
145 Assertions.assertEquals(fieldOrbit.getDate(), fieldOrbitWithOtherFrame.getDate());
146 Assertions.assertEquals(fieldOrbit.getMu(), fieldOrbitWithOtherFrame.getMu());
147 final FieldVector3D<Binary64> relativePosition = fieldOrbit.getPosition(fieldOrbitWithOtherFrame.getFrame()).subtract(
148 fieldOrbitWithOtherFrame.getPosition());
149 Assertions.assertEquals(0., relativePosition.getNorm().getReal(), 1e-6);
150 Assertions.assertEquals(fieldOrbit.hasNonKeplerianAcceleration(),
151 fieldOrbitWithOtherFrame.hasNonKeplerianAcceleration());
152 }
153
154 @Test
155 void testKepToKep() {
156 doTestKeplerianToKeplerian(Binary64Field.getInstance());
157 }
158
159 @Test
160 void testKepToCart() {
161 doTestKeplerianToCartesian(Binary64Field.getInstance());
162 }
163
164 @Test
165 void testKepToEquin() {
166 doTestKeplerianToEquinoctial(Binary64Field.getInstance());
167 }
168
169 @Test
170 void testAnomaly() {
171 doTestAnomaly(Binary64Field.getInstance());
172 }
173
174 @Test
175 void testPositionVelocityNorms() {
176 doTestPositionVelocityNorms(Binary64Field.getInstance());
177 }
178
179 @Test
180 void testGeometry() {
181 doTestGeometry(Binary64Field.getInstance());
182 }
183
184 @Test
185 void testSymmetry() {
186 doTestSymmetry(Binary64Field.getInstance());
187 }
188
189 @Test
190 void testNonInertialFrame() {
191 Assertions.assertThrows(IllegalArgumentException.class, () -> {
192 doTestNonInertialFrame(Binary64Field.getInstance());
193 });
194 }
195
196 @Test
197 void testPeriod() {
198 doTestPeriod(Binary64Field.getInstance());
199 }
200
201 @Test
202 void testHyperbola1() {
203 doTestHyperbola1(Binary64Field.getInstance());
204 }
205
206 @Test
207 void testHyperbola2() {
208 doTestHyperbola2(Binary64Field.getInstance());
209 }
210
211 @Test
212 void testToOrbitWithoutDerivatives() {
213 doTestToOrbitWithoutDerivatives(Binary64Field.getInstance());
214 }
215
216 @Test
217 void testToOrbitWithDerivatives() {
218 doTestToOrbitWithDerivatives(Binary64Field.getInstance());
219 }
220
221 @Test
222 void testDerivativesConversionSymmetry() {
223 doTestDerivativesConversionSymmetry(Binary64Field.getInstance());
224 }
225
226 @Test
227 void testDerivativesConversionSymmetryHyperbolic() {
228 doTestDerivativesConversionSymmetryHyperbolic(Binary64Field.getInstance());
229 }
230
231 @Test
232 void testToString() {
233 doTestToString(Binary64Field.getInstance());
234 }
235
236 @Test
237 void testInconsistentHyperbola() {
238 doTestInconsistentHyperbola(Binary64Field.getInstance());
239 }
240
241 @Test
242 void testVeryLargeEccentricity() {
243 doTestVeryLargeEccentricity(Binary64Field.getInstance());
244 }
245
246 @Test
247 void testKeplerEquation() {
248 doTestKeplerEquation(Binary64Field.getInstance());
249 }
250
251 @Test
252 void testNumericalIssue() {
253 doTestNumericalIssue25(Binary64Field.getInstance());
254 }
255
256 @Test
257 void testPerfectlyEquatorial() {
258 doTestPerfectlyEquatorial(Binary64Field.getInstance());
259 }
260
261 @Test
262 void testJacobianReferenceEllipse() {
263 doTestJacobianReferenceEllipse(Binary64Field.getInstance());
264 }
265
266 @Test
267 void testJacobianFinitedDiff() {
268 doTestJacobianFinitedifferencesEllipse(Binary64Field.getInstance());
269 }
270
271 @Test
272 void testJacobianReferenceHyperbola() {
273 doTestJacobianReferenceHyperbola(Binary64Field.getInstance());
274 }
275
276 @Test
277 void testJacobianFinitDiffHyperbola() {
278 doTestJacobianFinitedifferencesHyperbola(Binary64Field.getInstance());
279 }
280
281 @Test
282 void testKeplerianDerivatives() {
283 doTestKeplerianDerivatives(Binary64Field.getInstance());
284 }
285
286 @Test
287 void testNonKeplerianEllipticDerivatives() {
288 doTestNonKeplerianEllipticDerivatives(Binary64Field.getInstance());
289 }
290
291 @Test
292 void testNonKeplerianHyperbolicDerivatives() {
293 doTestNonKeplerianHyperbolicDerivatives(Binary64Field.getInstance());
294 }
295
296 @Test
297 void testPositionAngleDerivatives() {
298 doTestPositionAngleDerivatives(Binary64Field.getInstance());
299 }
300
301 @Test
302 void testPositionAngleHyperbolicDerivatives() {
303 doTestPositionAngleHyperbolicDerivatives(Binary64Field.getInstance());
304 }
305
306 @Test
307 void testEquatorialRetrograde() {
308 doTestEquatorialRetrograde(Binary64Field.getInstance());
309 }
310
311 @Test
312 void testOutOfRangeV() {
313 Assertions.assertThrows(IllegalArgumentException.class, () -> {
314 doTestOutOfRangeV(Binary64Field.getInstance());
315 });
316 }
317
318 @Test
319 void testPerfectlyEquatorialConversion() {
320 doTestPerfectlyEquatorialConversion(Binary64Field.getInstance());
321 }
322
323 @Test
324 void testCopyNonKeplerianAcceleration() {
325 doTestCopyNonKeplerianAcceleration(Binary64Field.getInstance());
326 }
327
328 @Test
329 void testIssue674() {
330 doTestIssue674(Binary64Field.getInstance());
331 }
332
333 @Test
334 void testNormalize() {
335 doTestNormalize(Binary64Field.getInstance());
336 }
337
338 @Test
339 void testWithKeplerianRates() {
340
341 final ComplexField field = ComplexField.getInstance();
342 final KeplerianOrbit expectedOrbit = createOrbitForTestFromKeplerianOrbit(true);
343 final FieldKeplerianOrbit<Complex> fieldOrbit = new FieldKeplerianOrbit<>(field, expectedOrbit);
344
345 final FieldKeplerianOrbit<Complex> actualFieldOrbit = fieldOrbit.withKeplerianRates();
346
347 Assertions.assertFalse(actualFieldOrbit.hasNonKeplerianRates());
348 Assertions.assertEquals(fieldOrbit.getMu(), actualFieldOrbit.getMu());
349 Assertions.assertEquals(fieldOrbit.getDate(), actualFieldOrbit.getDate());
350 Assertions.assertEquals(fieldOrbit.getFrame(), actualFieldOrbit.getFrame());
351 Assertions.assertEquals(fieldOrbit.getPosition(), actualFieldOrbit.getPosition());
352 }
353
354 @Test
355 void testFromKeplerianOrbitWithDerivatives() {
356
357 final ComplexField field = ComplexField.getInstance();
358 final KeplerianOrbit expectedOrbit = createOrbitForTestFromKeplerianOrbit(true);
359
360 final FieldKeplerianOrbit<Complex> fieldOrbit = new FieldKeplerianOrbit<>(field, expectedOrbit);
361
362 compareFieldOrbitToOrbit(fieldOrbit, expectedOrbit);
363 }
364
365 @Test
366 void testFromKeplerianOrbitWithoutDerivatives() {
367
368 final ComplexField field = ComplexField.getInstance();
369 final KeplerianOrbit expectedOrbit = createOrbitForTestFromKeplerianOrbit(false);
370
371 final FieldKeplerianOrbit<Complex> fieldOrbit = new FieldKeplerianOrbit<>(field, expectedOrbit);
372
373 compareFieldOrbitToOrbit(fieldOrbit, expectedOrbit);
374 }
375
376 private KeplerianOrbit createOrbitForTestFromKeplerianOrbit(final boolean withDerivatives) {
377 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
378 final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
379 final Frame frame = FramesFactory.getEME2000();
380 final double a = 10000.e3;
381 final double e = 1e-4;
382 final double i = 1;
383 final double raan = 3.;
384 final double aop = -2;
385 final double f = 0.5;
386 if (withDerivatives) {
387 final double derivative = 0.;
388 return new KeplerianOrbit(a, e, i, aop, raan, f, derivative, derivative, derivative, derivative, derivative,
389 derivative, positionAngleType, frame, date, mu);
390 } else {
391 return new KeplerianOrbit(a, e, i, aop, raan, f, positionAngleType, frame, date, mu);
392 }
393 }
394
395 private <T extends CalculusFieldElement<T>> void compareFieldOrbitToOrbit(final FieldKeplerianOrbit<T> fieldOrbit,
396 final KeplerianOrbit orbit) {
397 Assertions.assertEquals(orbit.getFrame(), fieldOrbit.getFrame());
398 Assertions.assertEquals(orbit.getMu(), fieldOrbit.getMu().getReal());
399 Assertions.assertEquals(orbit.getDate(), fieldOrbit.getDate().toAbsoluteDate());
400 Assertions.assertEquals(orbit.getA(), fieldOrbit.getA().getReal(), 1e-6);
401 Assertions.assertEquals(orbit.getE(), fieldOrbit.getE().getReal(), 1e-10);
402 final double toleranceAngle = 1e-10;
403 Assertions.assertEquals(orbit.getI(), fieldOrbit.getI().getReal(), toleranceAngle);
404 Assertions.assertEquals(orbit.getRightAscensionOfAscendingNode(),
405 fieldOrbit.getRightAscensionOfAscendingNode().getReal(), toleranceAngle);
406 Assertions.assertEquals(orbit.getPerigeeArgument(), fieldOrbit.getPerigeeArgument().getReal(), toleranceAngle);
407 Assertions.assertEquals(orbit.getTrueAnomaly(), fieldOrbit.getTrueAnomaly().getReal(), toleranceAngle);
408 Assertions.assertEquals(orbit.hasNonKeplerianAcceleration(), fieldOrbit.hasNonKeplerianAcceleration());
409 Assertions.assertEquals(orbit.getADot(), fieldOrbit.getADot().getReal());
410 Assertions.assertEquals(orbit.getEDot(), fieldOrbit.getEDot().getReal());
411 Assertions.assertEquals(orbit.getIDot(), fieldOrbit.getIDot().getReal());
412 Assertions.assertEquals(orbit.getRightAscensionOfAscendingNodeDot(),
413 fieldOrbit.getRightAscensionOfAscendingNodeDot().getReal());
414 Assertions.assertEquals(orbit.getPerigeeArgumentDot(), fieldOrbit.getPerigeeArgumentDot().getReal());
415 Assertions.assertEquals(orbit.getTrueAnomalyDot(), fieldOrbit.getTrueAnomalyDot().getReal());
416 }
417
418 @Test
419 void testCoverageCachedPositionAngleTypeWithRates() {
420
421 final double semiMajorAxis = 1e4;
422 final double eccentricity = 0.;
423 final double expectedAnomaly = 0.;
424 final double expectedAnomalyDot = 0.;
425 final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
426 final Binary64Field field = Binary64Field.getInstance();
427 final Binary64 zero = field.getZero();
428
429 for (final PositionAngleType inputPositionAngleType: PositionAngleType.values()) {
430 for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
431 final FieldKeplerianOrbit<Binary64> fieldOrbit = new FieldKeplerianOrbit<>(
432 zero.newInstance(semiMajorAxis), zero.newInstance(eccentricity), zero, zero, zero,
433 zero.newInstance(expectedAnomaly), zero, zero, zero, zero, zero,
434 zero.newInstance(expectedAnomalyDot), inputPositionAngleType, cachedPositionAngleType,
435 FramesFactory.getGCRF(), new FieldAbsoluteDate<>(field, date), zero.newInstance(mu));
436 Assertions.assertEquals(cachedPositionAngleType, fieldOrbit.getCachedPositionAngleType());
437 Assertions.assertEquals(expectedAnomaly, fieldOrbit.getTrueAnomaly().getReal());
438 Assertions.assertEquals(expectedAnomaly, fieldOrbit.getEccentricAnomaly().getReal());
439 Assertions.assertEquals(expectedAnomaly, fieldOrbit.getMeanAnomaly().getReal());
440 Assertions.assertEquals(expectedAnomalyDot, fieldOrbit.getTrueAnomalyDot().getReal());
441 Assertions.assertEquals(expectedAnomalyDot, fieldOrbit.getEccentricAnomalyDot().getReal());
442 Assertions.assertEquals(expectedAnomalyDot, fieldOrbit.getMeanAnomalyDot().getReal());
443 }
444 }
445 }
446
447 @Test
448 void testGetAnomalyVersusDouble() {
449
450 final double semiMajorAxis = 1e6;
451 final double eccentricity = 1e-2;
452 final double expectedAnomaly = 3.;
453
454 compareAnomalies(semiMajorAxis, eccentricity, expectedAnomaly);
455 }
456
457 @Test
458 void testGetAnomalyVersusDoubleHyperbolic() {
459
460 final double semiMajorAxis = -1e6;
461 final double eccentricity = 1.5;
462 final double expectedAnomaly = 1.;
463
464 compareAnomalies(semiMajorAxis, eccentricity, expectedAnomaly);
465 }
466
467 private void compareAnomalies(final double semiMajorAxis, final double eccentricity, final double anomaly) {
468 final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
469 final Binary64Field field = Binary64Field.getInstance();
470 final Binary64 zero = field.getZero();
471 final double tolerance = 1e-10;
472 for (final PositionAngleType inputPositionAngleType: PositionAngleType.values()) {
473 for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
474 final FieldKeplerianOrbit<Binary64> fieldOrbit = new FieldKeplerianOrbit<>(
475 zero.newInstance(semiMajorAxis), zero.newInstance(eccentricity), zero, zero, zero,
476 zero.newInstance(anomaly), inputPositionAngleType, cachedPositionAngleType,
477 FramesFactory.getGCRF(), new FieldAbsoluteDate<>(field, date), zero.newInstance(mu));
478 final KeplerianOrbit keplerianOrbit = fieldOrbit.toOrbit();
479 Assertions.assertEquals(keplerianOrbit.getTrueAnomaly(), fieldOrbit.getTrueAnomaly().getReal(),
480 tolerance);
481 Assertions.assertEquals(keplerianOrbit.getEccentricAnomaly(), fieldOrbit.getEccentricAnomaly().getReal(),
482 tolerance);
483 Assertions.assertEquals(keplerianOrbit.getMeanAnomaly(), fieldOrbit.getMeanAnomaly().getReal(),
484 tolerance);
485 }
486 }
487 }
488
489 @Test
490 void testGetAnomalyDotVersusDoubleHyperbolic() {
491 final double semiMajorAxis = -1e6;
492 final double eccentricity = 1.5;
493 final double anomaly = 1.;
494 final double anomalyDot = 0.1;
495 final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
496 final Binary64Field field = Binary64Field.getInstance();
497 final Binary64 zero = field.getZero();
498 final double tolerance = 1e-10;
499 for (final PositionAngleType inputPositionAngleType: PositionAngleType.values()) {
500 for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
501 final FieldKeplerianOrbit<Binary64> fieldOrbit = new FieldKeplerianOrbit<>(
502 zero.newInstance(semiMajorAxis), zero.newInstance(eccentricity), zero, zero, zero,
503 zero.newInstance(anomaly), zero, zero, zero, zero, zero, zero.newInstance(anomalyDot),
504 inputPositionAngleType, cachedPositionAngleType,
505 FramesFactory.getGCRF(), new FieldAbsoluteDate<>(field, date), zero.newInstance(mu));
506 final KeplerianOrbit keplerianOrbit = fieldOrbit.toOrbit();
507 Assertions.assertEquals(keplerianOrbit.getTrueAnomaly(), fieldOrbit.getTrueAnomaly().getReal(),
508 tolerance);
509 Assertions.assertEquals(keplerianOrbit.getEccentricAnomaly(), fieldOrbit.getEccentricAnomaly().getReal(),
510 tolerance);
511 Assertions.assertEquals(keplerianOrbit.getMeanAnomaly(), fieldOrbit.getMeanAnomaly().getReal(),
512 tolerance);
513 }
514 }
515 }
516
517 private <T extends CalculusFieldElement<T>> void doTestKeplerianToKeplerian(final Field<T> field) {
518
519 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
520 T a= field.getZero().add(24464560.0);
521
522 T e= field.getZero().add(0.7311);
523 T i= field.getZero().add(0.122138);
524 T pa= field.getZero().add(3.10686);
525 T raan= field.getZero().add(1.00681);
526 T anomaly= field.getZero().add(0.048363);
527
528
529
530 FieldKeplerianOrbit<T> kep =
531 new FieldKeplerianOrbit<>(a, e, i, pa, raan, anomaly, PositionAngleType.MEAN,
532 FramesFactory.getEME2000(), date, field.getZero().add(mu));
533
534 FieldVector3D<T> pos = kep.getPosition();
535
536 FieldVector3D<T> vit = kep.getVelocity();
537
538 FieldKeplerianOrbit<T> param = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(pos, vit),
539 FramesFactory.getEME2000(), date, field.getZero().add(mu));
540
541 Assertions.assertEquals(param.getA().getReal(), kep.getA().getReal(), kep.getA().abs().multiply(Utils.epsilonTest).getReal());
542 Assertions.assertEquals(param.getE().getReal(), kep.getE().getReal(), kep.getE().abs().multiply(Utils.epsilonTest).getReal());
543 Assertions.assertEquals(MathUtils.normalizeAngle(param.getI(), kep.getI()).getReal(), kep.getI().getReal(), kep.getI().abs().multiply(Utils.epsilonTest).getReal());
544 Assertions.assertEquals(MathUtils.normalizeAngle(param.getPerigeeArgument(), kep.getPerigeeArgument()).getReal(), kep.getPerigeeArgument().getReal(), kep.getPerigeeArgument().abs().multiply(Utils.epsilonTest).getReal());
545 Assertions.assertEquals(MathUtils.normalizeAngle(param.getRightAscensionOfAscendingNode(), kep.getRightAscensionOfAscendingNode()).getReal(), kep.getRightAscensionOfAscendingNode().getReal(), kep.getRightAscensionOfAscendingNode().abs().multiply(Utils.epsilonTest).getReal());
546 Assertions.assertEquals(MathUtils.normalizeAngle(param.getMeanAnomaly(), kep.getMeanAnomaly()).getReal(), kep.getMeanAnomaly().getReal(), kep.getMeanAnomaly().abs().multiply(Utils.epsilonTest).getReal());
547
548
549
550
551 T aC= field.getZero().add(24464560.0);
552
553 T eC= field.getZero().add(0.0);
554 T iC= field.getZero().add(0.122138);
555 T paC= field.getZero().add(3.10686);
556 T raanC= field.getZero().add(1.00681);
557 T anomalyC= field.getZero().add(0.048363);
558
559
560 FieldKeplerianOrbit<T> kepCir =
561 new FieldKeplerianOrbit<>(aC, eC, iC, paC, raanC, anomalyC, PositionAngleType.MEAN,
562 FramesFactory.getEME2000(), date, field.getZero().add(mu));
563
564 FieldVector3D<T> posCir = kepCir.getPosition();
565 FieldVector3D<T> vitCir = kepCir.getVelocity();
566
567 FieldKeplerianOrbit<T> paramCir = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(posCir, vitCir),
568 FramesFactory.getEME2000(), date, field.getZero().add(mu));
569 Assertions.assertEquals(paramCir.getA().getReal(), kepCir.getA().getReal(), kep.getA().abs().multiply(Utils.epsilonTest).getReal());
570 Assertions.assertEquals(paramCir.getE().getReal(), kepCir.getE().getReal(), kep.getE().abs().multiply(Utils.epsilonTest).getReal());
571 Assertions.assertEquals(MathUtils.normalizeAngle(paramCir.getI(), kepCir.getI()).getReal(), kepCir.getI().getReal(), kep.getI().abs().multiply(Utils.epsilonTest).getReal());
572 Assertions.assertEquals(MathUtils.normalizeAngle(paramCir.getLM(), kepCir.getLM()).getReal(), kepCir.getLM().getReal(), kep.getLM().abs().multiply(Utils.epsilonTest).getReal());
573 Assertions.assertEquals(MathUtils.normalizeAngle(paramCir.getLE(), kepCir.getLE()).getReal(), kepCir.getLE().getReal(), kep.getLM().abs().multiply(Utils.epsilonTest).getReal());
574 Assertions.assertEquals(MathUtils.normalizeAngle(paramCir.getLv(), kepCir.getLv()).getReal(), kepCir.getLv().getReal(), kep.getLv().abs().multiply(Utils.epsilonTest).getReal());
575
576
577 T aH= field.getZero().add(-24464560.0);
578
579 T eH= field.getZero().add(1.7311);
580 T iH= field.getZero().add(0.122138);
581 T paH= field.getZero().add(3.10686);
582 T raanH= field.getZero().add(1.00681);
583 T anomalyH= field.getZero().add(0.048363);
584
585
586
587 FieldKeplerianOrbit<T> kepHyp =
588 new FieldKeplerianOrbit<>(aH, eH, iH, paH, raanH,
589 anomalyH, PositionAngleType.MEAN,
590 FramesFactory.getEME2000(), date, field.getZero().add(mu));
591
592 FieldVector3D<T> posHyp = kepHyp.getPosition();
593
594 FieldVector3D<T> vitHyp = kepHyp.getVelocity();
595
596 FieldKeplerianOrbit<T> paramHyp = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(posHyp, vitHyp),
597 FramesFactory.getEME2000(), date, field.getZero().add(mu));
598
599 Assertions.assertEquals(paramHyp.getA().getReal(), kepHyp.getA().getReal(), kepHyp.getA().abs().multiply(Utils.epsilonTest).getReal());
600 Assertions.assertEquals(paramHyp.getE().getReal(), kepHyp.getE().getReal(), kepHyp.getE().abs().multiply(Utils.epsilonTest).getReal());
601 Assertions.assertEquals(MathUtils.normalizeAngle(paramHyp.getI(), kepHyp.getI()).getReal(), kepHyp.getI().getReal(), kepHyp.getI().abs().multiply(Utils.epsilonTest).getReal());
602 Assertions.assertEquals(MathUtils.normalizeAngle(paramHyp.getPerigeeArgument(), kepHyp.getPerigeeArgument()).getReal(), kepHyp.getPerigeeArgument().getReal(), kepHyp.getPerigeeArgument().abs().multiply(Utils.epsilonTest).getReal());
603 Assertions.assertEquals(MathUtils.normalizeAngle(paramHyp.getRightAscensionOfAscendingNode(), kepHyp.getRightAscensionOfAscendingNode()).getReal(), kepHyp.getRightAscensionOfAscendingNode().getReal(), kepHyp.getRightAscensionOfAscendingNode().abs().multiply(Utils.epsilonTest).getReal());
604 Assertions.assertEquals(MathUtils.normalizeAngle(paramHyp.getMeanAnomaly(), kepHyp.getMeanAnomaly()).getReal(), kepHyp.getMeanAnomaly().getReal(), kepHyp.getMeanAnomaly().abs().multiply(Utils.epsilonTest).getReal());
605 }
606
607 private <T extends CalculusFieldElement<T>> void doTestKeplerianToCartesian(final Field<T> field) {
608
609 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
610
611 T a= field.getZero().add(24464560.0);
612 T e= field.getZero().add(0.7311);
613 T i= field.getZero().add(0.122138);
614 T pa= field.getZero().add(3.10686);
615 T raan= field.getZero().add(1.00681);
616 T anomaly= field.getZero().add(0.048363);
617
618
619 FieldKeplerianOrbit<T> kep =
620 new FieldKeplerianOrbit<>(a, e, i, pa, raan, anomaly, PositionAngleType.MEAN,
621 FramesFactory.getEME2000(), date, field.getZero().add(mu));
622
623 FieldVector3D<T> pos = kep.getPosition();
624 FieldVector3D<T> vit = kep.getVelocity();
625 Assertions.assertEquals(-0.107622532467967e+07, pos.getX().getReal(), Utils.epsilonTest * FastMath.abs(pos.getX().getReal()));
626 Assertions.assertEquals(-0.676589636432773e+07, pos.getY().getReal(), Utils.epsilonTest * FastMath.abs(pos.getY().getReal()));
627 Assertions.assertEquals(-0.332308783350379e+06, pos.getZ().getReal(), Utils.epsilonTest * FastMath.abs(pos.getZ().getReal()));
628
629 Assertions.assertEquals( 0.935685775154103e+04, vit.getX().getReal(), Utils.epsilonTest * FastMath.abs(vit.getX().getReal()));
630 Assertions.assertEquals(-0.331234775037644e+04, vit.getY().getReal(), Utils.epsilonTest * FastMath.abs(vit.getY().getReal()));
631 Assertions.assertEquals(-0.118801577532701e+04, vit.getZ().getReal(), Utils.epsilonTest * FastMath.abs(vit.getZ().getReal()));
632 }
633
634 private <T extends CalculusFieldElement<T>> void doTestKeplerianToEquinoctial(final Field<T> field) {
635 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
636 T a= field.getZero().add(24464560.0);
637
638 T e= field.getZero().add(0.7311);
639 T i= field.getZero().add(0.122138);
640 T pa= field.getZero().add(3.10686);
641 T raan= field.getZero().add(1.00681);
642 T anomaly= field.getZero().add(0.048363);
643
644
645 FieldKeplerianOrbit<T> kep =
646 new FieldKeplerianOrbit<>(a, e, i, pa, raan, anomaly, PositionAngleType.MEAN,
647 FramesFactory.getEME2000(), date, field.getZero().add(mu));
648
649 Assertions.assertEquals(24464560.0, kep.getA().getReal(), Utils.epsilonTest * kep.getA().getReal());
650 Assertions.assertEquals(-0.412036802887626, kep.getEquinoctialEx().getReal(), Utils.epsilonE * FastMath.abs(kep.getE().getReal()));
651 Assertions.assertEquals(-0.603931190671706, kep.getEquinoctialEy().getReal(), Utils.epsilonE * FastMath.abs(kep.getE().getReal()));
652 Assertions.assertEquals(MathUtils.normalizeAngle(2*FastMath.asin(FastMath.sqrt((FastMath.pow(0.652494417368829e-01, 2)+FastMath.pow(0.103158450084864, 2))/4.)), kep.getI().getReal()), kep.getI().getReal(), Utils.epsilonAngle * FastMath.abs(kep.getI().getReal()));
653 Assertions.assertEquals(MathUtils.normalizeAngle(0.416203300000000e+01, kep.getLM().getReal()), kep.getLM().getReal(), Utils.epsilonAngle * FastMath.abs(kep.getLM().getReal()));
654
655 }
656
657 private <T extends CalculusFieldElement<T>> void doTestAnomaly(final Field<T> field) {
658 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
659 FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(7.0e6), field.getZero().add(1.0e6), field.getZero().add(4.0e6));
660 FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(-500.0), field.getZero().add(8000.0), field.getZero().add(1000.0));
661 FieldKeplerianOrbit<T> p = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(position, velocity),
662 FramesFactory.getEME2000(), date, field.getZero().add(mu));
663
664
665 T e = p.getE();
666 T eRatio = (e.multiply(-1).add(1)).divide(e.add(1)).sqrt();
667
668 T v = field.getZero().add(1.1);
669
670 T E = v.divide(2).tan().multiply(eRatio).atan().multiply(2);
671 T M = E.sin().multiply(e).multiply(-1).add(E);
672
673 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
674 p.getRightAscensionOfAscendingNode(), v , PositionAngleType.TRUE,
675 p.getFrame(), p.getDate(), p.getMu());
676
677 Assertions.assertEquals(p.getTrueAnomaly().getReal(), v.getReal(), Utils.epsilonAngle * FastMath.abs(v.getReal()));
678 Assertions.assertEquals(p.getEccentricAnomaly().getReal(), E.getReal(), Utils.epsilonAngle * FastMath.abs(E.getReal()));
679 Assertions.assertEquals(p.getMeanAnomaly().getReal(), M.getReal(), Utils.epsilonAngle * FastMath.abs(M.getReal()));
680
681 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
682 p.getRightAscensionOfAscendingNode(), field.getZero() , PositionAngleType.TRUE,
683 p.getFrame(), p.getDate(), p.getMu());
684
685 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
686 p.getRightAscensionOfAscendingNode(), E , PositionAngleType.ECCENTRIC,
687 p.getFrame(), p.getDate(), p.getMu());
688
689 Assertions.assertEquals(p.getTrueAnomaly().getReal(), v.getReal(), Utils.epsilonAngle * FastMath.abs(v.getReal()));
690 Assertions.assertEquals(p.getEccentricAnomaly().getReal(), E.getReal(), Utils.epsilonAngle * FastMath.abs(E.getReal()));
691 Assertions.assertEquals(p.getMeanAnomaly().getReal(), M.getReal(), Utils.epsilonAngle * FastMath.abs(M.getReal()));
692
693 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
694 p.getRightAscensionOfAscendingNode(), field.getZero() , PositionAngleType.TRUE,
695 p.getFrame(), p.getDate(), p.getMu());
696
697 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
698 p.getRightAscensionOfAscendingNode(), M, PositionAngleType.MEAN,
699 p.getFrame(), p.getDate(), p.getMu());
700 Assertions.assertEquals(p.getTrueAnomaly().getReal(), v.getReal(), Utils.epsilonAngle * FastMath.abs(v.getReal()));
701 Assertions.assertEquals(p.getEccentricAnomaly().getReal(), E.getReal(), Utils.epsilonAngle * FastMath.abs(E.getReal()));
702 Assertions.assertEquals(p.getMeanAnomaly().getReal(), M.getReal(), Utils.epsilonAngle * FastMath.abs(M.getReal()));
703
704
705 p = new FieldKeplerianOrbit<>(p.getA(), field.getZero(), p.getI(), p.getPerigeeArgument(),
706 p.getRightAscensionOfAscendingNode(), p.getLv() , PositionAngleType.TRUE,
707 p.getFrame(), p.getDate(), p.getMu());
708
709 E = v;
710 M = E;
711
712 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
713 p.getRightAscensionOfAscendingNode(), v , PositionAngleType.TRUE,
714 p.getFrame(), p.getDate(), p.getMu());
715 Assertions.assertEquals(p.getTrueAnomaly().getReal(), v.getReal(), Utils.epsilonAngle * FastMath.abs(v.getReal()));
716 Assertions.assertEquals(p.getEccentricAnomaly().getReal(), E.getReal(), Utils.epsilonAngle * FastMath.abs(E.getReal()));
717 Assertions.assertEquals(p.getMeanAnomaly().getReal(), M.getReal(), Utils.epsilonAngle * FastMath.abs(M.getReal()));
718
719 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
720 p.getRightAscensionOfAscendingNode(), field.getZero() , PositionAngleType.TRUE,
721 p.getFrame(), p.getDate(), p.getMu());
722
723 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
724 p.getRightAscensionOfAscendingNode(), E , PositionAngleType.ECCENTRIC, p.getFrame(), p.getDate(), p.getMu());
725 Assertions.assertEquals(p.getTrueAnomaly().getReal(), v.getReal(), Utils.epsilonAngle * FastMath.abs(v.getReal()));
726 Assertions.assertEquals(p.getEccentricAnomaly().getReal(), E.getReal(), Utils.epsilonAngle * FastMath.abs(E.getReal()));
727 Assertions.assertEquals(p.getMeanAnomaly().getReal(), M.getReal(), Utils.epsilonAngle * FastMath.abs(M.getReal()));
728
729 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
730 p.getRightAscensionOfAscendingNode(), field.getZero() , PositionAngleType.TRUE,
731 p.getFrame(), p.getDate(), p.getMu());
732
733 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
734 p.getRightAscensionOfAscendingNode(), M, PositionAngleType.MEAN,
735 p.getFrame(), p.getDate(), p.getMu());
736
737 Assertions.assertEquals(p.getTrueAnomaly().getReal(), v.getReal(), Utils.epsilonAngle * FastMath.abs(v.getReal()));
738 Assertions.assertEquals(p.getEccentricAnomaly().getReal(), E.getReal(), Utils.epsilonAngle * FastMath.abs(E.getReal()));
739 Assertions.assertEquals(p.getMeanAnomaly().getReal(), M.getReal(), Utils.epsilonAngle * FastMath.abs(M.getReal()));
740
741 }
742
743 private <T extends CalculusFieldElement<T>> void doTestPositionVelocityNorms(final Field<T> field) {
744 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
745 T aa= field.getZero().add(24464560.0);
746 T ee= field.getZero().add(0.7311);
747 T i= field.getZero().add(2.1);
748 T pa= field.getZero().add(3.10686);
749 T raan= field.getZero().add(1.00681);
750 T anomaly= field.getZero().add(0.67);
751
752
753 FieldKeplerianOrbit<T> p =
754 new FieldKeplerianOrbit<>(aa, ee, i, pa, raan, anomaly, PositionAngleType.MEAN,
755 FramesFactory.getEME2000(), date, field.getZero().add(mu));
756
757
758
759 T e = p.getE();
760 T v = p.getTrueAnomaly();
761 T ksi = v.cos().multiply(e).add(1);
762 T nu = v.sin().multiply(e);
763 T epsilon = e.multiply(-1).add(1).multiply(e.add(1)).sqrt();
764
765 T a = p.getA();
766 T na = a.reciprocal().multiply(mu).sqrt();
767
768
769 Assertions.assertEquals(a.getReal() * epsilon.getReal() * epsilon.getReal() / ksi.getReal(),
770 p.getPosition().getNorm().getReal(),
771 Utils.epsilonTest * FastMath.abs(p.getPosition().getNorm().getReal()));
772
773
774 Assertions.assertEquals(na.getReal() * FastMath.sqrt(ksi.getReal() * ksi.getReal() + nu.getReal() * nu.getReal()) / epsilon.getReal(),
775 p.getVelocity().getNorm().getReal(),
776 Utils.epsilonTest * FastMath.abs(p.getVelocity().getNorm().getReal()));
777
778
779
780 FieldKeplerianOrbit<T> pCirEqua =
781 new FieldKeplerianOrbit<>(field.getZero().add(24464560.0), field.getZero().add(0.1e-10), field.getZero().add(0.1e-8), field.getZero().add(3.10686), field.getZero().add(1.00681),
782 field.getZero().add(0.67), PositionAngleType.TRUE,
783 FramesFactory.getEME2000(), date, field.getZero().add(mu));
784
785 e = pCirEqua.getE();
786 v = pCirEqua.getTrueAnomaly();
787 ksi = v.cos().multiply(e).add(1);
788 nu = v.sin().multiply(e);
789 epsilon = e.multiply(-1).add(1).multiply(e.add(1)).sqrt();
790
791 a = pCirEqua.getA();
792 na = a.reciprocal().multiply(mu).sqrt();
793
794
795 Assertions.assertEquals(a.getReal() * epsilon.getReal() * epsilon.getReal() / ksi.getReal(),
796 pCirEqua.getPosition().getNorm().getReal(),
797 Utils.epsilonTest * FastMath.abs(pCirEqua.getPosition().getNorm().getReal()));
798
799
800 Assertions.assertEquals(na.getReal() * FastMath.sqrt(ksi.getReal() * ksi.getReal() + nu.getReal() * nu.getReal()) / epsilon.getReal(),
801 pCirEqua.getVelocity().getNorm().getReal(),
802 Utils.epsilonTest * FastMath.abs(pCirEqua.getVelocity().getNorm().getReal()));
803 }
804
805 private <T extends CalculusFieldElement<T>> void doTestGeometry(final Field<T> field) {
806 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
807
808 FieldKeplerianOrbit<T> p =
809 new FieldKeplerianOrbit<>(field.getZero().add(24464560.0), field.getZero().add(0.7311), field.getZero().add(2.1), field.getZero().add(3.10686), field.getZero().add(1.00681),
810 field.getZero().add(0.67), PositionAngleType.TRUE,
811 FramesFactory.getEME2000(), date, field.getZero().add(mu));
812
813 FieldVector3D<T> position = p.getPosition();
814 FieldVector3D<T> velocity = p.getVelocity();
815 FieldVector3D<T> momentum = p.getPVCoordinates().getMomentum().normalize();
816
817 T apogeeRadius = p.getA().multiply(p.getE().add(1));
818 T perigeeRadius = p.getA().multiply(p.getE().multiply(-1).add(1));
819
820 for (T lv = field.getZero(); lv.getReal() <= field.getZero().add(2 * FastMath.PI).getReal(); lv =lv.add(2 * FastMath.PI/100.)) {
821 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
822 p.getRightAscensionOfAscendingNode(), lv , PositionAngleType.TRUE,
823 p.getFrame(), p.getDate(), p.getMu());
824 position = p.getPosition();
825
826
827
828
829
830 Assertions.assertTrue((position.getNorm().getReal() - apogeeRadius.getReal()) <= ( apogeeRadius.getReal() * Utils.epsilonTest));
831 Assertions.assertTrue((position.getNorm().getReal() - perigeeRadius.getReal()) >= (- perigeeRadius.getReal() * Utils.epsilonTest));
832
833 position = position.normalize();
834 velocity = p.getVelocity();
835 velocity = velocity.normalize();
836
837
838
839
840 Assertions.assertTrue(FastMath.abs(FieldVector3D.dotProduct(position, momentum).getReal()) < Utils.epsilonTest);
841
842 Assertions.assertTrue(FastMath.abs(FieldVector3D.dotProduct(velocity, momentum).getReal()) < Utils.epsilonTest);
843
844 }
845
846
847 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
848 p.getRightAscensionOfAscendingNode(), field.getZero(), PositionAngleType.TRUE, p.getFrame(), p.getDate(), p.getMu());
849 Assertions.assertEquals(p.getPosition().getNorm().getReal(), perigeeRadius.getReal(), perigeeRadius.getReal() * Utils.epsilonTest);
850
851 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
852 p.getRightAscensionOfAscendingNode(), field.getZero().add(FastMath.PI) , PositionAngleType.TRUE, p.getFrame(), p.getDate(), p.getMu());
853 Assertions.assertEquals(p.getPosition().getNorm().getReal(), apogeeRadius.getReal(), apogeeRadius.getReal() * Utils.epsilonTest);
854
855
856
857 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
858 p.getRightAscensionOfAscendingNode(), field.getZero().add(FastMath.PI).subtract(p.getPerigeeArgument()) , PositionAngleType.TRUE,
859 p.getFrame(), p.getDate(), p.getMu());
860
861 Assertions.assertTrue(FastMath.abs(p.getPosition().getZ().getReal()) < p.getPosition().getNorm().getReal() * Utils.epsilonTest);
862
863 Assertions.assertTrue(p.getVelocity().getZ().getReal() < 0);
864
865
866 p = new FieldKeplerianOrbit<>(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
867 p.getRightAscensionOfAscendingNode(), field.getZero().add(2.0 * FastMath.PI - p.getPerigeeArgument().getReal()) , PositionAngleType.TRUE,
868 p.getFrame(), p.getDate(), p.getMu());
869 Assertions.assertTrue(FastMath.abs(p.getPosition().getZ().getReal()) < p.getPosition().getNorm().getReal() * Utils.epsilonTest);
870 Assertions.assertTrue(p.getVelocity().getZ().getReal() > 0);
871
872
873 FieldKeplerianOrbit<T> pCirEqua =
874 new FieldKeplerianOrbit<>(field.getZero().add(24464560.0),
875 field.getZero().add(0.1e-10),
876 field.getZero().add(0.1e-8),
877 field.getZero().add(3.10686),
878 field.getZero().add(1.00681),
879 field.getZero().add(0.67),
880 PositionAngleType.TRUE,
881 FramesFactory.getEME2000(), date, field.getZero().add(mu));
882
883 position = pCirEqua.getPosition();
884 velocity = pCirEqua.getVelocity();
885 momentum = FieldVector3D.crossProduct(position, velocity).normalize();
886
887 apogeeRadius = pCirEqua.getA().multiply(pCirEqua.getE().add(1));
888 perigeeRadius = pCirEqua.getA().multiply(pCirEqua.getE().multiply(-1).add(1));
889
890
891 Assertions.assertEquals(perigeeRadius.getReal(), apogeeRadius.getReal(), 1.e+4 * Utils.epsilonTest * apogeeRadius.getReal());
892
893
894
895 for (T lv = field.getZero(); lv.getReal() <= 2 * FastMath.PI; lv = lv.add(2*FastMath.PI/100.)) {
896
897 pCirEqua = new FieldKeplerianOrbit<>(pCirEqua.getA(), pCirEqua.getE(), pCirEqua.getI(), pCirEqua.getPerigeeArgument(),
898 pCirEqua.getRightAscensionOfAscendingNode(), lv, PositionAngleType.TRUE,
899 pCirEqua.getFrame(), pCirEqua.getDate(), pCirEqua.getMu());
900 position = pCirEqua.getPosition();
901
902
903
904 Assertions.assertTrue((position.getNorm().getReal() - apogeeRadius.getReal()) <= ( apogeeRadius.getReal() * Utils.epsilonTest));
905 Assertions.assertTrue((position.getNorm().getReal() - perigeeRadius.getReal()) >= (- perigeeRadius.getReal() * Utils.epsilonTest));
906
907 position = position.normalize();
908 velocity = pCirEqua.getVelocity();
909 velocity = velocity.normalize();
910
911
912
913
914 Assertions.assertTrue(FastMath.abs(FieldVector3D.dotProduct(position, momentum).getReal()) < Utils.epsilonTest);
915
916 Assertions.assertTrue(FastMath.abs(FieldVector3D.dotProduct(velocity, momentum).getReal()) < Utils.epsilonTest);
917 }
918 }
919
920 private <T extends CalculusFieldElement<T>> void doTestSymmetry(final Field<T> field) {
921 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
922
923 FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(-4947831.), field.getZero().add(-3765382.), field.getZero().add(-3708221.));
924 FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(-2079.), field.getZero().add(5291.), field.getZero().add(-7842.));
925
926 FieldKeplerianOrbit<T> p = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(position, velocity),
927 FramesFactory.getEME2000(), date, field.getZero().add(mu));
928 FieldVector3D<T> positionOffset = p.getPosition().subtract(position);
929 FieldVector3D<T> velocityOffset = p.getVelocity().subtract(velocity);
930
931 Assertions.assertEquals(0, positionOffset.getNorm().getReal(), 2.9e-9);
932 Assertions.assertEquals(0, velocityOffset.getNorm().getReal(), 1.0e-15);
933
934
935 position = new FieldVector3D<>(field.getZero().add(1742382.), field.getZero().add(-2.440243e7), field.getZero().add(-0.014517));
936 velocity = new FieldVector3D<>(field.getZero().add(4026.2), field.getZero().add(287.479), field.getZero().add(-3.e-6));
937
938
939 p = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(position, velocity),
940 FramesFactory.getEME2000(), date, field.getZero().add(mu));
941 positionOffset = p.getPosition().subtract(position);
942 velocityOffset = p.getVelocity().subtract(velocity);
943
944 Assertions.assertEquals(0, positionOffset.getNorm().getReal(), 4.8e-9);
945 Assertions.assertEquals(0, velocityOffset.getNorm().getReal(), 1.0e-15);
946
947 }
948
949 private <T extends CalculusFieldElement<T>> void doTestNonInertialFrame(final Field<T> field) throws IllegalArgumentException {
950 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
951 FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(-4947831.), field.getZero().add(-3765382.), field.getZero().add(-3708221.));
952 FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(-2079.), field.getZero().add(5291.), field.getZero().add(-7842.));
953 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>( position, velocity);
954 new FieldKeplerianOrbit<>(pvCoordinates,
955 new Frame(FramesFactory.getEME2000(), Transform.IDENTITY, "non-inertial", false),
956 date, field.getZero().add(mu));
957 }
958
959 private <T extends CalculusFieldElement<T>> void doTestPeriod(final Field<T> field) {
960 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(field.getZero().add(7654321.0), field.getZero().add(0.1), field.getZero().add(0.2), field.getZero(), field.getZero(), field.getZero(),
961 PositionAngleType.TRUE,
962 FramesFactory.getEME2000(), new FieldAbsoluteDate<>(field),
963 field.getZero().add(mu));
964 Assertions.assertEquals(6664.5521723383589487, orbit.getKeplerianPeriod().getReal(), 1.0e-12);
965 Assertions.assertEquals(0.00094277682051291315229, orbit.getKeplerianMeanMotion().getReal(), 1.0e-16);
966 }
967
968 private <T extends CalculusFieldElement<T>> void doTestHyperbola1(final Field<T> field) {
969 T zero = field.getZero();
970 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(-10000000.0), zero.add(2.5), zero.add(0.3),
971 zero, zero, zero,
972 PositionAngleType.TRUE,
973 FramesFactory.getEME2000(), new FieldAbsoluteDate<>(field),
974 field.getZero().add(mu));
975 FieldVector3D<T> perigeeP = orbit.getPosition();
976 FieldVector3D<T> u = perigeeP.normalize();
977 FieldVector3D<T> focus1 = new FieldVector3D<>(zero, zero, zero);
978 FieldVector3D<T> focus2 = new FieldVector3D<>(orbit.getA().multiply(-2).multiply(orbit.getE()), u);
979 for (T dt = zero.add(-5000); dt.getReal() < 5000; dt = dt.add(60)) {
980 FieldPVCoordinates<T> pv = orbit.shiftedBy(dt).getPVCoordinates();
981 T d1 = FieldVector3D.distance(pv.getPosition(), focus1);
982 T d2 = FieldVector3D.distance(pv.getPosition(), focus2);
983 Assertions.assertEquals(orbit.getA().multiply(-2).getReal(), d1.subtract(d2).abs().getReal(), 1.0e-6);
984 FieldKeplerianOrbit<T> rebuilt =
985 new FieldKeplerianOrbit<>(pv, orbit.getFrame(), orbit.getDate().shiftedBy(dt), field.getZero().add(mu));
986 Assertions.assertEquals(-10000000.0, rebuilt.getA().getReal(), 1.0e-6);
987 Assertions.assertEquals(2.5, rebuilt.getE().getReal(), 1.0e-13);
988 }
989 }
990
991 private <T extends CalculusFieldElement<T>> void doTestHyperbola2(final Field<T> field) {
992 T zero = field.getZero();
993 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(-10000000.0), zero.add(1.2), zero.add(0.3),
994 zero, zero, zero.add(-1.75),
995 PositionAngleType.MEAN,
996 FramesFactory.getEME2000(), new FieldAbsoluteDate<>(field),
997 field.getZero().add(mu));
998 FieldVector3D<T> perigeeP = new FieldKeplerianOrbit<>(orbit.getA(), orbit.getE(), orbit.getI(),
999 orbit.getPerigeeArgument(), orbit.getRightAscensionOfAscendingNode(),
1000 zero, PositionAngleType.TRUE, orbit.getFrame(),
1001 orbit.getDate(), orbit.getMu()).getPosition();
1002 FieldVector3D<T> u = perigeeP.normalize();
1003 FieldVector3D<T> focus1 = new FieldVector3D<>(zero, zero, zero);
1004 FieldVector3D<T> focus2 = new FieldVector3D<>(orbit.getA().multiply(-2).multiply(orbit.getE()), u);
1005 for (T dt = zero.add(-5000); dt.getReal() < 5000; dt = dt.add(60)) {
1006 FieldPVCoordinates<T> pv = orbit.shiftedBy(dt).getPVCoordinates();
1007 T d1 = FieldVector3D.distance(pv.getPosition(), focus1);
1008 T d2 = FieldVector3D.distance(pv.getPosition(), focus2);
1009 Assertions.assertEquals(orbit.getA().multiply(-2).getReal(), d1.subtract(d2).abs().getReal(), 1.0e-6);
1010 FieldKeplerianOrbit<T> rebuilt =
1011 new FieldKeplerianOrbit<>(pv, orbit.getFrame(), orbit.getDate().shiftedBy(dt), field.getZero().add(mu));
1012 Assertions.assertEquals(-10000000.0, rebuilt.getA().getReal(), 1.0e-6);
1013 Assertions.assertEquals(1.2, rebuilt.getE().getReal(), 1.0e-13);
1014 }
1015 }
1016
1017 private <T extends CalculusFieldElement<T>> void doTestToOrbitWithoutDerivatives(Field<T> field) {
1018 T zero = field.getZero();
1019 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
1020
1021 FieldVector3D<T> position = new FieldVector3D<>(zero.add(7.0e6), zero.add(1.0e6), zero.add(4.0e6));
1022 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-500.0), zero.add(8000.0), zero.add(1000.0));
1023 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity);
1024 FieldKeplerianOrbit<T> fieldOrbit = new FieldKeplerianOrbit<>(pvCoordinates, FramesFactory.getEME2000(), date, field.getZero().add(mu));
1025 KeplerianOrbit orbit = fieldOrbit.toOrbit();
1026 Assertions.assertFalse(orbit.hasNonKeplerianAcceleration());
1027 MatcherAssert.assertThat(orbit.getA(), relativelyCloseTo(fieldOrbit.getA().getReal(), 0));
1028 MatcherAssert.assertThat(orbit.getE(), relativelyCloseTo(fieldOrbit.getE().getReal(), 0));
1029 MatcherAssert.assertThat(orbit.getI(), relativelyCloseTo(fieldOrbit.getI().getReal(), 0));
1030 MatcherAssert.assertThat(orbit.getPerigeeArgument(), relativelyCloseTo(fieldOrbit.getPerigeeArgument().getReal(), 0));
1031 MatcherAssert.assertThat(orbit.getRightAscensionOfAscendingNode(), relativelyCloseTo(fieldOrbit.getRightAscensionOfAscendingNode().getReal(), 0));
1032 MatcherAssert.assertThat(orbit.getTrueAnomaly(), relativelyCloseTo(fieldOrbit.getTrueAnomaly().getReal(), 0));
1033 }
1034
1035 private <T extends CalculusFieldElement<T>> void doTestToOrbitWithDerivatives(Field<T> field) {
1036 T zero = field.getZero();
1037 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
1038
1039 FieldVector3D<T> position = new FieldVector3D<>(zero.add(7.0e6), zero.add(1.0e6), zero.add(4.0e6));
1040 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-500.0), zero.add(8000.0), zero.add(1000.0));
1041 T r2 = position.getNormSq();
1042 T r = r2.sqrt();
1043 final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(zero.add(mu).negate()),
1044 position);
1045 final FieldVector3D<T> nonKeplerianAcceleration = keplerianAcceleration.scalarMultiply(1.1);
1046 final FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity, nonKeplerianAcceleration);
1047 FieldKeplerianOrbit<T> fieldOrbit = new FieldKeplerianOrbit<>(pvCoordinates, FramesFactory.getEME2000(), date, field.getZero().add(mu));
1048 KeplerianOrbit orbit = fieldOrbit.toOrbit();
1049 Assertions.assertTrue(orbit.hasNonKeplerianAcceleration());
1050 MatcherAssert.assertThat(orbit.getA(), relativelyCloseTo(fieldOrbit.getA().getReal(), 0));
1051 MatcherAssert.assertThat(orbit.getE(), relativelyCloseTo(fieldOrbit.getE().getReal(), 0));
1052 MatcherAssert.assertThat(orbit.getI(), relativelyCloseTo(fieldOrbit.getI().getReal(), 0));
1053 MatcherAssert.assertThat(orbit.getPerigeeArgument(), relativelyCloseTo(fieldOrbit.getPerigeeArgument().getReal(), 0));
1054 MatcherAssert.assertThat(orbit.getRightAscensionOfAscendingNode(), relativelyCloseTo(fieldOrbit.getRightAscensionOfAscendingNode().getReal(), 0));
1055 MatcherAssert.assertThat(orbit.getTrueAnomaly(), relativelyCloseTo(fieldOrbit.getTrueAnomaly().getReal(), 0));
1056 MatcherAssert.assertThat(orbit.getADot(), relativelyCloseTo(fieldOrbit.getADot().getReal(), 0));
1057 MatcherAssert.assertThat(orbit.getEDot(), relativelyCloseTo(fieldOrbit.getEDot().getReal(), 0));
1058 MatcherAssert.assertThat(orbit.getIDot(), relativelyCloseTo(fieldOrbit.getIDot().getReal(), 0));
1059 MatcherAssert.assertThat(orbit.getPerigeeArgumentDot(), relativelyCloseTo(fieldOrbit.getPerigeeArgumentDot().getReal(), 0));
1060 MatcherAssert.assertThat(orbit.getRightAscensionOfAscendingNodeDot(), relativelyCloseTo(fieldOrbit.getRightAscensionOfAscendingNodeDot().getReal(), 0));
1061 MatcherAssert.assertThat(orbit.getTrueAnomalyDot(), relativelyCloseTo(fieldOrbit.getTrueAnomalyDot().getReal(), 0));
1062 MatcherAssert.assertThat(orbit.getEccentricAnomalyDot(), relativelyCloseTo(fieldOrbit.getEccentricAnomalyDot().getReal(), 0));
1063 MatcherAssert.assertThat(orbit.getMeanAnomalyDot(), relativelyCloseTo(fieldOrbit.getMeanAnomalyDot().getReal(), 0));
1064 MatcherAssert.assertThat(orbit.getAnomalyDot(PositionAngleType.TRUE), relativelyCloseTo(fieldOrbit.getAnomalyDot(PositionAngleType.TRUE).getReal(), 0));
1065 MatcherAssert.assertThat(orbit.getAnomalyDot(PositionAngleType.ECCENTRIC), relativelyCloseTo(fieldOrbit.getAnomalyDot(PositionAngleType.ECCENTRIC).getReal(), 0));
1066 MatcherAssert.assertThat(orbit.getAnomalyDot(PositionAngleType.MEAN), relativelyCloseTo(fieldOrbit.getAnomalyDot(PositionAngleType.MEAN).getReal(), 0));
1067 }
1068
1069 private <T extends CalculusFieldElement<T>> void doTestInconsistentHyperbola(final Field<T> field) {
1070 try {
1071 new FieldKeplerianOrbit<>(field.getZero().add(+10000000.0), field.getZero().add(2.5), field.getZero().add(0.3),
1072 field.getZero(), field.getZero(), field.getZero(),
1073 PositionAngleType.TRUE,
1074 FramesFactory.getEME2000(),
1075 FieldAbsoluteDate.getJ2000Epoch(field),
1076 field.getZero().add(mu));
1077 Assertions.fail("an exception should have been thrown");
1078 } catch (OrekitIllegalArgumentException oe) {
1079 Assertions.assertEquals(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, oe.getSpecifier());
1080 Assertions.assertEquals(+10000000.0, ((Double) oe.getParts()[0]).doubleValue(), 1.0e-3);
1081 Assertions.assertEquals(2.5, ((Double) oe.getParts()[1]).doubleValue(), 1.0e-15);
1082 }
1083 }
1084
1085 private <T extends CalculusFieldElement<T>> void doTestVeryLargeEccentricity(final Field<T> field) {
1086 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
1087 final Frame eme2000 = FramesFactory.getEME2000();
1088 final double meanAnomaly = 1.;
1089 final FieldKeplerianOrbit<T> orb0 = new FieldKeplerianOrbit<>(field.getZero().add(42600e3), field.getZero().add(0.9), field.getZero().add(0.00001), field.getZero().add(0), field.getZero().add(0),
1090 field.getZero().add(FastMath.toRadians(meanAnomaly)),
1091 PositionAngleType.MEAN, eme2000, date, field.getZero().add(mu));
1092
1093 final FieldVector3D<T> deltaV = new FieldVector3D<>(field.getZero().add(0.0), field.getZero().add(110000.0), field.getZero());
1094 final FieldPVCoordinates<T> pv1 = new FieldPVCoordinates<>(orb0.getPosition(),
1095 orb0.getVelocity().add(deltaV));
1096 final FieldKeplerianOrbit<T> orb1 = new FieldKeplerianOrbit<>(pv1, eme2000, date, field.getZero().add(mu));
1097
1098
1099
1100 final FieldPVCoordinates<T> pvTested = orb1.shiftedBy(field.getZero()).getPVCoordinates();
1101 final FieldVector3D<T> pTested = pvTested.getPosition();
1102 final FieldVector3D<T> vTested = pvTested.getVelocity();
1103
1104
1105 final FieldPVCoordinates<T> pvReference = orb1.getPVCoordinates();
1106 final FieldVector3D<T> pReference = pvReference.getPosition();
1107 final FieldVector3D<T> vReference = pvReference.getVelocity();
1108
1109 final double threshold = 1.e-15;
1110 Assertions.assertEquals(0, pTested.subtract(pReference).getNorm().getReal(), pReference.getNorm().multiply(threshold).getReal());
1111 Assertions.assertEquals(0, vTested.subtract(vReference).getNorm().getReal(), vReference.getNorm().multiply(threshold).getReal());
1112
1113 }
1114
1115 private <T extends CalculusFieldElement<T>> void doTestKeplerEquation(final Field<T> field) {
1116 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
1117 for (T M = field.getZero().add(-6 * FastMath.PI); M.getReal() < 6 * FastMath.PI; M = M.add(0.01)) {
1118 FieldKeplerianOrbit<T> pElliptic =
1119 new FieldKeplerianOrbit<>(field.getZero().add(24464560.0), field.getZero().add(0.7311), field.getZero().add(2.1), field.getZero().add(3.10686), field.getZero().add(1.00681),
1120 field.getZero().add(M), PositionAngleType.MEAN,
1121 FramesFactory.getEME2000(), date, field.getZero().add(mu));
1122 T E = pElliptic.getEccentricAnomaly();
1123 T e = pElliptic.getE();
1124 Assertions.assertEquals(M.getReal(), E.getReal() - e.getReal() * FastMath.sin(E.getReal()), 2.0e-14);
1125 }
1126
1127 for (T M = field.getZero().add(-6 * FastMath.PI); M.getReal() < 6 * FastMath.PI; M = M.add(0.01)) {
1128
1129 FieldKeplerianOrbit<T> pAlmostParabolic =
1130 new FieldKeplerianOrbit<>(field.getZero().add(24464560.0), field.getZero().add(0.9999), field.getZero().add(2.1), field.getZero().add(3.10686), field.getZero().add(1.00681),
1131 field.getZero().add(M), PositionAngleType.MEAN,
1132 FramesFactory.getEME2000(), date, field.getZero().add(mu));
1133
1134 T E = pAlmostParabolic.getEccentricAnomaly();
1135 T e = pAlmostParabolic.getE();
1136 Assertions.assertEquals(M.getReal(), E.getReal() - e.getReal() * FastMath.sin(E.getReal()), 3.0e-13);
1137 }
1138
1139 }
1140
1141 private <T extends CalculusFieldElement<T>> void doTestOutOfRangeV(Field<T> field) {
1142 T zero = field.getZero();
1143 new FieldKeplerianOrbit<>(zero.add(-7000434.460140012),
1144 zero.add(1.1999785407363386),
1145 zero.add(1.3962787004479158),
1146 zero.add(1.3962320168955138),
1147 zero.add(0.3490728321331678),
1148 zero.add(-2.55593407037698),
1149 PositionAngleType.TRUE, FramesFactory.getEME2000(),
1150 new FieldAbsoluteDate<>(field, "2000-01-01T12:00:00.391", TimeScalesFactory.getUTC()),
1151 field.getZero().add(mu));
1152 }
1153
1154 private <T extends CalculusFieldElement<T>> void doTestNumericalIssue25(Field<T> field) {
1155 FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(3782116.14107698), field.getZero().add(416663.11924914), field.getZero().add(5875541.62103057));
1156 FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(-6349.7848910501), field.getZero().add(288.4061811651), field.getZero().add(4066.9366759691));
1157 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(position, velocity),
1158 FramesFactory.getEME2000(),
1159 new FieldAbsoluteDate<>(field, "2004-01-01T23:00:00.000",
1160 TimeScalesFactory.getUTC()),
1161 field.getZero().add(3.986004415E14));
1162 Assertions.assertEquals(0.0, orbit.getE().getReal(), 2.0e-14);
1163 }
1164
1165
1166 private <T extends CalculusFieldElement<T>> void doTestPerfectlyEquatorial(final Field<T> field) {
1167
1168 FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(6957904.3624652653594), field.getZero().add(766529.11411558074507), field.getZero());
1169 FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(-7538.2817012412102845), field.getZero().add(342.38751001881413381), field.getZero());
1170 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(position, velocity),
1171 FramesFactory.getEME2000(),
1172 new FieldAbsoluteDate<>(field, "2004-01-01T23:00:00.000",
1173 TimeScalesFactory.getUTC()),
1174 field.getZero().add(mu));
1175 Assertions.assertEquals(0.0, orbit.getI().getReal(), 2.0e-14);
1176 Assertions.assertEquals(0.0, orbit.getRightAscensionOfAscendingNode().getReal(), 2.0e-14);
1177 }
1178
1179 private <T extends CalculusFieldElement<T>> void doTestJacobianReferenceEllipse(final Field<T> field) {
1180
1181 FieldAbsoluteDate<T> dateTca = new FieldAbsoluteDate<>(field, 2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
1182 double mu = 3.986004415e+14;
1183 FieldKeplerianOrbit<T> orbKep = new FieldKeplerianOrbit<>(field.getZero().add(7000000.0),
1184 field.getZero().add(0.01),
1185 field.getZero().add(FastMath.toRadians(80.)),
1186 field.getZero().add(FastMath.toRadians(80.)),
1187 field.getZero().add(FastMath.toRadians(20.)),
1188 field.getZero().add(FastMath.toRadians(40.)),
1189 PositionAngleType.MEAN,
1190 FramesFactory.getEME2000(), dateTca, field.getZero().add(mu));
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236 FieldVector3D<T> pRef = new FieldVector3D<>(field.getZero().add(-3691555.569874833337963), field.getZero().add(-240330.253992714860942), field.getZero().add(5879700.285850423388183));
1237 FieldVector3D<T> vRef = new FieldVector3D<>(field.getZero().add(-5936.229884450408463), field.getZero().add(-2871.067660163344044), field.getZero().add(-3786.209549192726627));
1238
1239 double[][] jRef = {
1240 { -1.0792090588217809, -7.02594292049818631E-002, 1.7189029642216496, -1459.4829009393857, -705.88138246206040, -930.87838644776593 },
1241 { -1.31195762636625214E-007, -3.90087231593959271E-008, 4.65917592901869866E-008, -2.02467187867647177E-004, -7.89767994436215424E-005, -2.81639203329454407E-005 },
1242 { 4.18334478744371316E-008, -1.14936453412947957E-007, 2.15670500707930151E-008, -2.26450325965329431E-005, 6.22167157217876380E-005, -1.16745469637130306E-005 },
1243 { 3.52735168061691945E-006, 3.82555734454450974E-006, 1.34715077236557634E-005, -8.06586262922115264E-003, -6.13725651685311825E-003, -1.71765290503914092E-002 },
1244 { 2.48948022169790885E-008, -6.83979069529389238E-008, 1.28344057971888544E-008, 3.86597661353874888E-005, -1.06216834498373629E-004, 1.99308724078785540E-005 },
1245 { -3.41911705254704525E-006, -3.75913623359912437E-006, -1.34013845492518465E-005, 8.19851888816422458E-003, 6.16449264680494959E-003, 1.69495878276556648E-002 }
1246 };
1247
1248
1249
1250
1251 FieldPVCoordinates<T> pv = orbKep.getPVCoordinates();
1252 Assertions.assertEquals(0, pv.getPosition().subtract(pRef).getNorm().getReal(), 2.0e-16 * pRef.getNorm().getReal());
1253 Assertions.assertEquals(0, pv.getVelocity().subtract(vRef).getNorm().getReal(), 2.0e-16 * vRef.getNorm().getReal());
1254
1255 T[][] jacobian = MathArrays.buildArray(field, 6 , 6);
1256 orbKep.getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
1257
1258 for (int i = 0; i < jacobian.length; i++) {
1259 T[] row = jacobian[i];
1260 double[] rowRef = jRef[i];
1261 for (int j = 0; j < row.length; j++) {
1262 Assertions.assertEquals(0, (row[j].getReal() - rowRef[j]) / rowRef[j], 2.0e-12);
1263 }
1264 }
1265
1266 }
1267
1268 private <T extends CalculusFieldElement<T>> void doTestJacobianFinitedifferencesEllipse(final Field<T> field) {
1269
1270 FieldAbsoluteDate<T> dateTca = new FieldAbsoluteDate<>(field, 2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
1271 double mu = 3.986004415e+14;
1272 FieldKeplerianOrbit<T> orbKep = new FieldKeplerianOrbit<>(field.getZero().add(7000000.0), field.getZero().add(0.01), field.getZero().add(FastMath.toRadians(80.)), field.getZero().add(FastMath.toRadians(80.)), field.getZero().add(FastMath.toRadians(20.)),
1273 field.getZero().add(FastMath.toRadians(40.)), PositionAngleType.MEAN,
1274 FramesFactory.getEME2000(), dateTca, field.getZero().add(mu));
1275
1276 for (PositionAngleType type : PositionAngleType.values()) {
1277 T hP = field.getZero().add(2.0);
1278 T[][] finiteDiffJacobian = finiteDifferencesJacobian(type, orbKep, hP, field);
1279 T[][] jacobian = MathArrays.buildArray(field, 6, 6);
1280 orbKep.getJacobianWrtCartesian(type, jacobian);
1281
1282
1283
1284 for (int i = 0; i < jacobian.length; i++) {
1285 T[] row = jacobian[i];
1286 T[] rowRef = finiteDiffJacobian[i];
1287 for (int j = 0; j < row.length; j++) {
1288 Assertions.assertEquals(0, (row[j].getReal() - rowRef[j].getReal()) / rowRef[j].getReal(), 2.0e-7);
1289 }
1290 }
1291
1292 T[][] invJacobian = MathArrays.buildArray(field, 6, 6);
1293 orbKep.getJacobianWrtParameters(type, invJacobian);
1294 MatrixUtils.createFieldMatrix(jacobian).
1295 multiply(MatrixUtils.createFieldMatrix(invJacobian)).
1296 walkInRowOrder(new FieldMatrixPreservingVisitor<T>() {
1297 public void start(int rows, int columns,
1298 int startRow, int endRow, int startColumn, int endColumn) {
1299 }
1300
1301 public void visit(int row, int column, T value) {
1302 Assertions.assertEquals(row == column ? 1.0 : 0.0, value.getReal(), 1.0e-9);
1303 }
1304
1305 public T end() {
1306 return null;
1307 }
1308 });
1309 }
1310
1311 }
1312
1313 private <T extends CalculusFieldElement<T>> void doTestJacobianReferenceHyperbola(final Field<T> field) {
1314
1315 FieldAbsoluteDate<T> dateTca = new FieldAbsoluteDate<>(field, 2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
1316 double mu = 3.986004415e+14;
1317 FieldKeplerianOrbit<T> orbKep = new FieldKeplerianOrbit<>(field.getZero().add(-7000000.0), field.getZero().add(1.2), field.getZero().add(FastMath.toRadians(80.)), field.getZero().add(FastMath.toRadians(80.)), field.getZero().add(FastMath.toRadians(20.)),
1318 field.getZero().add(FastMath.toRadians(40.)), PositionAngleType.MEAN,
1319 FramesFactory.getEME2000(), dateTca, field.getZero().add(mu));
1320
1321
1322
1323
1324
1325
1326
1327
1328
1329
1330
1331
1332
1333
1334
1335
1336
1337
1338
1339
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351
1352
1353
1354
1355
1356
1357
1358
1359
1360
1361
1362
1363
1364
1365
1366
1367
1368
1369 FieldVector3D<T> pRef = new FieldVector3D<>(field.getZero().add(-7654711.206549182534218), field.getZero().add(-3460171.872979687992483), field.getZero().add(-3592374.514463655184954));
1370 FieldVector3D<T> vRef = new FieldVector3D<>(field.getZero().add(-7886.368091820805603), field.getZero().add(-4359.739012331759113), field.getZero().add( -7937.060044548694350));
1371 double[][] jRef = {
1372 { -0.98364725131848019, -0.44463970750901238, -0.46162803814668391, -1938.9443476028839, -1071.8864775981751, -1951.4074832397598 },
1373 { -1.10548813242982574E-007, -2.52906747183730431E-008, 7.96500937398593591E-008, -9.70479823470940108E-006, -2.93209076428001017E-005, -1.37434463892791042E-004 },
1374 { 8.55737680891616672E-008, -2.35111995522618220E-007, 4.41171797903162743E-008, -8.05235180390949802E-005, 2.21236547547460423E-004, -4.15135455876865407E-005 },
1375 { -1.52641427784095578E-007, 1.10250447958827901E-008, 1.21265251605359894E-007, 7.63347077200903542E-005, -3.54738331412232378E-005, -2.31400737283033359E-004 },
1376 { 7.86711766048035274E-008, -2.16147281283624453E-007, 4.05585791077187359E-008, -3.56071805267582894E-005, 9.78299244677127374E-005, -1.83571253224293247E-005 },
1377 { -2.41488884881911384E-007, -1.00119615610276537E-007, -6.51494225096757969E-008, -2.43295075073248163E-004, -1.43273725071890463E-004, -2.91625510452094873E-004 }
1378 };
1379
1380 FieldPVCoordinates<T> pv = orbKep.getPVCoordinates();
1381 Assertions.assertEquals(0, pv.getPosition().subtract(pRef).getNorm().getReal() / pRef.getNorm().getReal(), 1.0e-16);
1382 Assertions.assertEquals(0, pv.getVelocity().subtract(vRef).getNorm().getReal() / vRef.getNorm().getReal(), 5.0e-16);
1383
1384 T[][] jacobian = MathArrays.buildArray(field, 6, 6);
1385 orbKep.getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
1386
1387 for (int i = 0; i < jacobian.length; i++) {
1388
1389 T[] row = jacobian[i];
1390 double[] rowRef = jRef[i];
1391 for (int j = 0; j < row.length; j++) {
1392
1393
1394 Assertions.assertEquals(0, (row[j].getReal() - rowRef[j]) / rowRef[j], 1.0e-14);
1395 }
1396
1397 }
1398
1399 }
1400
1401 private <T extends CalculusFieldElement<T>> void doTestJacobianFinitedifferencesHyperbola(final Field<T> field) {
1402
1403 FieldAbsoluteDate<T> dateTca = new FieldAbsoluteDate<>(field, 2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
1404 double mu = 3.986004415e+14;
1405 FieldKeplerianOrbit<T> orbKep = new FieldKeplerianOrbit<>(field.getZero().add(-7000000.0), field.getZero().add(1.2), field.getZero().add(FastMath.toRadians(80.)), field.getZero().add(FastMath.toRadians(80.)), field.getZero().add(FastMath.toRadians(20.)),
1406 field.getZero().add(FastMath.toRadians(40.)), PositionAngleType.MEAN,
1407 FramesFactory.getEME2000(), dateTca, field.getZero().add(mu));
1408
1409 for (PositionAngleType type : PositionAngleType.values()) {
1410 T hP =field.getZero().add(2.0);
1411 T[][] finiteDiffJacobian = finiteDifferencesJacobian(type, orbKep, hP, field);
1412 T[][] jacobian = MathArrays.buildArray(field, 6, 6);
1413 orbKep.getJacobianWrtCartesian(type, jacobian);
1414 for (int i = 0; i < jacobian.length; i++) {
1415 T[] row = jacobian[i];
1416 T[] rowRef = finiteDiffJacobian[i];
1417
1418 for (int j = 0; j < row.length; j++) {
1419 Assertions.assertEquals(0, (row[j].getReal() - rowRef[j].getReal()) / rowRef[j].getReal(), 3.0e-8);
1420 }
1421 }
1422
1423 T[][] invJacobian = MathArrays.buildArray(field, 6, 6);
1424 orbKep.getJacobianWrtParameters(type, invJacobian);
1425 MatrixUtils.createFieldMatrix(jacobian).
1426 multiply(MatrixUtils.createFieldMatrix(invJacobian)).
1427 walkInRowOrder(new FieldMatrixPreservingVisitor<T>() {
1428 public void start(int rows, int columns,
1429 int startRow, int endRow, int startColumn, int endColumn) {
1430 }
1431
1432 public void visit(int row, int column, T value) {
1433 Assertions.assertEquals(row == column ? 1.0 : 0.0, value.getReal(), 2.0e-8);
1434 }
1435
1436 public T end() {
1437 return null;
1438 }
1439 });
1440 }
1441
1442 }
1443
1444 private <T extends CalculusFieldElement<T>> T[][] finiteDifferencesJacobian(PositionAngleType type, FieldKeplerianOrbit<T> orbit, T hP, final Field<T> field)
1445 {
1446 T[][] jacobian = MathArrays.buildArray(field, 6, 6);
1447 for (int i = 0; i < 6; ++i) {
1448 fillColumn(type, i, orbit, hP, jacobian);
1449 }
1450 return jacobian;
1451 }
1452
1453 private <T extends CalculusFieldElement<T>> void fillColumn(PositionAngleType type, int i, FieldKeplerianOrbit<T> orbit, T hP, T[][] jacobian) {
1454
1455
1456
1457 FieldVector3D<T> p = orbit.getPosition();
1458 FieldVector3D<T> v = orbit.getVelocity();
1459 T hV = hP.multiply(orbit.getMu()).divide(v.getNorm().multiply(p.getNormSq()));
1460 T h;
1461 FieldVector3D<T> dP = new FieldVector3D<>(p.getX().getField().getZero(), p.getX().getField().getZero(), p.getX().getField().getZero());
1462 FieldVector3D<T> dV = new FieldVector3D<>(p.getX().getField().getZero(), p.getX().getField().getZero(), p.getX().getField().getZero());
1463 switch (i) {
1464 case 0:
1465 h = hP;
1466 dP = new FieldVector3D<>(hP, p.getX().getField().getZero(), p.getX().getField().getZero());
1467 break;
1468 case 1:
1469 h = hP;
1470 dP = new FieldVector3D<>(p.getX().getField().getZero(), hP, p.getX().getField().getZero());
1471 break;
1472 case 2:
1473 h = hP;
1474 dP = new FieldVector3D<>(p.getX().getField().getZero(), p.getX().getField().getZero(), hP);
1475 break;
1476 case 3:
1477 h = hV;
1478 dV = new FieldVector3D<>(hV, p.getX().getField().getZero(), p.getX().getField().getZero());
1479 break;
1480 case 4:
1481 h = hV;
1482 dV = new FieldVector3D<>(p.getX().getField().getZero(), hV, p.getX().getField().getZero());
1483 break;
1484 default:
1485 h = hV;
1486 dV = new FieldVector3D<>(p.getX().getField().getZero(), p.getX().getField().getZero(), hV);
1487 break;
1488 }
1489
1490 FieldKeplerianOrbit<T> oM4h = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, -4, dP), new FieldVector3D<>(1, v, -4, dV)),
1491 orbit.getFrame(), orbit.getDate(), orbit.getMu());
1492 FieldKeplerianOrbit<T> oM3h = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, -3, dP), new FieldVector3D<>(1, v, -3, dV)),
1493 orbit.getFrame(), orbit.getDate(), orbit.getMu());
1494 FieldKeplerianOrbit<T> oM2h = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, -2, dP), new FieldVector3D<>(1, v, -2, dV)),
1495 orbit.getFrame(), orbit.getDate(), orbit.getMu());
1496 FieldKeplerianOrbit<T> oM1h = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, -1, dP), new FieldVector3D<>(1, v, -1, dV)),
1497 orbit.getFrame(), orbit.getDate(), orbit.getMu());
1498 FieldKeplerianOrbit<T> oP1h = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, +1, dP), new FieldVector3D<>(1, v, +1, dV)),
1499 orbit.getFrame(), orbit.getDate(), orbit.getMu());
1500 FieldKeplerianOrbit<T> oP2h = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, +2, dP), new FieldVector3D<>(1, v, +2, dV)),
1501 orbit.getFrame(), orbit.getDate(), orbit.getMu());
1502 FieldKeplerianOrbit<T> oP3h = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, +3, dP), new FieldVector3D<>(1, v, +3, dV)),
1503 orbit.getFrame(), orbit.getDate(), orbit.getMu());
1504 FieldKeplerianOrbit<T> oP4h = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, +4, dP), new FieldVector3D<>(1, v, +4, dV)),
1505 orbit.getFrame(), orbit.getDate(), orbit.getMu());
1506
1507 jacobian[0][i] = (oP4h.getA().subtract(oM4h.getA()).multiply(-3)).
1508 add(oP3h.getA().subtract(oM3h.getA()).multiply(32)).
1509 subtract(oP2h.getA().subtract(oM2h.getA()).multiply(168)).
1510 add(oP1h.getA().subtract(oM1h.getA()).multiply(672)).
1511 divide(h.multiply(840));
1512
1513 jacobian[1][i] = (oP4h.getE().subtract(oM4h.getE()).multiply(-3)).
1514 add(oP3h.getE().subtract(oM3h.getE()).multiply(32)).
1515 subtract(oP2h.getE().subtract(oM2h.getE()).multiply(168)).
1516 add(oP1h.getE().subtract(oM1h.getE()).multiply(672)).
1517 divide(h.multiply(840));
1518
1519 jacobian[2][i] = (oP4h.getI().subtract(oM4h.getI()).multiply(-3)).
1520 add(oP3h.getI().subtract(oM3h.getI()).multiply(32)).
1521 subtract(oP2h.getI().subtract(oM2h.getI()).multiply(168)).
1522 add(oP1h.getI().subtract(oM1h.getI()).multiply(672)).
1523 divide(h.multiply(840));
1524 jacobian[3][i] = (oP4h.getPerigeeArgument().subtract(oM4h.getPerigeeArgument()).multiply(-3)).
1525 add(oP3h.getPerigeeArgument().subtract(oM3h.getPerigeeArgument()).multiply(32)).
1526 subtract(oP2h.getPerigeeArgument().subtract(oM2h.getPerigeeArgument()).multiply(168)).
1527 add(oP1h.getPerigeeArgument().subtract(oM1h.getPerigeeArgument()).multiply(672)).
1528 divide(h.multiply(840));
1529
1530 jacobian[4][i] = (oP4h.getRightAscensionOfAscendingNode().subtract(oM4h.getRightAscensionOfAscendingNode()).multiply(-3)).
1531 add(oP3h.getRightAscensionOfAscendingNode().subtract(oM3h.getRightAscensionOfAscendingNode()).multiply(32)).
1532 subtract(oP2h.getRightAscensionOfAscendingNode().subtract(oM2h.getRightAscensionOfAscendingNode()).multiply(168)).
1533 add(oP1h.getRightAscensionOfAscendingNode().subtract(oM1h.getRightAscensionOfAscendingNode()).multiply(672)).
1534 divide(h.multiply(840));
1535 jacobian[5][i] = (oP4h.getAnomaly(type).subtract(oM4h.getAnomaly(type)).multiply(-3)).
1536 add(oP3h.getAnomaly(type).subtract(oM3h.getAnomaly(type)).multiply(32)).
1537 subtract(oP2h.getAnomaly(type).subtract(oM2h.getAnomaly(type)).multiply(168)).
1538 add(oP1h.getAnomaly(type).subtract(oM1h.getAnomaly(type)).multiply(672)).
1539 divide(h.multiply(840));
1540
1541 }
1542
1543 private <T extends CalculusFieldElement<T>> void doTestPerfectlyEquatorialConversion(final Field<T> field) {
1544 FieldAbsoluteDate<T> dateTca = new FieldAbsoluteDate<>(field, 2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
1545
1546 FieldKeplerianOrbit<T> initial = new FieldKeplerianOrbit<>(field.getZero().add(13378000.0),
1547 field.getZero().add(0.05),
1548 field.getZero().add(0.0),
1549 field.getZero().add(0.0),
1550 field.getZero().add(FastMath.PI),
1551 field.getZero().add(0.0), PositionAngleType.MEAN,
1552 FramesFactory.getEME2000(), dateTca,
1553 field.getZero().add(Constants.EIGEN5C_EARTH_MU));
1554 FieldEquinoctialOrbit<T> equ = (FieldEquinoctialOrbit<T>) OrbitType.EQUINOCTIAL.convertType(initial);
1555 FieldKeplerianOrbit<T> converted = (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.convertType(equ);
1556 Assertions.assertEquals(FastMath.PI,
1557 MathUtils.normalizeAngle(converted.getRightAscensionOfAscendingNode().getReal() +
1558 converted.getPerigeeArgument().getReal(), FastMath.PI),
1559 1.0e-10);
1560 }
1561
1562 private <T extends CalculusFieldElement<T>> void doTestKeplerianDerivatives(final Field<T> field) {
1563 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
1564 final FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(field.getZero().add(-4947831.),
1565 field.getZero().add(-3765382.),
1566 field.getZero().add(-3708221.)),
1567 new FieldVector3D<>(field.getZero().add(-2079.),
1568 field.getZero().add(5291.),
1569 field.getZero().add(-7842.))),
1570 FramesFactory.getEME2000(), date, field.getZero().add(mu));
1571 final FieldVector3D<T> p = orbit.getPosition();
1572 final FieldVector3D<T> v = orbit.getVelocity();
1573 final FieldVector3D<T> a = orbit.getPVCoordinates().getAcceleration();
1574
1575
1576 Assertions.assertEquals(7.605422, a.getNorm().getReal(), 1.0e-6);
1577
1578
1579 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPosition().getX()),
1580 orbit.getVelocity().getX().getReal(),
1581 4.0e-12 * v.getNorm().getReal());
1582 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPosition().getY()),
1583 orbit.getVelocity().getY().getReal(),
1584 4.0e-12 * v.getNorm().getReal());
1585 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPosition().getZ()),
1586 orbit.getVelocity().getZ().getReal(),
1587 4.0e-12 * v.getNorm().getReal());
1588
1589
1590 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getVelocity().getX()),
1591 orbit.getPVCoordinates().getAcceleration().getX().getReal(),
1592 6.0e-12 * a.getNorm().getReal());
1593 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getVelocity().getY()),
1594 orbit.getPVCoordinates().getAcceleration().getY().getReal(),
1595 6.0e-12 * a.getNorm().getReal());
1596 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getVelocity().getZ()),
1597 orbit.getPVCoordinates().getAcceleration().getZ().getReal(),
1598 6.0e-12 * a.getNorm().getReal());
1599
1600
1601 final T r2 = p.getNormSq();
1602 final T r = r2.sqrt();
1603 FieldVector3D<T> keplerianJerk = new FieldVector3D<>(FieldVector3D.dotProduct(p, v).multiply(-3).divide(r2), a,
1604 a.getNorm().divide(r).multiply(-1), v);
1605 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPVCoordinates().getAcceleration().getX()),
1606 keplerianJerk.getX().getReal(),
1607 5.0e-12 * keplerianJerk.getNorm().getReal());
1608 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPVCoordinates().getAcceleration().getY()),
1609 keplerianJerk.getY().getReal(),
1610 5.0e-12 * keplerianJerk.getNorm().getReal());
1611 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPVCoordinates().getAcceleration().getZ()),
1612 keplerianJerk.getZ().getReal(),
1613 5.0e-12 * keplerianJerk.getNorm().getReal());
1614
1615 final T zero = a.getX().getField().getZero();
1616 Assertions.assertEquals(zero, orbit.getADot());
1617 Assertions.assertEquals(zero, orbit.getEquinoctialExDot());
1618 Assertions.assertEquals(zero, orbit.getEquinoctialEyDot());
1619 Assertions.assertEquals(zero, orbit.getHxDot());
1620 Assertions.assertEquals(zero, orbit.getHyDot());
1621 Assertions.assertEquals(zero, orbit.getEDot());
1622 Assertions.assertEquals(zero, orbit.getIDot());
1623 Assertions.assertEquals(zero, orbit.getPerigeeArgumentDot());
1624 Assertions.assertEquals(zero, orbit.getRightAscensionOfAscendingNodeDot());
1625
1626 }
1627
1628 private <T extends CalculusFieldElement<T>, S extends Function<FieldKeplerianOrbit<T>, T>>
1629 double differentiate(FieldKeplerianOrbit<T> orbit, S picker) {
1630 final DSFactory factory = new DSFactory(1, 1);
1631 FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
1632 UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {
1633 public double value(double dt) {
1634 return picker.apply(orbit.shiftedBy(orbit.getDate().getField().getZero().add(dt))).getReal();
1635 }
1636 });
1637 return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
1638 }
1639
1640 private <T extends CalculusFieldElement<T>> void doTestNonKeplerianEllipticDerivatives(Field<T> field) {
1641 final T zero = field.getZero();
1642
1643 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1644 final FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(6896874.444705), field.getZero().add(1956581.072644), field.getZero().add(-147476.245054));
1645 final FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(166.816407662), field.getZero().add(-1106.783301861), field.getZero().add(-7372.745712770));
1646 final FieldVector3D <T> acceleration = new FieldVector3D<>(field.getZero().add(-7.466182457944), field.getZero().add(-2.118153357345), field.getZero().add(0.160004048437));
1647 final TimeStampedFieldPVCoordinates<T> pv = new TimeStampedFieldPVCoordinates<>(date, position, velocity, acceleration);
1648 final Frame frame = FramesFactory.getEME2000();
1649 final T mu = zero.add(Constants.EIGEN5C_EARTH_MU);
1650 final FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(pv, frame, mu);
1651
1652 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getA()),
1653 orbit.getADot().getReal(),
1654 4.3e-8);
1655 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEx()),
1656 orbit.getEquinoctialExDot().getReal(),
1657 2.1e-15);
1658 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEy()),
1659 orbit.getEquinoctialEyDot().getReal(),
1660 5.3e-16);
1661 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHx()),
1662 orbit.getHxDot().getReal(),
1663 1.6e-15);
1664 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHy()),
1665 orbit.getHyDot().getReal(),
1666 7.3e-17);
1667 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLv()),
1668 orbit.getLvDot().getReal(),
1669 1.1e-14);
1670 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLE()),
1671 orbit.getLEDot().getReal(),
1672 7.2e-15);
1673 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLM()),
1674 orbit.getLMDot().getReal(),
1675 4.7e-15);
1676 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getE()),
1677 orbit.getEDot().getReal(),
1678 6.9e-16);
1679 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getI()),
1680 orbit.getIDot().getReal(),
1681 5.7e-16);
1682 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getPerigeeArgument()),
1683 orbit.getPerigeeArgumentDot().getReal(),
1684 1.5e-12);
1685 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getRightAscensionOfAscendingNode()),
1686 orbit.getRightAscensionOfAscendingNodeDot().getReal(),
1687 1.5e-15);
1688 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getTrueAnomaly()),
1689 orbit.getTrueAnomalyDot().getReal(),
1690 1.5e-12);
1691 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEccentricAnomaly()),
1692 orbit.getEccentricAnomalyDot().getReal(),
1693 1.5e-12);
1694 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getMeanAnomaly()),
1695 orbit.getMeanAnomalyDot().getReal(),
1696 1.5e-12);
1697 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.TRUE)),
1698 orbit.getAnomalyDot(PositionAngleType.TRUE).getReal(),
1699 1.5e-12);
1700 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.ECCENTRIC)),
1701 orbit.getAnomalyDot(PositionAngleType.ECCENTRIC).getReal(),
1702 1.5e-12);
1703 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.MEAN)),
1704 orbit.getAnomalyDot(PositionAngleType.MEAN).getReal(),
1705 1.5e-12);
1706
1707 }
1708
1709 private <T extends CalculusFieldElement<T>> void doTestNonKeplerianHyperbolicDerivatives(final Field<T> field) {
1710 final T zero = field.getZero();
1711
1712 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1713 final FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(224267911.905821), field.getZero().add(290251613.109399), field.getZero().add(45534292.777492));
1714 final FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(-1494.068165293), field.getZero().add(1124.771027677), field.getZero().add(526.915286134));
1715 final FieldVector3D <T> acceleration = new FieldVector3D<>(field.getZero().add(-0.001295920501), field.getZero().add(-0.002233045187), field.getZero().add(-0.000349906292));
1716 final TimeStampedFieldPVCoordinates<T> pv = new TimeStampedFieldPVCoordinates<>(date, position, velocity, acceleration);
1717 final Frame frame = FramesFactory.getEME2000();
1718 final T mu = zero.add(Constants.EIGEN5C_EARTH_MU);
1719 final FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(pv, frame, mu);
1720
1721 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getA()),
1722 orbit.getADot().getReal(),
1723 9.6e-8);
1724 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEx()),
1725 orbit.getEquinoctialExDot().getReal(),
1726 2.8e-16);
1727 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEy()),
1728 orbit.getEquinoctialEyDot().getReal(),
1729 3.6e-15);
1730 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHx()),
1731 orbit.getHxDot().getReal(),
1732 1.4e-15);
1733 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHy()),
1734 orbit.getHyDot().getReal(),
1735 9.4e-16);
1736 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLv()),
1737 orbit.getLvDot().getReal(),
1738 5.6e-16);
1739 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLE()),
1740 orbit.getLEDot().getReal(),
1741 9.0e-16);
1742 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLM()),
1743 orbit.getLMDot().getReal(),
1744 1.8e-15);
1745 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getE()),
1746 orbit.getEDot().getReal(),
1747 1.8e-15);
1748 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getI()),
1749 orbit.getIDot().getReal(),
1750 3.6e-15);
1751 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getPerigeeArgument()),
1752 orbit.getPerigeeArgumentDot().getReal(),
1753 9.4e-16);
1754 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getRightAscensionOfAscendingNode()),
1755 orbit.getRightAscensionOfAscendingNodeDot().getReal(),
1756 1.1e-15);
1757 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getTrueAnomaly()),
1758 orbit.getTrueAnomalyDot().getReal(),
1759 1.4e-15);
1760 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEccentricAnomaly()),
1761 orbit.getEccentricAnomalyDot().getReal(),
1762 9.2e-16);
1763 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getMeanAnomaly()),
1764 orbit.getMeanAnomalyDot().getReal(),
1765 1.4e-15);
1766 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.TRUE)),
1767 orbit.getAnomalyDot(PositionAngleType.TRUE).getReal(),
1768 1.4e-15);
1769 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.ECCENTRIC)),
1770 orbit.getAnomalyDot(PositionAngleType.ECCENTRIC).getReal(),
1771 9.2e-16);
1772 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.MEAN)),
1773 orbit.getAnomalyDot(PositionAngleType.MEAN).getReal(),
1774 1.4e-15);
1775
1776 }
1777
1778 private <T extends CalculusFieldElement<T>, S extends Function<FieldKeplerianOrbit<T>, T>>
1779 double differentiate(TimeStampedFieldPVCoordinates<T> pv, Frame frame, T mu, S picker) {
1780 final DSFactory factory = new DSFactory(1, 1);
1781 FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
1782 UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {
1783 public double value(double dt) {
1784 return picker.apply(new FieldKeplerianOrbit<>(pv.shiftedBy(dt), frame, mu)).getReal();
1785 }
1786 });
1787 return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
1788 }
1789
1790 private <T extends CalculusFieldElement<T>> void doTestPositionAngleDerivatives(final Field<T> field) {
1791 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1792 final FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(6896874.444705), field.getZero().add(1956581.072644), field.getZero().add(-147476.245054));
1793 final FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(166.816407662), field.getZero().add(-1106.783301861), field.getZero().add(-7372.745712770));
1794 final FieldVector3D <T> acceleration = new FieldVector3D<>(field.getZero().add(-7.466182457944), field.getZero().add(-2.118153357345), field.getZero().add(0.160004048437));
1795 final TimeStampedFieldPVCoordinates<T> pv = new TimeStampedFieldPVCoordinates<>(date, position, velocity, acceleration);
1796 final Frame frame = FramesFactory.getEME2000();
1797 final double mu = Constants.EIGEN5C_EARTH_MU;
1798 final FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(pv, frame, field.getZero().add(mu));
1799
1800 for (PositionAngleType type : PositionAngleType.values()) {
1801 final FieldKeplerianOrbit<T> rebuilt = new FieldKeplerianOrbit<>(orbit.getA(),
1802 orbit.getE(),
1803 orbit.getI(),
1804 orbit.getPerigeeArgument(),
1805 orbit.getRightAscensionOfAscendingNode(),
1806 orbit.getAnomaly(type),
1807 orbit.getADot(),
1808 orbit.getEDot(),
1809 orbit.getIDot(),
1810 orbit.getPerigeeArgumentDot(),
1811 orbit.getRightAscensionOfAscendingNodeDot(),
1812 orbit.getAnomalyDot(type),
1813 type, orbit.getFrame(), orbit.getDate(), orbit.getMu());
1814 MatcherAssert.assertThat(rebuilt.getA().getReal(), relativelyCloseTo(orbit.getA().getReal(), 1));
1815 MatcherAssert.assertThat(rebuilt.getE().getReal(), relativelyCloseTo(orbit.getE().getReal(), 1));
1816 MatcherAssert.assertThat(rebuilt.getI().getReal(), relativelyCloseTo(orbit.getI().getReal(), 1));
1817 MatcherAssert.assertThat(rebuilt.getPerigeeArgument().getReal(), relativelyCloseTo(orbit.getPerigeeArgument().getReal(), 1));
1818 MatcherAssert.assertThat(rebuilt.getRightAscensionOfAscendingNode().getReal(), relativelyCloseTo(orbit.getRightAscensionOfAscendingNode().getReal(), 1));
1819 MatcherAssert.assertThat(rebuilt.getADot().getReal(), relativelyCloseTo(orbit.getADot().getReal(), 1));
1820 MatcherAssert.assertThat(rebuilt.getEDot().getReal(), relativelyCloseTo(orbit.getEDot().getReal(), 1));
1821 MatcherAssert.assertThat(rebuilt.getIDot().getReal(), relativelyCloseTo(orbit.getIDot().getReal(), 1));
1822 MatcherAssert.assertThat(rebuilt.getPerigeeArgumentDot().getReal(), relativelyCloseTo(orbit.getPerigeeArgumentDot().getReal(), 1));
1823 MatcherAssert.assertThat(rebuilt.getRightAscensionOfAscendingNodeDot().getReal(), relativelyCloseTo(orbit.getRightAscensionOfAscendingNodeDot().getReal(), 1));
1824 for (PositionAngleType type2 : PositionAngleType.values()) {
1825 MatcherAssert.assertThat(rebuilt.getAnomaly(type2).getReal(), relativelyCloseTo(orbit.getAnomaly(type2).getReal(), 1));
1826 MatcherAssert.assertThat(rebuilt.getAnomalyDot(type2).getReal(), relativelyCloseTo(orbit.getAnomalyDot(type2).getReal(), 1));
1827 }
1828 }
1829
1830 }
1831
1832 private <T extends CalculusFieldElement<T>> void doTestPositionAngleHyperbolicDerivatives(final Field<T> field) {
1833 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1834 final FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(224267911.905821), field.getZero().add(290251613.109399), field.getZero().add(45534292.777492));
1835 final FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(-1494.068165293), field.getZero().add(1124.771027677), field.getZero().add(526.915286134));
1836 final FieldVector3D <T> acceleration = new FieldVector3D<>(field.getZero().add(-0.001295920501), field.getZero().add(-0.002233045187), field.getZero().add(-0.000349906292));
1837 final TimeStampedFieldPVCoordinates<T> pv = new TimeStampedFieldPVCoordinates<>(date, position, velocity, acceleration);
1838 final Frame frame = FramesFactory.getEME2000();
1839 final double mu = Constants.EIGEN5C_EARTH_MU;
1840 final FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(pv, frame, field.getZero().add(mu));
1841
1842 for (PositionAngleType type : PositionAngleType.values()) {
1843 final FieldKeplerianOrbit<T> rebuilt = new FieldKeplerianOrbit<>(orbit.getA(),
1844 orbit.getE(),
1845 orbit.getI(),
1846 orbit.getPerigeeArgument(),
1847 orbit.getRightAscensionOfAscendingNode(),
1848 orbit.getAnomaly(type),
1849 orbit.getADot(),
1850 orbit.getEDot(),
1851 orbit.getIDot(),
1852 orbit.getPerigeeArgumentDot(),
1853 orbit.getRightAscensionOfAscendingNodeDot(),
1854 orbit.getAnomalyDot(type),
1855 type, orbit.getFrame(), orbit.getDate(), orbit.getMu());
1856 MatcherAssert.assertThat(rebuilt.getA().getReal(), relativelyCloseTo(orbit.getA().getReal(), 1));
1857 MatcherAssert.assertThat(rebuilt.getE().getReal(), relativelyCloseTo(orbit.getE().getReal(), 1));
1858 MatcherAssert.assertThat(rebuilt.getI().getReal(), relativelyCloseTo(orbit.getI().getReal(), 1));
1859 MatcherAssert.assertThat(rebuilt.getPerigeeArgument().getReal(), relativelyCloseTo(orbit.getPerigeeArgument().getReal(), 1));
1860 MatcherAssert.assertThat(rebuilt.getRightAscensionOfAscendingNode().getReal(), relativelyCloseTo(orbit.getRightAscensionOfAscendingNode().getReal(), 1));
1861 MatcherAssert.assertThat(rebuilt.getADot().getReal(), relativelyCloseTo(orbit.getADot().getReal(), 1));
1862 MatcherAssert.assertThat(rebuilt.getEDot().getReal(), relativelyCloseTo(orbit.getEDot().getReal(), 1));
1863 MatcherAssert.assertThat(rebuilt.getIDot().getReal(), relativelyCloseTo(orbit.getIDot().getReal(), 1));
1864 MatcherAssert.assertThat(rebuilt.getPerigeeArgumentDot().getReal(), relativelyCloseTo(orbit.getPerigeeArgumentDot().getReal(), 1));
1865 MatcherAssert.assertThat(rebuilt.getRightAscensionOfAscendingNodeDot().getReal(), relativelyCloseTo(orbit.getRightAscensionOfAscendingNodeDot().getReal(), 1));
1866 for (PositionAngleType type2 : PositionAngleType.values()) {
1867 MatcherAssert.assertThat(rebuilt.getAnomaly(type2).getReal(), relativelyCloseTo(orbit.getAnomaly(type2).getReal(), 3));
1868 MatcherAssert.assertThat(rebuilt.getAnomalyDot(type2).getReal(), relativelyCloseTo(orbit.getAnomalyDot(type2).getReal(), 4));
1869 }
1870 }
1871
1872 }
1873
1874 private <T extends CalculusFieldElement<T>> void doTestEquatorialRetrograde(final Field<T> field) {
1875 FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(10000000.0), field.getZero(), field.getZero());
1876 FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero(), field.getZero().add(-6500.0), field.getZero().add(1.0e-10));
1877 T r2 = position.getNormSq();
1878 T r = r2.sqrt();
1879 FieldVector3D<T> acceleration = new FieldVector3D<>(r.multiply(r2.reciprocal().multiply(-mu)), position,
1880 field.getOne(), new FieldVector3D<>(field.getZero().add(-0.1),
1881 field.getZero().add(0.2),
1882 field.getZero().add(0.3)));
1883 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity, acceleration);
1884 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(pvCoordinates, FramesFactory.getEME2000(),
1885 FieldAbsoluteDate.getJ2000Epoch(field), field.getZero().add(mu));
1886 Assertions.assertEquals(10637829.465, orbit.getA().getReal(), 1.0e-3);
1887 Assertions.assertEquals(-738.145, orbit.getADot().getReal(), 1.0e-3);
1888 Assertions.assertEquals(0.05995861, orbit.getE().getReal(), 1.0e-8);
1889 Assertions.assertEquals(-6.523e-5, orbit.getEDot().getReal(), 1.0e-8);
1890 Assertions.assertEquals(FastMath.PI, orbit.getI().getReal(), 2.0e-14);
1891 Assertions.assertEquals(-4.615e-5, orbit.getIDot().getReal(), 1.0e-8);
1892 Assertions.assertTrue(Double.isNaN(orbit.getHx().getReal()));
1893 Assertions.assertTrue(Double.isNaN(orbit.getHxDot().getReal()));
1894 Assertions.assertTrue(Double.isNaN(orbit.getHy().getReal()));
1895 Assertions.assertTrue(Double.isNaN(orbit.getHyDot().getReal()));
1896 }
1897
1898 private <T extends CalculusFieldElement<T>> void doTestDerivativesConversionSymmetry(Field<T> field) {
1899 T zero = field.getZero();
1900 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:01:20.000", TimeScalesFactory.getUTC());
1901 FieldVector3D<T> position = new FieldVector3D<>(zero.add(6893443.400234382),
1902 zero.add(1886406.1073757345),
1903 zero.add(-589265.1150359757));
1904 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-281.1261461082365),
1905 zero.add(-1231.6165642450928),
1906 zero.add(-7348.756363469432));
1907 FieldVector3D<T> acceleration = new FieldVector3D<>(zero.add(-7.460341170581685),
1908 zero.add(-2.0415957334584527),
1909 zero.add(0.6393322823627762));
1910 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>( position, velocity, acceleration);
1911 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(pvCoordinates, FramesFactory.getEME2000(),
1912 date, zero.add(Constants.EIGEN5C_EARTH_MU));
1913 Assertions.assertTrue(orbit.hasNonKeplerianAcceleration());
1914 T r2 = position.getNormSq();
1915 T r = r2.sqrt();
1916 FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(orbit.getMu().negate()),
1917 position);
1918 Assertions.assertEquals(0.0101, FieldVector3D.distance(keplerianAcceleration, acceleration).getReal(), 1.0e-4);
1919
1920 for (OrbitType type : OrbitType.values()) {
1921 FieldOrbit<T> converted = type.convertType(orbit);
1922 Assertions.assertTrue(converted.hasNonKeplerianAcceleration());
1923 FieldKeplerianOrbit<T> rebuilt = (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.convertType(converted);
1924 Assertions.assertTrue(rebuilt.hasNonKeplerianAcceleration());
1925 Assertions.assertEquals(orbit.getADot().getReal(), rebuilt.getADot().getReal(), 3.0e-13);
1926 Assertions.assertEquals(orbit.getEDot().getReal(), rebuilt.getEDot().getReal(), 1.0e-15);
1927 Assertions.assertEquals(orbit.getIDot().getReal(), rebuilt.getIDot().getReal(), 1.0e-15);
1928 Assertions.assertEquals(orbit.getPerigeeArgumentDot().getReal(), rebuilt.getPerigeeArgumentDot().getReal(), 2.0e-15);
1929 Assertions.assertEquals(orbit.getRightAscensionOfAscendingNodeDot().getReal(), rebuilt.getRightAscensionOfAscendingNodeDot().getReal(), 1.0e-15);
1930 Assertions.assertEquals(orbit.getTrueAnomalyDot().getReal(), rebuilt.getTrueAnomalyDot().getReal(), 2.0e-15);
1931 }
1932
1933 }
1934
1935 private <T extends CalculusFieldElement<T>> void doTestDerivativesConversionSymmetryHyperbolic(Field<T> field) {
1936 T zero = field.getZero();
1937 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1938 FieldVector3D<T> position = new FieldVector3D<>(zero.add(224267911.905821),
1939 zero.add(290251613.109399),
1940 zero.add(45534292.777492));
1941 FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-1494.068165293),
1942 zero.add(1124.771027677),
1943 zero.add(526.915286134));
1944 FieldVector3D<T> acceleration = new FieldVector3D<>(zero.add(-0.001295920501),
1945 zero.add(-0.002233045187),
1946 zero.add(-0.000349906292));
1947 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>( position, velocity, acceleration);
1948 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(pvCoordinates, FramesFactory.getEME2000(),
1949 date, zero.add(Constants.EIGEN5C_EARTH_MU));
1950 Assertions.assertTrue(orbit.hasNonKeplerianAcceleration());
1951 T r2 = position.getNormSq();
1952 T r = r2.sqrt();
1953 FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(orbit.getMu().negate()),
1954 position);
1955 Assertions.assertEquals(4.78e-4, FieldVector3D.distance(keplerianAcceleration, acceleration).getReal(), 1.0e-6);
1956
1957 OrbitType type = OrbitType.CARTESIAN;
1958 FieldOrbit<T> converted = type.convertType(orbit);
1959 Assertions.assertTrue(converted.hasNonKeplerianAcceleration());
1960 FieldKeplerianOrbit<T> rebuilt = (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.convertType(converted);
1961 Assertions.assertTrue(rebuilt.hasNonKeplerianAcceleration());
1962 Assertions.assertEquals(orbit.getADot().getReal(), rebuilt.getADot().getReal(), 3.0e-13);
1963 Assertions.assertEquals(orbit.getEDot().getReal(), rebuilt.getEDot().getReal(), 1.0e-15);
1964 Assertions.assertEquals(orbit.getIDot().getReal(), rebuilt.getIDot().getReal(), 1.0e-15);
1965 Assertions.assertEquals(orbit.getPerigeeArgumentDot().getReal(), rebuilt.getPerigeeArgumentDot().getReal(), 1.0e-15);
1966 Assertions.assertEquals(orbit.getRightAscensionOfAscendingNodeDot().getReal(), rebuilt.getRightAscensionOfAscendingNodeDot().getReal(), 1.0e-15);
1967 Assertions.assertEquals(orbit.getTrueAnomalyDot().getReal(), rebuilt.getTrueAnomalyDot().getReal(), 1.0e-15);
1968
1969 }
1970
1971 private <T extends CalculusFieldElement<T>> void doTestToString(Field<T> field) {
1972 FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(-29536113.0),
1973 field.getZero().add(30329259.0),
1974 field.getZero().add(-100125.0));
1975 FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(-2194.0),
1976 field.getZero().add(-2141.0),
1977 field.getZero().add(-8.0));
1978 FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity);
1979 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(pvCoordinates, FramesFactory.getEME2000(),
1980 FieldAbsoluteDate.getJ2000Epoch(field), field.getZero().add(mu));
1981 Assertions.assertEquals("Keplerian parameters: {a: 4.225517000282565E7; e: 0.002146216321416967; i: 0.20189257051515358; pa: 13.949966363606599; raan: -87.91788415673473; v: -151.79096272977213;}",
1982 orbit.toString());
1983 }
1984
1985 private <T extends CalculusFieldElement<T>> void doTestCopyNonKeplerianAcceleration(Field<T> field)
1986 {
1987
1988 final Frame eme2000 = FramesFactory.getEME2000();
1989
1990
1991 final FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(42164140),
1992 field.getZero(),
1993 field.getZero());
1994
1995 final FieldPVCoordinates<T> pv =
1996 new FieldPVCoordinates<>(position,
1997 new FieldVector3D<>(field.getZero(),
1998 position.getNorm().reciprocal().multiply(mu).sqrt(),
1999 field.getZero()));
2000
2001 final FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(pv, eme2000, FieldAbsoluteDate.getJ2000Epoch(field), field.getZero().add(mu));
2002
2003
2004 final FieldOrbit<T> orbitCopy = new FieldKeplerianOrbit<>(orbit);
2005
2006
2007 final FieldOrbit<T> shiftedOrbit = orbit.shiftedBy(10);
2008 final FieldOrbit<T> shiftedOrbitCopy = orbitCopy.shiftedBy(10);
2009
2010 Assertions.assertEquals(0.0,
2011 FieldVector3D.distance(shiftedOrbit.getPosition(),
2012 shiftedOrbitCopy.getPosition()).getReal(),
2013 1.0e-10);
2014 Assertions.assertEquals(0.0,
2015 FieldVector3D.distance(shiftedOrbit.getVelocity(),
2016 shiftedOrbitCopy.getVelocity()).getReal(),
2017 1.0e-10);
2018
2019 }
2020
2021 private <T extends CalculusFieldElement<T>> void doTestIssue674(Field<T> field) {
2022 try {
2023 final T zero = field.getZero();
2024 final FieldAbsoluteDate<T> date = FieldAbsoluteDate.getJ2000Epoch(field);
2025 new FieldKeplerianOrbit<>(zero.add(24464560.0), zero.add(-0.7311), zero.add(0.122138),
2026 zero.add(3.10686), zero.add(1.00681),
2027 zero.add(0.048363), PositionAngleType.MEAN,
2028 FramesFactory.getEME2000(), date, zero.add(mu));
2029 Assertions.fail("an exception should have been thrown");
2030 } catch (OrekitException oe) {
2031 Assertions.assertEquals(OrekitMessages.INVALID_PARAMETER_RANGE, oe.getSpecifier());
2032 Assertions.assertEquals("eccentricity", oe.getParts()[0]);
2033 Assertions.assertEquals(-0.7311, ((Double) oe.getParts()[1]).doubleValue(), Double.MIN_VALUE);
2034 Assertions.assertEquals(0.0, ((Double) oe.getParts()[2]).doubleValue(), Double.MIN_VALUE);
2035 Assertions.assertEquals(Double.POSITIVE_INFINITY, ((Double) oe.getParts()[3]).doubleValue(), Double.MIN_VALUE);
2036 }
2037 }
2038
2039 private <T extends CalculusFieldElement<T>> void doTestNormalize(Field<T> field) {
2040 final T zero = field.getZero();
2041 FieldKeplerianOrbit<T> withoutDerivatives =
2042 new FieldKeplerianOrbit<>(zero.newInstance(42166712.0), zero.newInstance(0.005),
2043 zero.newInstance(1.6), zero.newInstance(-0.3),
2044 zero.newInstance(1.25), zero.newInstance(0.4), PositionAngleType.MEAN,
2045 FramesFactory.getEME2000(), FieldAbsoluteDate.getJ2000Epoch(field),
2046 zero.newInstance(mu));
2047 FieldKeplerianOrbit<T> ref =
2048 new FieldKeplerianOrbit<>(zero.newInstance(24000000.0), zero.newInstance(0.012),
2049 zero.newInstance(1.9), zero.newInstance(-6.28),
2050 zero.newInstance(6.28), zero.newInstance(12.56), PositionAngleType.MEAN,
2051 FramesFactory.getEME2000(), FieldAbsoluteDate.getJ2000Epoch(field),
2052 zero.newInstance(mu));
2053
2054 FieldKeplerianOrbit<T> normalized1 = (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.normalize(withoutDerivatives, ref);
2055 Assertions.assertFalse(normalized1.hasNonKeplerianAcceleration());
2056 Assertions.assertEquals(0.0, normalized1.getA().subtract(withoutDerivatives.getA()).getReal(), 1.0e-6);
2057 Assertions.assertEquals(0.0, normalized1.getE().subtract(withoutDerivatives.getE()).getReal(), 1.0e-10);
2058 Assertions.assertEquals(0.0, normalized1.getI().subtract(withoutDerivatives.getI()).getReal(), 1.0e-10);
2059 Assertions.assertEquals(-MathUtils.TWO_PI, normalized1.getPerigeeArgument().subtract(withoutDerivatives.getPerigeeArgument()).getReal(), 1.0e-10);
2060 Assertions.assertEquals(+MathUtils.TWO_PI, normalized1.getRightAscensionOfAscendingNode().subtract(withoutDerivatives.getRightAscensionOfAscendingNode()).getReal(), 1.0e-10);
2061 Assertions.assertEquals(2 * MathUtils.TWO_PI, normalized1.getTrueAnomaly().subtract(withoutDerivatives.getTrueAnomaly()).getReal(), 1.0e-10);
2062
2063 T[] p = MathArrays.buildArray(field, 6);
2064 T[] pDot = MathArrays.buildArray(field, 6);
2065 for (int i = 0; i < pDot.length; ++i) {
2066 pDot[i] = zero.newInstance(i);
2067 }
2068 OrbitType.KEPLERIAN.mapOrbitToArray(withoutDerivatives, PositionAngleType.TRUE, p, null);
2069 FieldKeplerianOrbit<T> withDerivatives = (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.mapArrayToOrbit(p, pDot,
2070 PositionAngleType.TRUE,
2071 withoutDerivatives.getDate(),
2072 withoutDerivatives.getMu(),
2073 withoutDerivatives.getFrame());
2074 FieldKeplerianOrbit<T> normalized2 = (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.normalize(withDerivatives, ref);
2075 Assertions.assertTrue(normalized2.hasNonKeplerianAcceleration());
2076 Assertions.assertEquals(0.0, normalized2.getA().subtract(withDerivatives.getA()).getReal(), 1.0e-6);
2077 Assertions.assertEquals(0.0, normalized2.getE().subtract(withDerivatives.getE()).getReal(), 1.0e-10);
2078 Assertions.assertEquals(0.0, normalized2.getI().subtract(withDerivatives.getI()).getReal(), 1.0e-10);
2079 Assertions.assertEquals(-MathUtils.TWO_PI, normalized2.getPerigeeArgument().subtract(withDerivatives.getPerigeeArgument()).getReal(), 1.0e-10);
2080 Assertions.assertEquals(+MathUtils.TWO_PI, normalized2.getRightAscensionOfAscendingNode().subtract(withDerivatives.getRightAscensionOfAscendingNode()).getReal(), 1.0e-10);
2081 Assertions.assertEquals(2 * MathUtils.TWO_PI, normalized2.getTrueAnomaly().subtract(withDerivatives.getTrueAnomaly()).getReal(), 1.0e-10);
2082 Assertions.assertEquals(0.0, normalized2.getADot().subtract(withDerivatives.getADot()).getReal(), 1.0e-6);
2083 Assertions.assertEquals(0.0, normalized2.getEDot().subtract(withDerivatives.getEDot()).getReal(), 1.0e-10);
2084 Assertions.assertEquals(0.0, normalized2.getIDot().subtract(withDerivatives.getIDot()).getReal(), 1.0e-10);
2085 Assertions.assertEquals(0.0, normalized2.getPerigeeArgumentDot().subtract(withDerivatives.getPerigeeArgumentDot()).getReal(), 1.0e-10);
2086 Assertions.assertEquals(0.0, normalized2.getRightAscensionOfAscendingNodeDot().subtract(withDerivatives.getRightAscensionOfAscendingNodeDot()).getReal(), 1.0e-10);
2087 Assertions.assertEquals(0.0, normalized2.getTrueAnomalyDot().subtract(withDerivatives.getTrueAnomalyDot()).getReal(), 1.0e-10);
2088
2089 }
2090
2091 }