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