1   /* Copyright 2002-2025 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.orbits;
18  
19  import org.hamcrest.MatcherAssert;
20  import org.hipparchus.CalculusFieldElement;
21  import org.hipparchus.Field;
22  import org.hipparchus.analysis.UnivariateFunction;
23  import org.hipparchus.analysis.differentiation.DSFactory;
24  import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
25  import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
26  import org.hipparchus.complex.Complex;
27  import org.hipparchus.complex.ComplexField;
28  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
29  import org.hipparchus.geometry.euclidean.threed.Vector3D;
30  import org.hipparchus.linear.FieldMatrixPreservingVisitor;
31  import org.hipparchus.linear.MatrixUtils;
32  import org.hipparchus.util.Binary64;
33  import org.hipparchus.util.Binary64Field;
34  import org.hipparchus.util.FastMath;
35  import org.hipparchus.util.MathArrays;
36  import org.hipparchus.util.MathUtils;
37  import org.junit.jupiter.api.Assertions;
38  import org.junit.jupiter.api.BeforeEach;
39  import org.junit.jupiter.api.Test;
40  import org.junit.jupiter.params.ParameterizedTest;
41  import org.junit.jupiter.params.provider.EnumSource;
42  import org.orekit.Utils;
43  import org.orekit.errors.OrekitIllegalArgumentException;
44  import org.orekit.errors.OrekitMessages;
45  import org.orekit.frames.Frame;
46  import org.orekit.frames.FramesFactory;
47  import org.orekit.frames.Transform;
48  import org.orekit.time.AbsoluteDate;
49  import org.orekit.time.FieldAbsoluteDate;
50  import org.orekit.time.TimeScalesFactory;
51  import org.orekit.utils.Constants;
52  import org.orekit.utils.FieldPVCoordinates;
53  import org.orekit.utils.PVCoordinates;
54  import org.orekit.utils.TimeStampedFieldPVCoordinates;
55  
56  import java.util.function.Function;
57  
58  import static org.orekit.OrekitMatchers.relativelyCloseTo;
59  
60  
61  class FieldEquinoctialOrbitTest {
62  
63      // Body mu
64      private double mu;
65  
66      @BeforeEach
67      public void setUp() {
68  
69          Utils.setDataRoot("regular-data");
70  
71          // Body mu
72          mu = 3.9860047e14;
73      }
74  
75      @ParameterizedTest
76      @EnumSource(PositionAngleType.class)
77      void testWithCachedPositionAngleType(final PositionAngleType positionAngleType) {
78          // GIVEN
79          final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
80          final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
81          final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
82          final double muEarth = 3.9860047e14;
83          final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), AbsoluteDate.ARBITRARY_EPOCH, muEarth);
84          final EquinoctialOrbit equinoctialOrbit = new EquinoctialOrbit(cartesianOrbit);
85          final Binary64Field field = Binary64Field.getInstance();
86          final FieldEquinoctialOrbit<Binary64> fieldOrbit = new FieldEquinoctialOrbit<>(field, equinoctialOrbit);
87          // WHEN
88          final FieldEquinoctialOrbit<Binary64> orbit = fieldOrbit.withCachedPositionAngleType(positionAngleType);
89          // THEN
90          Assertions.assertEquals(fieldOrbit.getFrame(), orbit.getFrame());
91          Assertions.assertEquals(fieldOrbit.getDate(), orbit.getDate());
92          Assertions.assertEquals(fieldOrbit.getMu(), orbit.getMu());
93          final Vector3D relativePosition = fieldOrbit.getPosition(orbit.getFrame()).subtract(
94                  orbit.getPosition()).toVector3D();
95          Assertions.assertEquals(0., relativePosition.getNorm(), 1e-6);
96          Assertions.assertEquals(fieldOrbit.hasNonKeplerianAcceleration(),
97                  orbit.hasNonKeplerianAcceleration());
98      }
99  
100     @Test
101     void testNonKeplerianAcceleration() {
102         // GIVEN
103         final PVCoordinates pvCoordinates = new PVCoordinates(new Vector3D(1, 2, 3),
104                 Vector3D.MINUS_K.scalarMultiply(0.1), Vector3D.MINUS_I);
105         final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(),
106                 AbsoluteDate.ARBITRARY_EPOCH, 1.);
107         final EquinoctialOrbit equinoctialOrbit = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(cartesianOrbit);
108         final Binary64Field field = Binary64Field.getInstance();
109         final FieldCartesianOrbit<Binary64> fieldCartesianOrbit = new FieldCartesianOrbit<>(field, cartesianOrbit);
110         final FieldEquinoctialOrbit<Binary64> fieldEquinoctialOrbit = new FieldEquinoctialOrbit<>(field, equinoctialOrbit);
111         // WHEN
112         final FieldVector3D<Binary64> nonKeplerianAcceleration = fieldEquinoctialOrbit.nonKeplerianAcceleration();
113         // THEN
114         final FieldVector3D<Binary64> difference = nonKeplerianAcceleration.subtract(fieldCartesianOrbit.nonKeplerianAcceleration());
115         Assertions.assertEquals(0., difference.getNorm().getReal(), 1e-10);
116     }
117 
118     @Test
119     void testInFrameNonKeplerian() {
120         testTemplateInFrame(Vector3D.MINUS_J, PositionAngleType.TRUE);
121     }
122 
123     @ParameterizedTest
124     @EnumSource(PositionAngleType.class)
125     void testInFrameKeplerian(final PositionAngleType positionAngleType) {
126         testTemplateInFrame(Vector3D.ZERO, positionAngleType);
127     }
128 
129     private void testTemplateInFrame(final Vector3D acceleration, final PositionAngleType positionAngleType) {
130         // GIVEN
131         final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
132         final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
133         final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
134         final double muEarth = 3.9860047e14;
135         final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(),
136                 AbsoluteDate.ARBITRARY_EPOCH, muEarth);
137         final FieldEquinoctialOrbit<Binary64> fieldOrbit = new FieldEquinoctialOrbit<>(Binary64Field.getInstance(),
138                 cartesianOrbit).withCachedPositionAngleType(positionAngleType);
139         // WHEN
140         final FieldEquinoctialOrbit<Binary64> fieldOrbitWithOtherFrame = fieldOrbit.inFrame(FramesFactory.getGCRF());
141         // THEN
142         Assertions.assertNotEquals(fieldOrbit.getFrame(), fieldOrbitWithOtherFrame.getFrame());
143         Assertions.assertEquals(fieldOrbit.getDate(), fieldOrbitWithOtherFrame.getDate());
144         Assertions.assertEquals(fieldOrbit.getMu(), fieldOrbitWithOtherFrame.getMu());
145         final FieldVector3D<Binary64> relativePosition = fieldOrbit.getPosition(fieldOrbitWithOtherFrame.getFrame()).subtract(
146                 fieldOrbitWithOtherFrame.getPosition());
147         Assertions.assertEquals(0., relativePosition.getNorm().getReal(), 1e-6);
148         Assertions.assertEquals(fieldOrbit.hasNonKeplerianAcceleration(),
149                 fieldOrbitWithOtherFrame.hasNonKeplerianAcceleration());
150     }
151 
152     @Test
153     void testEquinoctialToEquinoctialEll() {
154         doTestEquinoctialToEquinoctialEll(Binary64Field.getInstance());
155     }
156 
157     @Test
158     void testEquinoctialToEquinoctialCirc() {
159         doTestEquinoctialToEquinoctialCirc(Binary64Field.getInstance());
160     }
161 
162     @Test
163     void testEquinoctialToCartesian() {
164         doTestEquinoctialToCartesian(Binary64Field.getInstance());
165     }
166 
167     @Test
168     void testEquinoctialToKeplerian() {
169         doTestEquinoctialToKeplerian(Binary64Field.getInstance());
170     }
171 
172     @Test
173     void testNumericalIssue25() {
174         doTestNumericalIssue25(Binary64Field.getInstance());
175     }
176 
177     @Test
178     void testAnomaly() {
179         doTestAnomaly(Binary64Field.getInstance());
180     }
181 
182     @Test
183     void testPositionVelocityNorms() {
184         doTestPositionVelocityNorms(Binary64Field.getInstance());
185     }
186 
187     @Test
188     void testGeometry() {
189         doTestGeometry(Binary64Field.getInstance());
190     }
191 
192     @Test
193     void testRadiusOfCurvature() {
194         doTestRadiusOfCurvature(Binary64Field.getInstance());
195     }
196 
197     @Test
198     void testSymmetry() {
199         doTestSymmetry(Binary64Field.getInstance());
200     }
201 
202     @Test
203     void testJacobianReference() {
204         doTestJacobianReference(Binary64Field.getInstance());
205     }
206 
207     @Test
208     void testJacobianFinitedifferences() {
209         doTestJacobianFinitedifferences(Binary64Field.getInstance());
210     }
211 
212     @Test
213     void testHyperbolic1() {
214         doTestHyperbolic1(Binary64Field.getInstance());
215     }
216 
217     @Test
218     void testHyperbolic2() {
219         doTestHyperbolic2(Binary64Field.getInstance());
220     }
221 
222     @Test
223     void testToOrbitWithoutDerivatives() {
224         doTestToOrbitWithoutDerivatives(Binary64Field.getInstance());
225     }
226 
227     @Test
228     void testToOrbitWithDerivatives() {
229         doTestToOrbitWithDerivatives(Binary64Field.getInstance());
230     }
231 
232     @Test
233     void testDerivativesConversionSymmetry() {
234         doTestDerivativesConversionSymmetry(Binary64Field.getInstance());
235     }
236 
237     @Test
238     void testToString() {
239         doTestToString(Binary64Field.getInstance());
240     }
241 
242     @Test
243     void testNonInertialFrame() {
244         Assertions.assertThrows(IllegalArgumentException.class,
245                                 () -> doTestNonInertialFrame(Binary64Field.getInstance()));
246     }
247 
248     @Test
249     void testNonKeplerianDerivatives() {
250         doTestNonKeplerianDerivatives(Binary64Field.getInstance());
251     }
252 
253     @Test
254     void testPositionAngleDerivatives() {
255         doTestPositionAngleDerivatives(Binary64Field.getInstance());
256     }
257 
258     @Test
259     void testEquatorialRetrograde() {
260         doTestEquatorialRetrograde(Binary64Field.getInstance());
261     }
262 
263     @Test
264     void testCopyNonKeplerianAcceleration() {
265         doTestCopyNonKeplerianAcceleration(Binary64Field.getInstance());
266     }
267 
268     @Test
269     void testNormalize() {
270         doTestNormalize(Binary64Field.getInstance());
271     }
272 
273     @Test
274     void testWithKeplerianRates() {
275         // GIVEN
276         final ComplexField field = ComplexField.getInstance();
277         final EquinoctialOrbit expectedOrbit = createOrbitTestFromEquinoctialOrbit(true);
278         final FieldEquinoctialOrbit<Complex> fieldOrbit = new FieldEquinoctialOrbit<>(field, expectedOrbit);
279         // WHEN
280         final FieldEquinoctialOrbit<Complex> actualFieldOrbit = fieldOrbit.withKeplerianRates();
281         // THEN
282         Assertions.assertFalse(actualFieldOrbit.hasNonKeplerianRates());
283         Assertions.assertEquals(fieldOrbit.getMu(), actualFieldOrbit.getMu());
284         Assertions.assertEquals(fieldOrbit.getDate(), actualFieldOrbit.getDate());
285         Assertions.assertEquals(fieldOrbit.getFrame(), actualFieldOrbit.getFrame());
286         Assertions.assertEquals(fieldOrbit.getPosition(), actualFieldOrbit.getPosition());
287     }
288 
289     @Test
290     void testFromEquinoctialOrbitWithDerivatives() {
291         // GIVEN
292         final ComplexField field = ComplexField.getInstance();
293         final EquinoctialOrbit expectedOrbit = createOrbitTestFromEquinoctialOrbit(true);
294         // WHEN
295         final FieldEquinoctialOrbit<Complex> fieldOrbit = new FieldEquinoctialOrbit<>(field, expectedOrbit);
296         // THEN
297         compareFieldOrbitToOrbit(fieldOrbit, expectedOrbit);
298     }
299 
300     @Test
301     void testFromEquinoctialOrbitWithoutDerivatives() {
302         // GIVEN
303         final ComplexField field = ComplexField.getInstance();
304         final EquinoctialOrbit expectedOrbit = createOrbitTestFromEquinoctialOrbit(false);
305         // WHEN
306         final FieldEquinoctialOrbit<Complex> fieldOrbit = new FieldEquinoctialOrbit<>(field, expectedOrbit);
307         // THEN
308         compareFieldOrbitToOrbit(fieldOrbit, expectedOrbit);
309     }
310 
311     private EquinoctialOrbit createOrbitTestFromEquinoctialOrbit(final boolean withDerivatives) {
312         final PositionAngleType positionAngleType = PositionAngleType.TRUE;
313         final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
314         final Frame frame = FramesFactory.getEME2000();
315         final double a = 10000.e3;
316         final double ex = 1e-4;
317         final double ey = 1e-3;
318         final double hx = 1.e-2;
319         final double hy = -2.e-3;
320         final double lv = 0.5;
321         if (withDerivatives) {
322             final double derivative = 0.;
323             return new EquinoctialOrbit(a, ex, ey, hx, hy, lv, derivative, derivative, derivative, derivative,
324                     derivative, derivative, positionAngleType, frame, date, mu);
325         } else {
326             return new EquinoctialOrbit(a, ex, ey, hx, hy, lv, positionAngleType, frame, date, mu);
327         }
328     }
329 
330     @Test
331     void testCoverageCachedPositionAngleTypeWithRates() {
332         // GIVEN
333         final double semiMajorAxis = 1e4;
334         final double ex = 0.;
335         final double ey = 0.;
336         final double expectedL = 0.;
337         final double expectedLDot = 0.;
338         final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
339         final Binary64Field field = Binary64Field.getInstance();
340         final Binary64 zero = field.getZero();
341         // WHEN & THEN
342         for (final PositionAngleType inputPositionAngleType: PositionAngleType.values()) {
343             for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
344                 final FieldEquinoctialOrbit<Binary64> fieldOrbit = new FieldEquinoctialOrbit<>(
345                         zero.newInstance(semiMajorAxis), zero.newInstance(ex), zero.newInstance(ey), zero, zero,
346                         zero.newInstance(expectedL), zero, zero, zero, zero, zero, zero.newInstance(expectedLDot),
347                         inputPositionAngleType, cachedPositionAngleType,
348                         FramesFactory.getGCRF(), new FieldAbsoluteDate<>(field, date), zero.newInstance(mu));
349                 Assertions.assertEquals(cachedPositionAngleType, fieldOrbit.getCachedPositionAngleType());
350                 Assertions.assertEquals(expectedL, fieldOrbit.getLv().getReal());
351                 Assertions.assertEquals(expectedL, fieldOrbit.getLM().getReal());
352                 Assertions.assertEquals(expectedL, fieldOrbit.getLE().getReal());
353                 Assertions.assertEquals(expectedLDot, fieldOrbit.getLvDot().getReal());
354                 Assertions.assertEquals(expectedLDot, fieldOrbit.getLMDot().getReal());
355                 Assertions.assertEquals(expectedLDot, fieldOrbit.getLEDot().getReal());
356             }
357         }
358     }
359 
360     private <T extends CalculusFieldElement<T>> void compareFieldOrbitToOrbit(final FieldEquinoctialOrbit<T> fieldOrbit,
361                                                                               final EquinoctialOrbit orbit) {
362         Assertions.assertEquals(orbit.getFrame(), fieldOrbit.getFrame());
363         Assertions.assertEquals(orbit.getMu(), fieldOrbit.getMu().getReal());
364         Assertions.assertEquals(orbit.getDate(), fieldOrbit.getDate().toAbsoluteDate());
365         Assertions.assertEquals(orbit.getA(), fieldOrbit.getA().getReal());
366         Assertions.assertEquals(orbit.getEquinoctialEx(), fieldOrbit.getEquinoctialEx().getReal());
367         Assertions.assertEquals(orbit.getEquinoctialEy(), fieldOrbit.getEquinoctialEy().getReal());
368         Assertions.assertEquals(orbit.getHx(), fieldOrbit.getHx().getReal());
369         Assertions.assertEquals(orbit.getHy(), fieldOrbit.getHy().getReal());
370         Assertions.assertEquals(orbit.getLv(), fieldOrbit.getLv().getReal());
371         Assertions.assertEquals(orbit.hasNonKeplerianAcceleration(), fieldOrbit.hasNonKeplerianAcceleration());
372         Assertions.assertEquals(orbit.getADot(), fieldOrbit.getADot().getReal());
373         Assertions.assertEquals(orbit.getEquinoctialExDot(), fieldOrbit.getEquinoctialExDot().getReal());
374         Assertions.assertEquals(orbit.getEquinoctialEyDot(), fieldOrbit.getEquinoctialEyDot().getReal());
375         Assertions.assertEquals(orbit.getHxDot(), fieldOrbit.getHxDot().getReal());
376         Assertions.assertEquals(orbit.getHyDot(), fieldOrbit.getHyDot().getReal());
377         Assertions.assertEquals(orbit.getLvDot(), fieldOrbit.getLvDot().getReal());
378 
379     }
380 
381     @Test
382     void testGetLVersusDouble() {
383         // GIVEN
384         final double semiMajorAxis = 1e7;
385         final double ex = 1e-2;
386         final double ey = 1e-3;
387         final double expectedL = 2;
388         final AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
389         final Binary64Field field = Binary64Field.getInstance();
390         final Binary64 zero = field.getZero();
391         // WHEN & THEN
392         for (final PositionAngleType inputPositionAngleType: PositionAngleType.values()) {
393             for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
394                 final FieldEquinoctialOrbit<Binary64> fieldOrbit = new FieldEquinoctialOrbit<>(
395                         zero.newInstance(semiMajorAxis), zero.newInstance(ex), zero.newInstance(ey), zero, zero,
396                         zero.newInstance(expectedL), inputPositionAngleType, cachedPositionAngleType,
397                         FramesFactory.getGCRF(), new FieldAbsoluteDate<>(field, date), zero.newInstance(mu));
398                 final EquinoctialOrbit equinoctialOrbit = fieldOrbit.toOrbit();
399                 Assertions.assertEquals(equinoctialOrbit.getLE(), fieldOrbit.getLE().getReal());
400                 Assertions.assertEquals(equinoctialOrbit.getLv(), fieldOrbit.getLv().getReal());
401                 Assertions.assertEquals(equinoctialOrbit.getLM(), fieldOrbit.getLM().getReal());
402             }
403         }
404     }
405 
406     private <T extends CalculusFieldElement<T>> void doTestEquinoctialToEquinoctialEll(Field<T> field) {
407         T zero = field.getZero();
408         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
409 
410         T ix = zero.add(1.200e-04);
411         T iy = zero.add(-1.16e-04);
412         T inc = ix.multiply(ix).add(iy.multiply(iy)).divide(4.).sqrt().asin().multiply(2);
413         T hx = inc.divide(2.).tan().multiply(ix).divide(inc.divide(2.).sin().multiply(2));
414         T hy = inc.divide(2.).tan().multiply(iy).divide(inc.divide(2.).sin().multiply(2));
415 
416         // elliptic orbit
417         FieldEquinoctialOrbit<T> equi =
418             new FieldEquinoctialOrbit<>(zero.add(42166712.0), zero.add( 0.5), zero.add(-0.5), hx, hy,
419                                         zero.add(5.300), PositionAngleType.MEAN,
420                                         FramesFactory.getEME2000(), date, zero.add(mu));
421         FieldVector3D<T> pos = equi.getPosition();
422         FieldVector3D<T> vit = equi.getVelocity();
423 
424         FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>(pos, vit);
425 
426         FieldEquinoctialOrbit<T> param = new FieldEquinoctialOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(), date, zero.add(mu));
427         Assertions.assertEquals(param.getA().getReal(), equi.getA().getReal(), Utils.epsilonTest * equi.getA().getReal());
428         Assertions.assertEquals(param.getEquinoctialEx().getReal(), equi.getEquinoctialEx().getReal(),
429                      Utils.epsilonE * FastMath.abs(equi.getE().getReal()));
430         Assertions.assertEquals(param.getEquinoctialEy().getReal(), equi.getEquinoctialEy().getReal(),
431                      Utils.epsilonE * FastMath.abs(equi.getE().getReal()));
432         Assertions.assertEquals(param.getHx().getReal(), equi.getHx().getReal(), Utils.epsilonAngle
433                      * FastMath.abs(equi.getI().getReal()));
434         Assertions.assertEquals(param.getHy().getReal(), equi.getHy().getReal(), Utils.epsilonAngle
435                      * FastMath.abs(equi.getI().getReal()));
436         Assertions.assertEquals(MathUtils.normalizeAngle(param.getLv().getReal(), equi.getLv().getReal()), equi.getLv().getReal(),
437             Utils.epsilonAngle * FastMath.abs(equi.getLv().getReal()));
438 
439     }
440 
441     private <T extends CalculusFieldElement<T>> void doTestEquinoctialToEquinoctialCirc(Field<T> field) {
442         T zero = field.getZero();
443         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
444 
445         T ix = zero.add(1.200e-04);
446         T iy = zero.add(-1.16e-04);
447         T inc = ix.multiply(ix).add(iy.multiply(iy)).divide(4.).sqrt().asin().multiply(2);
448         T hx = inc.divide(2.).tan().multiply(ix).divide(inc.divide(2.).sin().multiply(2));
449         T hy = inc.divide(2.).tan().multiply(iy).divide(inc.divide(2.).sin().multiply(2));
450 
451         // circular orbit
452         FieldEquinoctialOrbit<T> equiCir =
453             new FieldEquinoctialOrbit<>(zero.add(42166712.0), zero.add(0.1e-10), zero.add(-0.1e-10), hx, hy,
454                                         zero.add(5.300), PositionAngleType.MEAN,
455                                         FramesFactory.getEME2000(), date, zero.add(mu));
456         FieldVector3D<T> posCir = equiCir.getPosition();
457         FieldVector3D<T> vitCir = equiCir.getVelocity();
458 
459         FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>(posCir, vitCir);
460 
461         FieldEquinoctialOrbit<T> paramCir = new FieldEquinoctialOrbit<>(FieldPVCoordinates, FramesFactory.getEME2000(),
462                                                          date, zero.add(mu));
463         Assertions.assertEquals(paramCir.getA().getReal(), equiCir.getA().getReal(), Utils.epsilonTest
464                      * equiCir.getA().getReal());
465         Assertions.assertEquals(paramCir.getEquinoctialEx().getReal(), equiCir.getEquinoctialEx().getReal(),
466                      Utils.epsilonEcir * FastMath.abs(equiCir.getE().getReal()));
467         Assertions.assertEquals(paramCir.getEquinoctialEy().getReal(), equiCir.getEquinoctialEy().getReal(),
468                      Utils.epsilonEcir * FastMath.abs(equiCir.getE().getReal()));
469         Assertions.assertEquals(paramCir.getHx().getReal(), equiCir.getHx().getReal(), Utils.epsilonAngle
470                      * FastMath.abs(equiCir.getI().getReal()));
471         Assertions.assertEquals(paramCir.getHy().getReal(), equiCir.getHy().getReal(), Utils.epsilonAngle
472                      * FastMath.abs(equiCir.getI().getReal()));
473         Assertions.assertEquals(MathUtils.normalizeAngle(paramCir.getLv().getReal(), equiCir.getLv().getReal()), equiCir
474                      .getLv().getReal(), Utils.epsilonAngle * FastMath.abs(equiCir.getLv().getReal()));
475 
476     }
477 
478     private <T extends CalculusFieldElement<T>> void doTestEquinoctialToCartesian(Field<T> field) {
479         T zero = field.getZero();
480         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
481 
482         T ix = zero.add(1.200e-04);
483         T iy = zero.add(-1.16e-04);
484         T inc = ix.multiply(ix).add(iy.multiply(iy)).divide(4.).sqrt().asin().multiply(2);
485         T hx = inc.divide(2.).tan().multiply(ix).divide(inc.divide(2.).sin().multiply(2));
486         T hy = inc.divide(2.).tan().multiply(iy).divide(inc.divide(2.).sin().multiply(2));
487 
488         FieldEquinoctialOrbit<T> equi =
489                         new FieldEquinoctialOrbit<>(zero.add(42166712.0), zero.add(-7.900e-06), zero.add(1.100e-04), hx, hy,
490                                                     zero.add(5.300), PositionAngleType.MEAN,
491                                                    FramesFactory.getEME2000(), date, zero.add(mu));
492         FieldVector3D<T> pos = equi.getPosition();
493         FieldVector3D<T> vit = equi.getVelocity();
494 
495         // verif of 1/a = 2/X - V2/mu
496         T oneovera = (pos.getNorm().reciprocal().multiply(2)).subtract(vit.getNorm().multiply(vit.getNorm()).divide(mu));
497         Assertions.assertEquals(oneovera.getReal(), 1. / equi.getA().getReal(), 1.0e-7);
498 
499         Assertions.assertEquals(0.233745668678733e+08, pos.getX().getReal(), Utils.epsilonTest
500                      * FastMath.abs(pos.getX().getReal()));
501         Assertions.assertEquals(-0.350998914352669e+08, pos.getY().getReal(), Utils.epsilonTest
502                      * FastMath.abs(pos.getY().getReal()));
503         Assertions.assertEquals(-0.150053723123334e+04, pos.getZ().getReal(), Utils.epsilonTest
504                      * FastMath.abs(pos.getZ().getReal()));
505 
506         Assertions.assertEquals(2558.7096558809967, vit.getX().getReal(), Utils.epsilonTest
507                      * FastMath.abs(vit.getX().getReal()));
508         Assertions.assertEquals(1704.1586039092576, vit.getY().getReal(), Utils.epsilonTest
509                      * FastMath.abs(vit.getY().getReal()));
510         Assertions.assertEquals(0.5013093577879, vit.getZ().getReal(), Utils.epsilonTest
511                      * FastMath.abs(vit.getZ().getReal()));
512     }
513 
514     private <T extends CalculusFieldElement<T>> void doTestEquinoctialToKeplerian(Field<T> field) {
515         T zero = field.getZero();
516         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
517 
518         T ix = zero.add(1.200e-04);
519         T iy = zero.add(-1.16e-04);
520         T inc = ix.multiply(ix).add(iy.multiply(iy)).divide(4.).sqrt().asin().multiply(2);
521         T hx = inc.divide(2.).tan().multiply(ix).divide(inc.divide(2.).sin().multiply(2));
522         T hy = inc.divide(2.).tan().multiply(iy).divide(inc.divide(2.).sin().multiply(2));
523 
524         FieldEquinoctialOrbit<T> equi =
525                         new FieldEquinoctialOrbit<>(zero.add(42166712.0), zero.add(-7.900e-06), zero.add(1.100e-04), hx, hy,
526                                                     zero.add(5.300), PositionAngleType.MEAN,
527                                                     FramesFactory.getEME2000(), date, zero.add(mu));
528 
529         FieldKeplerianOrbit<T> kep = new FieldKeplerianOrbit<>(equi);
530 
531         Assertions.assertEquals(42166712.000, equi.getA().getReal(), Utils.epsilonTest * kep.getA().getReal());
532         Assertions.assertEquals(0.110283316961361e-03, kep.getE().getReal(), Utils.epsilonE
533                      * FastMath.abs(kep.getE().getReal()));
534         Assertions.assertEquals(0.166901168553917e-03, kep.getI().getReal(), Utils.epsilonAngle
535                      * FastMath.abs(kep.getI().getReal()));
536         Assertions.assertEquals(MathUtils.normalizeAngle(-3.87224326008837, kep.getPerigeeArgument().getReal()),
537                      kep.getPerigeeArgument().getReal(), Utils.epsilonTest
538                      * FastMath.abs(kep.getPerigeeArgument().getReal()));
539         Assertions.assertEquals(MathUtils.normalizeAngle(5.51473467358854, kep
540                                      .getRightAscensionOfAscendingNode().getReal()), kep
541                                      .getRightAscensionOfAscendingNode().getReal(), Utils.epsilonTest
542                                      * FastMath.abs(kep.getRightAscensionOfAscendingNode().getReal()));
543         Assertions.assertEquals(MathUtils.normalizeAngle(3.65750858649982, kep.getMeanAnomaly().getReal()), kep
544                      .getMeanAnomaly().getReal(), Utils.epsilonTest * FastMath.abs(kep.getMeanAnomaly().getReal()));
545 
546     }
547 
548     private <T extends CalculusFieldElement<T>> void doTestHyperbolic1(Field<T> field) {
549         try {
550             T zero = field.getZero();
551             FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
552             new FieldEquinoctialOrbit<>(zero.add(-42166712.0), zero.add(1.9), zero.add(0.5), zero.add(0.01), zero.add(-0.02), zero.add(5.300),
553                                         PositionAngleType.MEAN,  FramesFactory.getEME2000(), date, zero.add(mu));
554             Assertions.fail("an exception should have been thrown");
555         } catch (OrekitIllegalArgumentException oe) {
556             Assertions.assertEquals(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, oe.getSpecifier());
557         }
558     }
559 
560     private <T extends CalculusFieldElement<T>> void doTestHyperbolic2(Field<T> field) {
561             T zero = field.getZero();
562             FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
563             FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(-42166712.0), zero.add(1.9), zero.add(0.5),
564                                                             zero.add(0.01), zero.add(-0.02), zero.add(5.300),
565                                                             PositionAngleType.MEAN,  FramesFactory.getEME2000(), date, zero.add(mu));
566         try {
567             new FieldEquinoctialOrbit<>(orbit.getPVCoordinates(), orbit.getFrame(), orbit.getMu());
568             Assertions.fail("an exception should have been thrown");
569         } catch (OrekitIllegalArgumentException oe) {
570             Assertions.assertEquals(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, oe.getSpecifier());
571         }
572     }
573 
574     private <T extends CalculusFieldElement<T>> void doTestToOrbitWithoutDerivatives(Field<T> field) {
575         T zero =  field.getZero();
576         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
577 
578         FieldVector3D<T> position = new FieldVector3D<>(zero.add(7.0e6), zero.add(1.0e6), zero.add(4.0e6));
579         FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-500.0), zero.add(8000.0), zero.add(1000.0));
580         FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity);
581         FieldEquinoctialOrbit<T>  fieldOrbit = new FieldEquinoctialOrbit<>(pvCoordinates, FramesFactory.getEME2000(), date, zero.add(mu));
582         EquinoctialOrbit orbit = fieldOrbit.toOrbit();
583         Assertions.assertFalse(orbit.hasNonKeplerianAcceleration());
584         MatcherAssert.assertThat(orbit.getA(),                             relativelyCloseTo(fieldOrbit.getA().getReal(),             0));
585         MatcherAssert.assertThat(orbit.getEquinoctialEx(),                 relativelyCloseTo(fieldOrbit.getEquinoctialEx().getReal(), 0));
586         MatcherAssert.assertThat(orbit.getEquinoctialEy(),                 relativelyCloseTo(fieldOrbit.getEquinoctialEy().getReal(), 0));
587         MatcherAssert.assertThat(orbit.getHx(),                            relativelyCloseTo(fieldOrbit.getHx().getReal(),            0));
588         MatcherAssert.assertThat(orbit.getHy(),                            relativelyCloseTo(fieldOrbit.getHy().getReal(),            0));
589         MatcherAssert.assertThat(orbit.getLv(),                            relativelyCloseTo(fieldOrbit.getLv().getReal(),            0));
590     }
591 
592     private <T extends CalculusFieldElement<T>> void doTestToOrbitWithDerivatives(Field<T> field) {
593         T zero =  field.getZero();
594         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
595 
596         FieldVector3D<T> position = new FieldVector3D<>(zero.add(7.0e6), zero.add(1.0e6), zero.add(4.0e6));
597         FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-500.0), zero.add(8000.0), zero.add(1000.0));
598         T r2 = position.getNormSq();
599         T r = r2.sqrt();
600         final FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(zero.add(mu).negate()),
601                 position);
602         final FieldVector3D<T> nonKeplerianAcceleration = keplerianAcceleration.scalarMultiply(1.1);
603         final FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity, nonKeplerianAcceleration);
604         final FieldEquinoctialOrbit<T>  fieldOrbit = new FieldEquinoctialOrbit<>(pvCoordinates, FramesFactory.getEME2000(), date, zero.add(mu));
605         EquinoctialOrbit orbit = fieldOrbit.toOrbit();
606         Assertions.assertTrue(orbit.hasNonKeplerianAcceleration());
607         MatcherAssert.assertThat(orbit.getA(),                             relativelyCloseTo(fieldOrbit.getA().getReal(),                           0));
608         MatcherAssert.assertThat(orbit.getEquinoctialEx(),                 relativelyCloseTo(fieldOrbit.getEquinoctialEx().getReal(),               0));
609         MatcherAssert.assertThat(orbit.getEquinoctialEy(),                 relativelyCloseTo(fieldOrbit.getEquinoctialEy().getReal(),               0));
610         MatcherAssert.assertThat(orbit.getHx(),                            relativelyCloseTo(fieldOrbit.getHx().getReal(),                          0));
611         MatcherAssert.assertThat(orbit.getHy(),                            relativelyCloseTo(fieldOrbit.getHy().getReal(),                          0));
612         MatcherAssert.assertThat(orbit.getLv(),                            relativelyCloseTo(fieldOrbit.getLv().getReal(),                          0));
613         MatcherAssert.assertThat(orbit.getADot(),                          relativelyCloseTo(fieldOrbit.getADot().getReal(),                        0));
614         MatcherAssert.assertThat(orbit.getEquinoctialExDot(),              relativelyCloseTo(fieldOrbit.getEquinoctialExDot().getReal(),            0));
615         MatcherAssert.assertThat(orbit.getEquinoctialEyDot(),              relativelyCloseTo(fieldOrbit.getEquinoctialEyDot().getReal(),            0));
616         MatcherAssert.assertThat(orbit.getHxDot(),                         relativelyCloseTo(fieldOrbit.getHxDot().getReal(),                       0));
617         MatcherAssert.assertThat(orbit.getHyDot(),                         relativelyCloseTo(fieldOrbit.getHyDot().getReal(),                       0));
618         MatcherAssert.assertThat(orbit.getLvDot(),                         relativelyCloseTo(fieldOrbit.getLvDot().getReal(),                       0));
619         MatcherAssert.assertThat(orbit.getLEDot(),                         relativelyCloseTo(fieldOrbit.getLEDot().getReal(),                       0));
620         MatcherAssert.assertThat(orbit.getLMDot(),                         relativelyCloseTo(fieldOrbit.getLMDot().getReal(),                       0));
621         MatcherAssert.assertThat(orbit.getEDot(),                          relativelyCloseTo(fieldOrbit.getEDot().getReal(),                        0));
622         MatcherAssert.assertThat(orbit.getIDot(),                          relativelyCloseTo(fieldOrbit.getIDot().getReal(),                        0));
623         MatcherAssert.assertThat(orbit.getLDot(PositionAngleType.TRUE),        relativelyCloseTo(fieldOrbit.getLDot(PositionAngleType.TRUE).getReal(),      0));
624         MatcherAssert.assertThat(orbit.getLDot(PositionAngleType.ECCENTRIC),   relativelyCloseTo(fieldOrbit.getLDot(PositionAngleType.ECCENTRIC).getReal(), 0));
625         MatcherAssert.assertThat(orbit.getLDot(PositionAngleType.MEAN),        relativelyCloseTo(fieldOrbit.getLDot(PositionAngleType.MEAN).getReal(),      0));
626     }
627 
628     private <T extends CalculusFieldElement<T>> void doTestNumericalIssue25(Field<T> field) {
629         T zero = field.getZero();
630         FieldVector3D<T> position = new FieldVector3D<>(zero.add(3782116.14107698), zero.add(416663.11924914), zero.add(5875541.62103057));
631         FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-6349.7848910501), zero.add(288.4061811651), zero.add(4066.9366759691));
632         FieldEquinoctialOrbit<T> orbit = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(position, velocity),
633                                                                      FramesFactory.getEME2000(),
634                                                                      new FieldAbsoluteDate<>(field, "2004-01-01T23:00:00.000",
635                                                                                              TimeScalesFactory.getUTC()),
636                                                                      zero.add(3.986004415E14));
637         Assertions.assertEquals(0.0, orbit.getE().getReal(), 2.0e-14);
638     }
639 
640     private <T extends CalculusFieldElement<T>> void doTestAnomaly(Field<T> field) {
641 
642         T one = field.getOne();
643         T zero = field.getZero();
644         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
645         // elliptic orbit
646         FieldVector3D<T> position = new FieldVector3D<>(zero.add(7.0e6), zero.add(1.0e6), zero.add(4.0e6));
647         FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-500.0), zero.add(8000.0), zero.add(1000.0));
648 
649         FieldEquinoctialOrbit<T> p = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(position, velocity), FramesFactory.getEME2000(), date, zero.add(mu));
650         FieldKeplerianOrbit<T> kep = new FieldKeplerianOrbit<>(p);
651 
652         T e = p.getE();
653         T eRatio = one.subtract(e).divide(e.add(1)).sqrt();
654         T paPraan = kep.getPerigeeArgument().add(
655                        kep.getRightAscensionOfAscendingNode());
656 
657         T lv = zero.add(1.1);
658         // formulations for elliptic case
659         T lE = eRatio.multiply(lv.subtract(paPraan).divide(2).tan()).atan().multiply(2).add(paPraan);
660         T lM = lE.subtract(e.multiply(lE.subtract(paPraan).sin()));
661 
662         p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
663                                         p.getEquinoctialEy() , p.getHx(), p.getHy() , lv , PositionAngleType.TRUE,
664                                         p.getFrame(), p.getDate(), p.getMu());
665         Assertions.assertEquals(p.getLv().getReal(), lv.getReal(), Utils.epsilonAngle * FastMath.abs(lv.getReal()));
666         Assertions.assertEquals(p.getLE().getReal(), lE.getReal(), Utils.epsilonAngle * FastMath.abs(lE.getReal()));
667         Assertions.assertEquals(p.getLM().getReal(), lM.getReal(), Utils.epsilonAngle * FastMath.abs(lM.getReal()));
668         p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
669                                         p.getEquinoctialEy() , p.getHx(), p.getHy() , zero , PositionAngleType.TRUE,
670                                         p.getFrame(), p.getDate(), p.getMu());
671 
672         p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
673                                         p.getEquinoctialEy() , p.getHx(), p.getHy() , lE , PositionAngleType.ECCENTRIC,
674                                         p.getFrame(), p.getDate(), p.getMu());
675         Assertions.assertEquals(p.getLv().getReal(), lv.getReal(), Utils.epsilonAngle * FastMath.abs(lv.getReal()));
676         Assertions.assertEquals(p.getLE().getReal(), lE.getReal(), Utils.epsilonAngle * FastMath.abs(lE.getReal()));
677         Assertions.assertEquals(p.getLM().getReal(), lM.getReal(), Utils.epsilonAngle * FastMath.abs(lM.getReal()));
678         p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
679                                         p.getEquinoctialEy() , p.getHx(), p.getHy() , zero , PositionAngleType.TRUE,
680                                         p.getFrame(), p.getDate(), p.getMu());
681 
682         p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
683                                         p.getEquinoctialEy() , p.getHx(), p.getHy() , lM , PositionAngleType.MEAN,
684                                       p.getFrame(), p.getDate(), p.getMu());
685         Assertions.assertEquals(p.getLv  ().getReal(), lv.getReal(), Utils.epsilonAngle * FastMath.abs(lv.getReal()));
686         Assertions.assertEquals(p.getLE().getReal(), lE.getReal(), Utils.epsilonAngle * FastMath.abs(lE.getReal()));
687         Assertions.assertEquals(p.getLM().getReal(), lM.getReal(), Utils.epsilonAngle * FastMath.abs(lM.getReal()));
688 
689         // circular orbit
690         p = new FieldEquinoctialOrbit<>(p.getA(), zero ,
691                                         zero, p.getHx(), p.getHy() , p.getLv() , PositionAngleType.TRUE,
692                                         p.getFrame(), p.getDate(), p.getMu());
693 
694         lE = lv;
695         lM = lE;
696 
697         p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
698                                         p.getEquinoctialEy() , p.getHx(), p.getHy() , lv , PositionAngleType.TRUE,
699                                         p.getFrame(), p.getDate(), p.getMu());
700         Assertions.assertEquals(p.getLv().getReal(), lv.getReal(), Utils.epsilonAngle * FastMath.abs(lv.getReal()));
701         Assertions.assertEquals(p.getLE().getReal(), lE.getReal(), Utils.epsilonAngle * FastMath.abs(lE.getReal()));
702         Assertions.assertEquals(p.getLM().getReal(), lM.getReal(), Utils.epsilonAngle * FastMath.abs(lM.getReal()));
703         p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
704                                         p.getEquinoctialEy() , p.getHx(), p.getHy() , zero , PositionAngleType.TRUE,
705                                         p.getFrame(), p.getDate(), p.getMu());
706 
707         p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
708                                         p.getEquinoctialEy() , p.getHx(), p.getHy() , lE , PositionAngleType.ECCENTRIC,
709                                         p.getFrame(), p.getDate(), p.getMu());
710         Assertions.assertEquals(p.getLv().getReal(), lv.getReal(), Utils.epsilonAngle * FastMath.abs(lv.getReal()));
711         Assertions.assertEquals(p.getLE().getReal(), lE.getReal(), Utils.epsilonAngle * FastMath.abs(lE.getReal()));
712         Assertions.assertEquals(p.getLM().getReal(), lM.getReal(), Utils.epsilonAngle * FastMath.abs(lM.getReal()));
713         p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
714                                         p.getEquinoctialEy() , p.getHx(), p.getHy() , zero , PositionAngleType.TRUE,
715                                         p.getFrame(), p.getDate(), p.getMu());
716 
717         p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
718                                         p.getEquinoctialEy() , p.getHx(), p.getHy() , lM , PositionAngleType.MEAN, p.getFrame(), p.getDate(), p.getMu());
719         Assertions.assertEquals(p.getLv().getReal(), lv.getReal(), Utils.epsilonAngle * FastMath.abs(lv.getReal()));
720         Assertions.assertEquals(p.getLE().getReal(), lE.getReal(), Utils.epsilonAngle * FastMath.abs(lE.getReal()));
721         Assertions.assertEquals(p.getLM().getReal(), lM.getReal(), Utils.epsilonAngle * FastMath.abs(lM.getReal()));
722     }
723 
724     private <T extends CalculusFieldElement<T>> void doTestPositionVelocityNorms(Field<T> field) {
725 
726         T zero = field.getZero();
727         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
728 
729         // elliptic and non equatorial (i retrograde) orbit
730         FieldEquinoctialOrbit<T> p =
731             new FieldEquinoctialOrbit<>(zero.add(42166712.0), zero.add(0.5), zero.add(-0.5), zero.add(1.200), zero.add(2.1),
732                                         zero.add(0.67), PositionAngleType.TRUE,
733                                         FramesFactory.getEME2000(), date, zero.add(mu));
734 
735         T ex = p.getEquinoctialEx();
736         T ey = p.getEquinoctialEy();
737         T lv = p.getLv();
738         T ksi = ex.multiply(lv.cos()).add(ey.multiply(lv.sin())).add(1);
739         T nu = ex.multiply(lv.sin()).subtract(ey.multiply(lv.cos()));
740         T epsilon = ex.negate().multiply(ex).subtract(ey.multiply(ey)).add(1.0).sqrt();
741 
742         T a = p.getA();
743         T na = a.reciprocal().multiply(p.getMu()).sqrt();
744 
745         Assertions.assertEquals(a.getReal() * epsilon.getReal() * epsilon.getReal() / ksi.getReal(),
746                             p.getPosition().getNorm().getReal(),
747                      Utils.epsilonTest * FastMath.abs(p.getPosition().getNorm().getReal()));
748         Assertions.assertEquals(na.getReal() * FastMath.sqrt(ksi.getReal() * ksi.getReal() + nu.getReal() * nu.getReal()) / epsilon.getReal(),
749                             p.getVelocity().getNorm().getReal(),
750                             Utils.epsilonTest * FastMath.abs(p.getVelocity().getNorm().getReal()));
751 
752         // circular and equatorial orbit
753         FieldEquinoctialOrbit<T> pCirEqua =
754             new FieldEquinoctialOrbit<>(zero.add(42166712.0), zero.add(0.1e-8), zero.add(0.1e-8), zero.add(0.1e-8), zero.add(0.1e-8),
755                                         zero.add(0.67), PositionAngleType.TRUE,
756                                         FramesFactory.getEME2000(), date, zero.add(mu));
757 
758         ex = pCirEqua.getEquinoctialEx();
759         ey = pCirEqua.getEquinoctialEy();
760         lv = pCirEqua.getLv();
761         ksi = ex.multiply(lv.cos()).add(ey.multiply(lv.sin())).add(1);
762         nu = ex.multiply(lv.sin()).subtract(ey.multiply(lv.cos()));
763         epsilon = ex.negate().multiply(ex).subtract(ey.multiply(ey)).add(1.0).sqrt();
764 
765         a = pCirEqua.getA();
766         na = a.reciprocal().multiply(p.getMu()).sqrt();
767 
768         Assertions.assertEquals(a.getReal() * epsilon.getReal() * epsilon.getReal() / ksi.getReal(), pCirEqua.getPosition()
769                             .getNorm().getReal(), Utils.epsilonTest
770                             * FastMath.abs(pCirEqua.getPosition().getNorm().getReal()));
771         Assertions.assertEquals(na.getReal() * FastMath.sqrt(ksi.getReal() * ksi.getReal() + nu.getReal() * nu.getReal()) / epsilon.getReal(),
772                             pCirEqua.getVelocity().getNorm().getReal(), Utils.epsilonTest
773                             * FastMath.abs(pCirEqua.getVelocity().getNorm().getReal()));
774     }
775 
776     private <T extends CalculusFieldElement<T>> void doTestGeometry(Field<T> field) {
777 
778 
779         T one = field.getOne();
780         T zero = field.getZero();
781         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
782 
783 
784         // elliptic and non equatorial (i retrograde) orbit
785         FieldEquinoctialOrbit<T> p =
786             new FieldEquinoctialOrbit<>(zero.add(42166712.0), zero.add(0.5), zero.add(-0.5), zero.add(1.200), zero.add(2.1),
787                                         zero.add(0.67), PositionAngleType.TRUE,
788                                         FramesFactory.getEME2000(), date, zero.add(mu));
789 
790         FieldVector3D<T> position = p.getPosition();
791         FieldVector3D<T> velocity = p.getVelocity();
792         FieldVector3D<T> momentum = p.getPVCoordinates().getMomentum().normalize();
793 
794         T apogeeRadius = p.getA().multiply(p.getE().add(1.0));
795         T perigeeRadius = p.getA().multiply(one.subtract(p.getE()));
796 
797         for (T lv = zero; lv.getReal() <= 2 * FastMath.PI; lv = lv.add(2 * FastMath.PI / 100.)) {
798             p = new FieldEquinoctialOrbit<>(p.getA(), p.getEquinoctialEx(),
799                                             p.getEquinoctialEy() , p.getHx(), p.getHy() , lv , PositionAngleType.TRUE,
800                                             p.getFrame(), p.getDate(), p.getMu());
801             position = p.getPosition();
802 
803             // test if the norm of the position is in the range [perigee radius,
804             // apogee radius]
805             // Warning: these tests are without absolute value by choice
806             Assertions.assertTrue((position.getNorm().getReal() - apogeeRadius.getReal()) <= (apogeeRadius.getReal() * Utils.epsilonTest));
807             Assertions.assertTrue((position.getNorm().getReal() - perigeeRadius.getReal()) >= (-perigeeRadius.getReal() * Utils.epsilonTest));
808 
809             position = position.normalize();
810             velocity = p.getVelocity();
811             velocity = velocity.normalize();
812 
813             // at this stage of computation, all the vectors (position, velocity and
814             // momemtum) are normalized here
815 
816             // test of orthogonality between position and momentum
817             Assertions.assertTrue(FastMath.abs(FieldVector3D.dotProduct(position, momentum).getReal()) < Utils.epsilonTest);
818             // test of orthogonality between velocity and momentum
819             Assertions.assertTrue(FastMath.abs(FieldVector3D.dotProduct(velocity, momentum).getReal()) < Utils.epsilonTest);
820         }
821 
822 
823         // circular and equatorial orbit
824         FieldEquinoctialOrbit<T> pCirEqua =
825             new FieldEquinoctialOrbit<>(zero.add(42166712.0), zero.add(0.1e-8), zero.add(0.1e-8), zero.add(0.1e-8), zero.add(0.1e-8),
826                                         zero.add(0.67), PositionAngleType.TRUE,
827                                         FramesFactory.getEME2000(), date, zero.add(mu));
828 
829         position = pCirEqua.getPosition();
830         velocity = pCirEqua.getVelocity();
831 
832         momentum = FieldVector3D.crossProduct(position, velocity).normalize();
833 
834         apogeeRadius = pCirEqua.getA().multiply(pCirEqua.getE().add(1));
835         perigeeRadius = pCirEqua.getA().multiply(one.subtract(pCirEqua.getE()));
836         // test if apogee equals perigee
837         Assertions.assertEquals(perigeeRadius.getReal(), apogeeRadius.getReal(), 1.e+4 * Utils.epsilonTest
838                      * apogeeRadius.getReal());
839 
840         for (T lv = zero; lv.getReal() <= 2 * FastMath.PI; lv =lv.add(2 * FastMath.PI / 100.)) {
841             pCirEqua = new FieldEquinoctialOrbit<>(pCirEqua.getA(), pCirEqua.getEquinoctialEx(),
842                                                    pCirEqua.getEquinoctialEy() , pCirEqua.getHx(), pCirEqua.getHy() , lv , PositionAngleType.TRUE,
843                                                    pCirEqua.getFrame(), p.getDate(), p.getMu());
844             position = pCirEqua.getPosition();
845 
846             // test if the norm pf the position is in the range [perigee radius,
847             // apogee radius]
848             Assertions.assertTrue((position.getNorm().getReal() - apogeeRadius.getReal()) <= (apogeeRadius.getReal() * Utils.epsilonTest));
849             Assertions.assertTrue((position.getNorm().getReal() - perigeeRadius.getReal()) >= (-perigeeRadius.getReal() * Utils.epsilonTest));
850 
851             position = position.normalize();
852             velocity = pCirEqua.getVelocity();
853             velocity = velocity.normalize();
854 
855             // at this stage of computation, all the vectors (position, velocity and
856             // momemtum) are normalized here
857 
858             // test of orthogonality between position and momentum
859             Assertions.assertTrue(FastMath.abs(FieldVector3D.dotProduct(position, momentum).getReal()) < Utils.epsilonTest);
860             // test of orthogonality between velocity and momentum
861             Assertions.assertTrue(FastMath.abs(FieldVector3D.dotProduct(velocity, momentum).getReal()) < Utils.epsilonTest);
862         }
863     }
864 
865     private <T extends CalculusFieldElement<T>> void doTestRadiusOfCurvature(Field<T> field) {
866         T one = field.getOne();
867         T zero = field.getZero();
868         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
869 
870         // elliptic and non equatorial (i retrograde) orbit
871         FieldEquinoctialOrbit<T> p =
872             new FieldEquinoctialOrbit<>(zero.add(42166712.0), zero.add(0.5), zero.add(-0.5), zero.add(1.200), zero.add(2.1),
873                                         zero.add(0.67), PositionAngleType.TRUE,
874                                         FramesFactory.getEME2000(), date, zero.add(mu));
875 
876         // arbitrary orthogonal vectors in the orbital plane
877         FieldVector3D<T> u = p.getPVCoordinates().getMomentum().orthogonal();
878         FieldVector3D<T> v = FieldVector3D.crossProduct(p.getPVCoordinates().getMomentum(), u).normalize();
879 
880         // compute radius of curvature in the orbital plane from Cartesian coordinates
881         T xDot    = FieldVector3D.dotProduct(p.getVelocity(),     u);
882         T yDot    = FieldVector3D.dotProduct(p.getVelocity(),     v);
883         T xDotDot = FieldVector3D.dotProduct(p.getPVCoordinates().getAcceleration(), u);
884         T yDotDot = FieldVector3D.dotProduct(p.getPVCoordinates().getAcceleration(), v);
885         T dot2    = xDot.multiply(xDot).add(yDot.multiply(yDot));
886         T rCart   = dot2.multiply(dot2.sqrt()).divide(
887                          xDot.multiply(yDotDot).subtract(yDot.multiply(xDotDot).abs()));
888 
889         // compute radius of curvature in the orbital plane from orbital parameters
890         T ex   = p.getEquinoctialEx();
891         T ey   = p.getEquinoctialEy();
892         T f    = ex.multiply(p.getLE().cos()).add( ey.multiply(p.getLE().sin()));
893 
894         T oMf2 = one.subtract(f.multiply(f));
895         T rOrb = p.getA().multiply(oMf2).multiply(oMf2.divide(
896                                          one.subtract(ex.multiply(ex).add(ey.multiply(ey)))).sqrt());
897 
898         // both methods to compute radius of curvature should match
899         Assertions.assertEquals(rCart.getReal(), rOrb.getReal(), 2.0e-15 * p.getA().getReal());
900 
901         // at this place for such an eccentric orbit,
902         // the radius of curvature is much smaller than semi major axis
903         Assertions.assertEquals(0.8477 * p.getA().getReal(), rCart.getReal(), 1.0e-4 * p.getA().getReal());
904 
905     }
906 
907     private <T extends CalculusFieldElement<T>> void doTestSymmetry(Field<T> field) {
908         T zero = field.getZero();
909         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
910 
911         // elliptic and non equatorial orbit
912         FieldVector3D<T> position = new FieldVector3D<>(zero.add(4512.9), zero.add(18260.), zero.add( -5127.));
913         FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(134664.6), zero.add(90066.8), zero.add(72047.6));
914 
915         FieldEquinoctialOrbit<T> p = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(position, velocity),
916                                                                  FramesFactory.getEME2000(), date, zero.add(mu));
917 
918         FieldVector3D<T> positionOffset = p.getPosition().subtract(position);
919         FieldVector3D<T> velocityOffset = p.getVelocity().subtract(velocity);
920 
921         Assertions.assertEquals(0, positionOffset.getNorm().getReal(), 7.5e-12);
922         Assertions.assertEquals(0, velocityOffset.getNorm().getReal(), 1.0e-15);
923 
924         // circular and equatorial orbit
925         position = new FieldVector3D<>(zero.add(33051.2), zero.add(26184.9), zero.add(-1.3E-5));
926         velocity = new FieldVector3D<>(zero.add(-60376.2), zero.add(76208.), zero.add(2.7E-4));
927 
928         p = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(position, velocity),
929                                         FramesFactory.getEME2000(), date, zero.add(mu));
930 
931         positionOffset = p.getPosition().subtract(position);
932         velocityOffset = p.getVelocity().subtract(velocity);
933 
934         Assertions.assertEquals(0, positionOffset.getNorm().getReal(), 1.1e-11);
935         Assertions.assertEquals(0, velocityOffset.getNorm().getReal(), 1.0e-15);
936     }
937 
938     private <T extends CalculusFieldElement<T>> void doTestNonInertialFrame(Field<T> field) throws IllegalArgumentException {
939 
940         T zero = field.getZero();
941         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field);
942 
943         FieldVector3D<T> position = new FieldVector3D<>(zero.add(4512.9), zero.add(18260.), zero.add(-5127.));
944         FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(134664.6), zero.add(90066.8), zero.add(72047.6));
945         FieldPVCoordinates<T> FieldPVCoordinates = new FieldPVCoordinates<>( position, velocity);
946         new FieldEquinoctialOrbit<>(FieldPVCoordinates,
947                                     new Frame(FramesFactory.getEME2000(), Transform.IDENTITY, "non-inertial", false),
948                                     date, zero.add(mu));
949     }
950 
951     private <T extends CalculusFieldElement<T>> void doTestJacobianReference(Field<T> field) {
952 
953         T zero = field.getZero();
954         FieldAbsoluteDate<T> dateTca = new FieldAbsoluteDate<>(field, 2000, 4, 1, 0, 0, 0.000, TimeScalesFactory.getUTC());
955         double mu =  3.986004415e+14;
956         FieldEquinoctialOrbit<T> orbEqu = new FieldEquinoctialOrbit<>(zero.add(7000000.0), zero.add(0.01), zero.add(-0.02), zero.add(1.2), zero.add(2.1),
957                                                                       zero.add(FastMath.toRadians(40.)), PositionAngleType.MEAN,
958                                                                       FramesFactory.getEME2000(), dateTca, zero.add(mu));
959 
960         // the following reference values have been computed using the free software
961         // version 6.2 of the MSLIB fortran library by the following program:
962         //         program equ_jacobian
963         //
964         //         use mslib
965         //         implicit none
966         //
967         //         integer, parameter :: nb = 11
968         //         integer :: i,j
969         //         type(tm_code_retour)      ::  code_retour
970         //
971         //         real(pm_reel), parameter :: mu= 3.986004415e+14_pm_reel
972         //         real(pm_reel),dimension(3)::vit_car,pos_car
973         //         type(tm_orb_cir_equa)::cir_equa
974         //         real(pm_reel), dimension(6,6)::jacob
975         //         real(pm_reel)::norme,hx,hy,f,dix,diy
976         //         intrinsic sqrt
977         //
978         //         cir_equa%a=7000000_pm_reel
979         //         cir_equa%ex=0.01_pm_reel
980         //         cir_equa%ey=-0.02_pm_reel
981         //
982         //         ! mslib cir-equ parameters use ix = 2 sin(i/2) cos(gom) and iy = 2 sin(i/2) sin(gom)
983         //         ! equinoctial parameters use hx = tan(i/2) cos(gom) and hy = tan(i/2) sin(gom)
984         //         ! the conversions between these parameters and their differentials can be computed
985         //         ! from the ratio f = 2cos(i/2) which can be found either from (ix, iy) or (hx, hy):
986         //         !   f = sqrt(4 - ix^2 - iy^2) =  2 / sqrt(1 + hx^2 + hy^2)
987         //         !  hx = ix / f,  hy = iy / f
988         //         !  ix = hx * f, iy = hy *f
989         //         ! dhx = ((1 + hx^2) / f) dix + (hx hy / f) diy, dhy = (hx hy / f) dix + ((1 + hy^2) /f) diy
990         //         ! dix = ((1 - ix^2 / 4) f dhx - (ix iy / 4) f dhy, diy = -(ix iy / 4) f dhx + (1 - iy^2 / 4) f dhy
991         //         hx=1.2_pm_reel
992         //         hy=2.1_pm_reel
993         //         f=2_pm_reel/sqrt(1+hx*hx+hy*hy)
994         //         cir_equa%ix=hx*f
995         //         cir_equa%iy=hy*f
996         //
997         //         cir_equa%pso_M=40_pm_reel*pm_deg_rad
998         //
999         //         call mv_cir_equa_car(mu,cir_equa,pos_car,vit_car,code_retour)
1000         //         write(*,*)code_retour%valeur
1001         //         write(*,1000)pos_car,vit_car
1002         //
1003         //
1004         //         call mu_norme(pos_car,norme,code_retour)
1005         //         write(*,*)norme
1006         //
1007         //         call mv_car_cir_equa (mu, pos_car, vit_car, cir_equa, code_retour, jacob)
1008         //         write(*,*)code_retour%valeur
1009         //
1010         //         f=sqrt(4_pm_reel-cir_equa%ix*cir_equa%ix-cir_equa%iy*cir_equa%iy)
1011         //         hx=cir_equa%ix/f
1012         //         hy=cir_equa%iy/f
1013         //         write(*,*)"ix = ", cir_equa%ix, ", iy = ", cir_equa%iy
1014         //         write(*,*)"equinoctial = ", cir_equa%a, cir_equa%ex, cir_equa%ey, hx, hy, cir_equa%pso_M*pm_rad_deg
1015         //
1016         //         do j = 1,6
1017         //           dix=jacob(4,j)
1018         //           diy=jacob(5,j)
1019         //           jacob(4,j)=((1_pm_reel+hx*hx)*dix+(hx*hy)*diy)/f
1020         //           jacob(5,j)=((hx*hy)*dix+(1_pm_reel+hy*hy)*diy)/f
1021         //         end do
1022         //
1023         //         do i = 1,6
1024         //            write(*,*) " ",(jacob(i,j),j=1,6)
1025         //         end do
1026         //
1027         //         1000 format (6(f24.15,1x))
1028         //         end program equ_jacobian
1029         FieldVector3D<T> pRef = new FieldVector3D<>(zero.add(2004367.298657628707588), zero.add(6575317.978060320019722), zero.add(-1518024.843913963763043));
1030         FieldVector3D<T> vRef = new FieldVector3D<>(zero.add(5574.048661495634406), zero.add(-368.839015744295409), zero.add(5009.529487849066754));
1031         double[][] jRefR = {
1032             {  0.56305379787310628,        1.8470954710993663,      -0.42643364527246025,        1370.4369387322224,       -90.682848736736688 ,       1231.6441195141242      },
1033             {  9.52434720041122055E-008,  9.49704503778007296E-008,  4.46607520107935678E-008,  1.69704446323098610E-004,  7.05603505855828105E-005,  1.14825140460141970E-004 },
1034             { -5.41784097802642701E-008,  9.54903765833015538E-008, -8.95815777332234450E-008,  1.01864980963344096E-004, -1.03194262242761416E-004,  1.40668700715197768E-004 },
1035             {  1.96680305426455816E-007, -1.12388745957974467E-007, -2.27118924123407353E-007,  2.06472886488132167E-004, -1.17984506564646906E-004, -2.38427023682723818E-004 },
1036             { -2.24382495052235118E-007,  1.28218568601277626E-007,  2.59108357381747656E-007,  1.89034327703662092E-004, -1.08019615830663994E-004, -2.18289640324466583E-004 },
1037             { -3.04001022071876804E-007,  1.22214683774559989E-007,  1.35141804810132761E-007, -1.34034616931480536E-004, -2.14283975204169379E-004,  1.29018773893081404E-004 }
1038         };
1039         T[][] jRef = MathArrays.buildArray(field, 6, 6);
1040         for (int ii = 0; ii<6; ii++){
1041             for (int jj = 0; jj< 6; jj++){
1042                 jRef[ii][jj] = zero.add(jRefR[ii][jj]);
1043             }
1044         }
1045         FieldPVCoordinates<T> pv = orbEqu.getPVCoordinates();
1046         Assertions.assertEquals(0, pv.getPosition().subtract(pRef).getNorm().getReal(), 2.0e-16 * pRef.getNorm().getReal());
1047         Assertions.assertEquals(0, pv.getVelocity().subtract(vRef).getNorm().getReal(), 2.0e-16 * vRef.getNorm().getReal());
1048 
1049         T[][] jacobian = MathArrays.buildArray(field, 6, 6);
1050         orbEqu.getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
1051 
1052 
1053         for (int i = 0; i < jacobian.length; i++) {
1054             T[] row    = jacobian[i];
1055             T[] rowRef = jRef[i];
1056             for (int j = 0; j < row.length; j++) {
1057                 Assertions.assertEquals(0, (row[j].getReal() - rowRef[j].getReal()) / rowRef[j].getReal(), 4.0e-15);
1058             }
1059         }
1060 
1061     }
1062 
1063     private <T extends CalculusFieldElement<T>> void doTestJacobianFinitedifferences(Field<T> field) {
1064 
1065         T zero = field.getZero();
1066         FieldAbsoluteDate<T> dateTca = new FieldAbsoluteDate<>(field, 2000, 4, 1, 0, 0, 0.000, TimeScalesFactory.getUTC());
1067         double mu =  3.986004415e+14;
1068         FieldEquinoctialOrbit<T> orbEqu = new FieldEquinoctialOrbit<>(zero.add(7000000.0), zero.add(0.01), zero.add(-0.02), zero.add(1.2), zero.add(2.1),
1069                                                                       zero.add(FastMath.toRadians(40.)), PositionAngleType.MEAN,
1070                                                                       FramesFactory.getEME2000(), dateTca, zero.add(mu));
1071 
1072         for (PositionAngleType type : PositionAngleType.values()) {
1073             T hP = zero.add(2.0);
1074             T[][] finiteDiffJacobian = finiteDifferencesJacobian(type, orbEqu, hP);
1075             T[][] jacobian = MathArrays.buildArray(field, 6, 6);
1076             orbEqu.getJacobianWrtCartesian(type, jacobian);
1077 
1078             for (int i = 0; i < jacobian.length; i++) {
1079                 T[] row    = jacobian[i];
1080                 T[] rowRef = finiteDiffJacobian[i];
1081                 for (int j = 0; j < row.length; j++) {
1082                     Assertions.assertEquals(0, (row[j].getReal() - rowRef[j].getReal()) / rowRef[j].getReal(), 8.0e-9);
1083                 }
1084             }
1085 
1086             T[][] invJacobian = MathArrays.buildArray(field, 6, 6);
1087             orbEqu.getJacobianWrtParameters(type, invJacobian);
1088             MatrixUtils.createFieldMatrix(jacobian).
1089                             multiply(MatrixUtils.createFieldMatrix(invJacobian)).
1090             walkInRowOrder(new FieldMatrixPreservingVisitor<T>() {
1091                 public void start(int rows, int columns,
1092                                   int startRow, int endRow, int startColumn, int endColumn) {
1093                 }
1094 
1095                 public void visit(int row, int column, T value) {
1096                     Assertions.assertEquals(row == column ? 1.0 : 0.0, value.getReal(), 1.4e-9);
1097                 }
1098 
1099                 public T end() {
1100                     return null;
1101                 }
1102             });
1103 
1104         }
1105 
1106     }
1107 
1108     private <T extends CalculusFieldElement<T>>  T[][] finiteDifferencesJacobian(PositionAngleType type, FieldEquinoctialOrbit<T> orbit, T hP)
1109         {
1110         Field<T> field = hP.getField();
1111         T[][] jacobian = MathArrays.buildArray(field, 6, 6);
1112         for (int i = 0; i < 6; ++i) {
1113             fillColumn(type, i, orbit, hP, jacobian);
1114         }
1115         return jacobian;
1116     }
1117 
1118     private <T extends CalculusFieldElement<T>> void fillColumn(PositionAngleType type, int i, FieldEquinoctialOrbit<T> orbit, T hP, T[][] jacobian) {
1119         T zero = hP.getField().getZero();
1120         // at constant energy (i.e. constant semi major axis), we have dV = -mu dP / (V * r^2)
1121         // we use this to compute a velocity step size from the position step size
1122         FieldVector3D<T> p = orbit.getPosition();
1123         FieldVector3D<T> v = orbit.getVelocity();
1124         T hV = hP.multiply(orbit.getMu()).divide(v.getNorm().multiply(p.getNormSq()));
1125 
1126         T h;
1127         FieldVector3D<T> dP = new FieldVector3D<>(hP.getField().getZero(), hP.getField().getZero(), hP.getField().getZero());
1128         FieldVector3D<T> dV = new FieldVector3D<>(hP.getField().getZero(), hP.getField().getZero(), hP.getField().getZero());
1129         switch (i) {
1130         case 0:
1131             h = hP;
1132             dP = new FieldVector3D<>(hP, zero, zero);
1133             break;
1134         case 1:
1135             h = hP;
1136             dP = new FieldVector3D<>(zero, hP, zero);
1137             break;
1138         case 2:
1139             h = hP;
1140             dP = new FieldVector3D<>(zero, zero, hP);
1141             break;
1142         case 3:
1143             h = hV;
1144             dV = new FieldVector3D<>(hV, zero, zero);
1145             break;
1146         case 4:
1147             h = hV;
1148             dV = new FieldVector3D<>(zero, hV, zero);
1149             break;
1150         default:
1151             h = hV;
1152             dV = new FieldVector3D<>(zero, zero, hV);
1153             break;
1154         }
1155 
1156         FieldEquinoctialOrbit<T> oM4h = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, -4, dP), new FieldVector3D<>(1, v, -4, dV)),
1157                                                                     orbit.getFrame(), orbit.getDate(), orbit.getMu());
1158         FieldEquinoctialOrbit<T> oM3h = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, -3, dP), new FieldVector3D<>(1, v, -3, dV)),
1159                                                                     orbit.getFrame(), orbit.getDate(), orbit.getMu());
1160         FieldEquinoctialOrbit<T> oM2h = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, -2, dP), new FieldVector3D<>(1, v, -2, dV)),
1161                                                                     orbit.getFrame(), orbit.getDate(), orbit.getMu());
1162         FieldEquinoctialOrbit<T> oM1h = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, -1, dP), new FieldVector3D<>(1, v, -1, dV)),
1163                                                                     orbit.getFrame(), orbit.getDate(), orbit.getMu());
1164         FieldEquinoctialOrbit<T> oP1h = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, +1, dP), new FieldVector3D<>(1, v, +1, dV)),
1165                                                                     orbit.getFrame(), orbit.getDate(), orbit.getMu());
1166         FieldEquinoctialOrbit<T> oP2h = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, +2, dP), new FieldVector3D<>(1, v, +2, dV)),
1167                                                                     orbit.getFrame(), orbit.getDate(), orbit.getMu());
1168         FieldEquinoctialOrbit<T> oP3h = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, +3, dP), new FieldVector3D<>(1, v, +3, dV)),
1169                                                                     orbit.getFrame(), orbit.getDate(), orbit.getMu());
1170         FieldEquinoctialOrbit<T> oP4h = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(new FieldVector3D<>(1, p, +4, dP), new FieldVector3D<>(1, v, +4, dV)),
1171                                                                     orbit.getFrame(), orbit.getDate(), orbit.getMu());
1172 
1173         jacobian[0][i] =(zero.add( -3).multiply(oP4h.getA()            .subtract(oM4h.getA())).add(
1174                          zero.add( 32).multiply(oP3h.getA()            .subtract(oM3h.getA()))).subtract(
1175                          zero.add(168).multiply(oP2h.getA()            .subtract(oM2h.getA()))).add(
1176                          zero.add(672).multiply(oP1h.getA()            .subtract(oM1h.getA())))).divide(h.multiply(840));
1177         jacobian[1][i] =(zero.add( -3).multiply(oP4h.getEquinoctialEx().subtract(oM4h.getEquinoctialEx())).add(
1178                          zero.add( 32).multiply(oP3h.getEquinoctialEx().subtract(oM3h.getEquinoctialEx()))).subtract(
1179                          zero.add(168).multiply(oP2h.getEquinoctialEx().subtract(oM2h.getEquinoctialEx()))).add(
1180                          zero.add(672).multiply(oP1h.getEquinoctialEx().subtract(oM1h.getEquinoctialEx())))).divide(h.multiply(840));
1181         jacobian[2][i] =(zero.add( -3).multiply(oP4h.getEquinoctialEy().subtract(oM4h.getEquinoctialEy())).add(
1182                          zero.add( 32).multiply(oP3h.getEquinoctialEy().subtract(oM3h.getEquinoctialEy()))).subtract(
1183                          zero.add(168).multiply(oP2h.getEquinoctialEy().subtract(oM2h.getEquinoctialEy()))).add(
1184                          zero.add(672).multiply(oP1h.getEquinoctialEy().subtract(oM1h.getEquinoctialEy())))).divide(h.multiply(840));
1185         jacobian[3][i] =(zero.add( -3).multiply(oP4h.getHx()           .subtract(oM4h.getHx())).add(
1186                          zero.add( 32).multiply(oP3h.getHx()           .subtract(oM3h.getHx()))).subtract(
1187                          zero.add(168).multiply(oP2h.getHx()           .subtract(oM2h.getHx()))).add(
1188                          zero.add(672).multiply(oP1h.getHx()           .subtract(oM1h.getHx())))).divide(h.multiply(840));
1189         jacobian[4][i] =(zero.add( -3).multiply(oP4h.getHy()           .subtract(oM4h.getHy())).add(
1190                          zero.add( 32).multiply(oP3h.getHy()           .subtract(oM3h.getHy()))).subtract(
1191                          zero.add(168).multiply(oP2h.getHy()           .subtract(oM2h.getHy()))).add(
1192                          zero.add(672).multiply(oP1h.getHy()           .subtract(oM1h.getHy())))).divide(h.multiply(840));
1193         jacobian[5][i] =(zero.add( -3).multiply(oP4h.getL(type)        .subtract(oM4h.getL(type))).add(
1194                          zero.add( 32).multiply(oP3h.getL(type)        .subtract(oM3h.getL(type)))).subtract(
1195                          zero.add(168).multiply(oP2h.getL(type)        .subtract(oM2h.getL(type)))).add(
1196                          zero.add(672).multiply(oP1h.getL(type)        .subtract(oM1h.getL(type))))).divide(h.multiply(840));
1197 
1198     }
1199 
1200     private <T extends CalculusFieldElement<T>> void doTestNonKeplerianDerivatives(Field<T> field) {
1201         final T zero = field.getZero();
1202 
1203         final FieldAbsoluteDate<T> date         = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1204         final FieldVector3D<T>     position     = new FieldVector3D<>(field.getZero().add(6896874.444705),  field.getZero().add(1956581.072644),  field.getZero().add(-147476.245054));
1205         final FieldVector3D<T>     velocity     = new FieldVector3D<>(field.getZero().add(166.816407662), field.getZero().add(-1106.783301861), field.getZero().add(-7372.745712770));
1206         final FieldVector3D <T>    acceleration = new FieldVector3D<>(field.getZero().add(-7.466182457944), field.getZero().add(-2.118153357345),  field.getZero().add(0.160004048437));
1207         final TimeStampedFieldPVCoordinates<T> pv = new TimeStampedFieldPVCoordinates<>(date, position, velocity, acceleration);
1208         final Frame frame = FramesFactory.getEME2000();
1209         final T mu   = zero.add(Constants.EIGEN5C_EARTH_MU);
1210         final FieldEquinoctialOrbit<T> orbit = new FieldEquinoctialOrbit<>(pv, frame, mu);
1211 
1212         Assertions.assertEquals(differentiate(pv, frame, mu, FieldEquinoctialOrbit::getA),
1213                             orbit.getADot().getReal(),
1214                             4.3e-8);
1215         Assertions.assertEquals(differentiate(pv, frame, mu, FieldEquinoctialOrbit::getEquinoctialEx),
1216                             orbit.getEquinoctialExDot().getReal(),
1217                             2.1e-15);
1218         Assertions.assertEquals(differentiate(pv, frame, mu, FieldEquinoctialOrbit::getEquinoctialEy),
1219                             orbit.getEquinoctialEyDot().getReal(),
1220                             5.3e-16);
1221         Assertions.assertEquals(differentiate(pv, frame, mu, FieldEquinoctialOrbit::getHx),
1222                             orbit.getHxDot().getReal(),
1223                             4.4e-15);
1224         Assertions.assertEquals(differentiate(pv, frame, mu, FieldEquinoctialOrbit::getHy),
1225                             orbit.getHyDot().getReal(),
1226                             1.5e-15);
1227         Assertions.assertEquals(differentiate(pv, frame, mu, FieldEquinoctialOrbit::getLv),
1228                             orbit.getLvDot().getReal(),
1229                             1.2e-15);
1230         Assertions.assertEquals(differentiate(pv, frame, mu, FieldEquinoctialOrbit::getLE),
1231                             orbit.getLEDot().getReal(),
1232                             7.7e-16);
1233         Assertions.assertEquals(differentiate(pv, frame, mu, FieldEquinoctialOrbit::getLM),
1234                             orbit.getLMDot().getReal(),
1235                             8.8e-16);
1236         Assertions.assertEquals(differentiate(pv, frame, mu, FieldEquinoctialOrbit::getE),
1237                             orbit.getEDot().getReal(),
1238                             6.9e-16);
1239         Assertions.assertEquals(differentiate(pv, frame, mu, FieldEquinoctialOrbit::getI),
1240                             orbit.getIDot().getReal(),
1241                             3.5e-15);
1242         Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getL(PositionAngleType.TRUE)),
1243                             orbit.getLDot(PositionAngleType.TRUE).getReal(),
1244                             1.2e-15);
1245         Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getL(PositionAngleType.ECCENTRIC)),
1246                             orbit.getLDot(PositionAngleType.ECCENTRIC).getReal(),
1247                             7.7e-16);
1248         Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getL(PositionAngleType.MEAN)),
1249                             orbit.getLDot(PositionAngleType.MEAN).getReal(),
1250                             8.8e-16);
1251 
1252     }
1253 
1254     private <T extends CalculusFieldElement<T>, S extends Function<FieldEquinoctialOrbit<T>, T>>
1255     double differentiate(TimeStampedFieldPVCoordinates<T> pv, Frame frame, T mu, S picker) {
1256         final DSFactory factory = new DSFactory(1, 1);
1257         FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
1258         UnivariateDifferentiableFunction diff =
1259                 differentiator.differentiate((UnivariateFunction) dt -> picker.apply(new FieldEquinoctialOrbit<>(pv.shiftedBy(dt),
1260                                                                                                                  frame, mu)).getReal());
1261         return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
1262      }
1263 
1264     private <T extends CalculusFieldElement<T>> void doTestPositionAngleDerivatives(final Field<T> field) {
1265         final T zero = field.getZero();
1266 
1267         final FieldAbsoluteDate<T> date         = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1268         final FieldVector3D<T>     position     = new FieldVector3D<>(field.getZero().add(6896874.444705),  field.getZero().add(1956581.072644),  field.getZero().add(-147476.245054));
1269         final FieldVector3D<T>     velocity     = new FieldVector3D<>(field.getZero().add(166.816407662), field.getZero().add(-1106.783301861), field.getZero().add(-7372.745712770));
1270         final FieldVector3D <T>    acceleration = new FieldVector3D<>(field.getZero().add(-7.466182457944), field.getZero().add(-2.118153357345),  field.getZero().add(0.160004048437));
1271         final TimeStampedFieldPVCoordinates<T> pv = new TimeStampedFieldPVCoordinates<>(date, position, velocity, acceleration);
1272         final Frame frame = FramesFactory.getEME2000();
1273         final double mu   = Constants.EIGEN5C_EARTH_MU;
1274         final FieldEquinoctialOrbit<T> orbit = new FieldEquinoctialOrbit<>(pv, frame, zero.add(mu));
1275 
1276         for (PositionAngleType type : PositionAngleType.values()) {
1277             final FieldEquinoctialOrbit<T> rebuilt = new FieldEquinoctialOrbit<>(orbit.getA(),
1278                                                                                  orbit.getEquinoctialEx(),
1279                                                                                  orbit.getEquinoctialEy(),
1280                                                                                  orbit.getHx(),
1281                                                                                  orbit.getHy(),
1282                                                                                  orbit.getL(type),
1283                                                                                  orbit.getADot(),
1284                                                                                  orbit.getEquinoctialExDot(),
1285                                                                                  orbit.getEquinoctialEyDot(),
1286                                                                                  orbit.getHxDot(),
1287                                                                                  orbit.getHyDot(),
1288                                                                                  orbit.getLDot(type),
1289                                                                                  type, orbit.getFrame(), orbit.getDate(), orbit.getMu());
1290             MatcherAssert.assertThat(rebuilt.getA().getReal(),                                relativelyCloseTo(orbit.getA().getReal(),                1));
1291             MatcherAssert.assertThat(rebuilt.getEquinoctialEx().getReal(),                    relativelyCloseTo(orbit.getEquinoctialEx().getReal(),    1));
1292             MatcherAssert.assertThat(rebuilt.getEquinoctialEy().getReal(),                    relativelyCloseTo(orbit.getEquinoctialEy().getReal(),    1));
1293             MatcherAssert.assertThat(rebuilt.getHx().getReal(),                               relativelyCloseTo(orbit.getHx().getReal(),               1));
1294             MatcherAssert.assertThat(rebuilt.getHy().getReal(),                               relativelyCloseTo(orbit.getHy().getReal(),               1));
1295             MatcherAssert.assertThat(rebuilt.getADot().getReal(),                             relativelyCloseTo(orbit.getADot().getReal(),             1));
1296             MatcherAssert.assertThat(rebuilt.getEquinoctialExDot().getReal(),                 relativelyCloseTo(orbit.getEquinoctialExDot().getReal(), 1));
1297             MatcherAssert.assertThat(rebuilt.getEquinoctialEyDot().getReal(),                 relativelyCloseTo(orbit.getEquinoctialEyDot().getReal(), 1));
1298             MatcherAssert.assertThat(rebuilt.getHxDot().getReal(),                            relativelyCloseTo(orbit.getHxDot().getReal(),            1));
1299             MatcherAssert.assertThat(rebuilt.getHyDot().getReal(),                            relativelyCloseTo(orbit.getHyDot().getReal(),            1));
1300             for (PositionAngleType type2 : PositionAngleType.values()) {
1301                 MatcherAssert.assertThat(rebuilt.getL(type2).getReal(),    relativelyCloseTo(orbit.getL(type2).getReal(),    1));
1302                 MatcherAssert.assertThat(rebuilt.getLDot(type2).getReal(), relativelyCloseTo(orbit.getLDot(type2).getReal(), 1));
1303             }
1304         }
1305 
1306     }
1307 
1308     private <T extends CalculusFieldElement<T>> void doTestEquatorialRetrograde(final Field<T> field) {
1309             final T zero = field.getZero();
1310             FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(10000000.0), field.getZero(), field.getZero());
1311             FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero(), field.getZero().add(-6500.0), field.getZero());
1312             T r2 = position.getNormSq();
1313             T r  = r2.sqrt();
1314             FieldVector3D<T> acceleration = new FieldVector3D<>(r.multiply(r2.reciprocal().multiply(zero.add(mu).negate())), position,
1315                                                                 field.getOne(), new FieldVector3D<>(field.getZero().add(-0.1),
1316                                                                                                     field.getZero().add(0.2),
1317                                                                                                     field.getZero().add(0.3)));
1318             FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity, acceleration);
1319             // we use an intermediate Keplerian orbit so eccentricity can be computed
1320             // when using directly PV, eccentricity ends up in NaN, due to the way computation is organized
1321             // this is not really considered a problem as anyway retrograde equatorial cannot be fully supported
1322             FieldEquinoctialOrbit<T> orbit = new FieldEquinoctialOrbit<>(new FieldKeplerianOrbit<>(pvCoordinates,
1323                                                                                                    FramesFactory.getEME2000(),
1324                                                                                                    FieldAbsoluteDate.getJ2000Epoch(field),
1325                                                                                                    zero.add(mu)));
1326             Assertions.assertEquals(10637829.465, orbit.getA().getReal(), 1.0e-3);
1327             Assertions.assertEquals(-738.145, orbit.getADot().getReal(), 1.0e-3);
1328             Assertions.assertEquals(0.05995861, orbit.getE().getReal(), 1.0e-8);
1329             Assertions.assertEquals(-6.523e-5, orbit.getEDot().getReal(), 1.0e-8);
1330             Assertions.assertTrue(Double.isNaN(orbit.getI().getReal()));
1331             Assertions.assertTrue(Double.isNaN(orbit.getHx().getReal()));
1332             Assertions.assertTrue(Double.isNaN(orbit.getHxDot().getReal()));
1333             Assertions.assertTrue(Double.isNaN(orbit.getHy().getReal()));
1334             Assertions.assertTrue(Double.isNaN(orbit.getHyDot().getReal()));
1335 
1336     }
1337 
1338     private <T extends CalculusFieldElement<T>> void doTestDerivativesConversionSymmetry(Field<T> field) {
1339         T zero = field.getZero();
1340         final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:01:20.000", TimeScalesFactory.getUTC());
1341         FieldVector3D<T> position     = new FieldVector3D<>(zero.add(6893443.400234382),
1342                                                             zero.add(1886406.1073757345),
1343                                                             zero.add(-589265.1150359757));
1344         FieldVector3D<T> velocity     = new FieldVector3D<>(zero.add(-281.1261461082365),
1345                                                             zero.add(-1231.6165642450928),
1346                                                             zero.add(-7348.756363469432));
1347         FieldVector3D<T> acceleration = new FieldVector3D<>(zero.add(-7.460341170581685),
1348                                                             zero.add(-2.0415957334584527),
1349                                                             zero.add(0.6393322823627762));
1350         FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>( position, velocity, acceleration);
1351         FieldEquinoctialOrbit<T> orbit = new FieldEquinoctialOrbit<>(pvCoordinates, FramesFactory.getEME2000(),
1352                                                                      date, zero.add(Constants.EIGEN5C_EARTH_MU));
1353         Assertions.assertTrue(orbit.hasNonKeplerianAcceleration());
1354         T r2 = position.getNormSq();
1355         T r  = r2.sqrt();
1356         FieldVector3D<T> keplerianAcceleration = new FieldVector3D<>(r.multiply(r2).reciprocal().multiply(orbit.getMu().negate()),
1357                                                                      position);
1358         Assertions.assertEquals(0.0101, FieldVector3D.distance(keplerianAcceleration, acceleration).getReal(), 1.0e-4);
1359 
1360         for (OrbitType type : OrbitType.values()) {
1361             FieldOrbit<T> converted = type.convertType(orbit);
1362             Assertions.assertTrue(converted.hasNonKeplerianAcceleration());
1363             FieldEquinoctialOrbit<T> rebuilt = (FieldEquinoctialOrbit<T>) OrbitType.EQUINOCTIAL.convertType(converted);
1364             Assertions.assertTrue(rebuilt.hasNonKeplerianAcceleration());
1365             Assertions.assertEquals(orbit.getADot().getReal(),             rebuilt.getADot().getReal(),             3.0e-13);
1366             Assertions.assertEquals(orbit.getEquinoctialExDot().getReal(), rebuilt.getEquinoctialExDot().getReal(), 1.0e-15);
1367             Assertions.assertEquals(orbit.getEquinoctialEyDot().getReal(), rebuilt.getEquinoctialEyDot().getReal(), 1.0e-15);
1368             Assertions.assertEquals(orbit.getHxDot().getReal(),            rebuilt.getHxDot().getReal(),            1.0e-15);
1369             Assertions.assertEquals(orbit.getHyDot().getReal(),            rebuilt.getHyDot().getReal(),            1.0e-15);
1370             Assertions.assertEquals(orbit.getLvDot().getReal(),            rebuilt.getLvDot().getReal(),            1.0e-15);
1371         }
1372 
1373     }
1374 
1375     private <T extends CalculusFieldElement<T>> void doTestToString(Field<T> field) {
1376         FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(-29536113.0),
1377                                                         field.getZero().add(30329259.0),
1378                                                         field.getZero().add(-100125.0));
1379         FieldVector3D<T> velocity = new FieldVector3D<>(field.getZero().add(-2194.0),
1380                                                         field.getZero().add(-2141.0),
1381                                                         field.getZero().add(-8.0));
1382         FieldPVCoordinates<T> pvCoordinates = new FieldPVCoordinates<>(position, velocity);
1383         FieldEquinoctialOrbit<T> orbit = new FieldEquinoctialOrbit<>(pvCoordinates, FramesFactory.getEME2000(),
1384                                                                      FieldAbsoluteDate.getJ2000Epoch(field), field.getZero().add(mu));
1385         Assertions.assertEquals("equinoctial parameters: {a: 4.225517000282565E7; ex: 5.927324978565528E-4; ey: -0.002062743969643666; hx: 6.401103130239252E-5; hy: -0.0017606836670756732; lv: 134.24111947709974;}",
1386                             orbit.toString());
1387     }
1388 
1389     private <T extends CalculusFieldElement<T>> void doTestCopyNonKeplerianAcceleration(Field<T> field) {
1390 
1391         final Frame eme2000     = FramesFactory.getEME2000();
1392 
1393         // Define GEO satellite position
1394         final FieldVector3D<T> position = new FieldVector3D<>(field.getZero().add(42164140),
1395                         field.getZero(),
1396                         field.getZero());
1397         // Build PVCoodrinates starting from its position and computing the corresponding circular velocity
1398         final FieldPVCoordinates<T> pv  =
1399                         new FieldPVCoordinates<>(position,
1400                                         new FieldVector3D<>(field.getZero(),
1401                                                         position.getNorm().reciprocal().multiply(mu).sqrt(),
1402                                                         field.getZero()));
1403         // Build a KeplerianOrbit in eme2000
1404         final FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(pv, eme2000, FieldAbsoluteDate.getJ2000Epoch(field), field.getZero().add(mu));
1405 
1406         // Build another KeplerianOrbit as a copy of the first one
1407         final FieldOrbit<T> orbitCopy = new FieldKeplerianOrbit<>(orbit);
1408 
1409         // Shift the orbit of a time-interval
1410         final FieldOrbit<T> shiftedOrbit     = orbit.shiftedBy(10); // This works good
1411         final FieldOrbit<T> shiftedOrbitCopy = orbitCopy.shiftedBy(10); // This does not work
1412 
1413         Assertions.assertEquals(0.0,
1414                             FieldVector3D.distance(shiftedOrbit.getPosition(),
1415                                                    shiftedOrbitCopy.getPosition()).getReal(),
1416                             1.0e-10);
1417         Assertions.assertEquals(0.0,
1418                             FieldVector3D.distance(shiftedOrbit.getVelocity(),
1419                                                    shiftedOrbitCopy.getVelocity()).getReal(),
1420                             1.0e-10);
1421 
1422     }
1423 
1424     private <T extends CalculusFieldElement<T>> void doTestNormalize(Field<T> field) {
1425         final T zero = field.getZero();
1426         FieldEquinoctialOrbit<T> withoutDerivatives =
1427                         new FieldEquinoctialOrbit<>(zero.newInstance(42166712.0), zero.newInstance(0.005),
1428                                                     zero.newInstance(-0.025), zero.newInstance(0.17),
1429                                                     zero.newInstance(0.34), zero.newInstance(0.4), PositionAngleType.MEAN,
1430                                                     FramesFactory.getEME2000(), FieldAbsoluteDate.getJ2000Epoch(field),
1431                                                     zero.newInstance(mu));
1432         FieldEquinoctialOrbit<T> ref =
1433                         new FieldEquinoctialOrbit<>(zero.newInstance(24000000.0), zero.newInstance(-0.012),
1434                                                     zero.newInstance(0.01), zero.newInstance(0.2),
1435                                                     zero.newInstance(0.1), zero.newInstance(-6.28), PositionAngleType.MEAN,
1436                                                     FramesFactory.getEME2000(), FieldAbsoluteDate.getJ2000Epoch(field),
1437                                                     zero.newInstance(mu));
1438 
1439         FieldEquinoctialOrbit<T> normalized1 = (FieldEquinoctialOrbit<T>) OrbitType.EQUINOCTIAL.normalize(withoutDerivatives, ref);
1440         Assertions.assertFalse(normalized1.hasNonKeplerianAcceleration());
1441         Assertions.assertEquals(0.0, normalized1.getA().subtract(withoutDerivatives.getA()).getReal(),          1.0e-6);
1442         Assertions.assertEquals(0.0, normalized1.getEquinoctialEx().subtract(withoutDerivatives.getEquinoctialEx()).getReal(), 1.0e-10);
1443         Assertions.assertEquals(0.0, normalized1.getEquinoctialEy().subtract(withoutDerivatives.getEquinoctialEy()).getReal(), 1.0e-10);
1444         Assertions.assertEquals(0.0, normalized1.getHx().subtract(withoutDerivatives.getHx()).getReal(),          1.0e-10);
1445         Assertions.assertEquals(0.0, normalized1.getHy().subtract(withoutDerivatives.getHy()).getReal(), 1.0e-10);
1446         Assertions.assertEquals(-MathUtils.TWO_PI, normalized1.getLv().subtract(withoutDerivatives.getLv()).getReal(), 1.0e-10);
1447 
1448         T[] p    = MathArrays.buildArray(field, 6);
1449         T[] pDot = MathArrays.buildArray(field, 6);
1450         for (int i = 0; i < pDot.length; ++i) {
1451             pDot[i] = zero.newInstance(i);
1452         }
1453         OrbitType.EQUINOCTIAL.mapOrbitToArray(withoutDerivatives, PositionAngleType.TRUE, p, null);
1454         FieldEquinoctialOrbit<T> withDerivatives = (FieldEquinoctialOrbit<T>) OrbitType.EQUINOCTIAL.mapArrayToOrbit(p, pDot,
1455                                                                                                                     PositionAngleType.TRUE,
1456                                                                                                                     withoutDerivatives.getDate(),
1457                                                                                                                     withoutDerivatives.getMu(),
1458                                                                                                                     withoutDerivatives.getFrame());
1459         FieldEquinoctialOrbit<T> normalized2 = (FieldEquinoctialOrbit<T>) OrbitType.EQUINOCTIAL.normalize(withDerivatives, ref);
1460         Assertions.assertTrue(normalized2.hasNonKeplerianAcceleration());
1461         Assertions.assertEquals(0.0, normalized2.getA().subtract(withDerivatives.getA()).getReal(),          1.0e-6);
1462         Assertions.assertEquals(0.0, normalized2.getEquinoctialEx().subtract(withDerivatives.getEquinoctialEx()).getReal(), 1.0e-10);
1463         Assertions.assertEquals(0.0, normalized2.getEquinoctialEy().subtract(withDerivatives.getEquinoctialEy()).getReal(), 1.0e-10);
1464         Assertions.assertEquals(0.0, normalized2.getHx().subtract(withDerivatives.getHx()).getReal(),          1.0e-10);
1465         Assertions.assertEquals(0.0, normalized2.getHy().subtract(withDerivatives.getHy()).getReal(), 1.0e-10);
1466         Assertions.assertEquals(-MathUtils.TWO_PI, normalized2.getLv().subtract(withDerivatives.getLv()).getReal(), 1.0e-10);
1467         Assertions.assertEquals(0.0, normalized2.getADot().subtract(withDerivatives.getADot()).getReal(),          1.0e-6);
1468         Assertions.assertEquals(0.0, normalized2.getEquinoctialExDot().subtract(withDerivatives.getEquinoctialExDot()).getReal(), 1.0e-10);
1469         Assertions.assertEquals(0.0, normalized2.getEquinoctialEyDot().subtract(withDerivatives.getEquinoctialEyDot()).getReal(), 1.0e-10);
1470         Assertions.assertEquals(0.0, normalized2.getHxDot().subtract(withDerivatives.getHxDot()).getReal(),          1.0e-10);
1471         Assertions.assertEquals(0.0, normalized2.getHyDot().subtract(withDerivatives.getHyDot()).getReal(), 1.0e-10);
1472         Assertions.assertEquals(0.0, normalized2.getLvDot().subtract(withDerivatives.getLvDot()).getReal(), 1.0e-10);
1473 
1474     }
1475 
1476 }