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