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