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.hipparchus.analysis.UnivariateFunction;
20  import org.hipparchus.analysis.differentiation.DSFactory;
21  import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
22  import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
23  import org.hipparchus.geometry.euclidean.threed.Vector3D;
24  import org.hipparchus.linear.MatrixUtils;
25  import org.hipparchus.linear.RealMatrixPreservingVisitor;
26  import org.hipparchus.util.FastMath;
27  import org.hipparchus.util.MathUtils;
28  import org.junit.jupiter.api.AfterEach;
29  import org.junit.jupiter.api.Assertions;
30  import org.junit.jupiter.api.BeforeEach;
31  import org.junit.jupiter.api.Test;
32  import org.orekit.Utils;
33  import org.orekit.frames.Frame;
34  import org.orekit.frames.FramesFactory;
35  import org.orekit.frames.Transform;
36  import org.orekit.time.AbsoluteDate;
37  import org.orekit.time.TimeScalesFactory;
38  import org.orekit.utils.Constants;
39  import org.orekit.utils.PVCoordinates;
40  import org.orekit.utils.TimeStampedPVCoordinates;
41  
42  import java.lang.reflect.InvocationTargetException;
43  import java.lang.reflect.Method;
44  import java.util.function.Function;
45  
46  
47  public class CartesianOrbitTest {
48  
49      // Computation date
50      private AbsoluteDate date;
51  
52      // Body mu
53      private double mu;
54  
55      @Test
56      void testInFrameKeplerian() {
57          testTemplateInFrame(Vector3D.ZERO);
58      }
59  
60      @Test
61      void testInFrameNonKeplerian() {
62          testTemplateInFrame(Vector3D.PLUS_K);
63      }
64  
65      private void testTemplateInFrame(final Vector3D acceleration) {
66          // GIVEN
67          final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
68          final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
69          final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
70          final double muEarth = 3.9860047e14;
71          final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, muEarth);
72          // WHEN
73          final CartesianOrbit orbitWithOtherFrame = cartesianOrbit.inFrame(FramesFactory.getGCRF());
74          // THEN
75          Assertions.assertNotEquals(cartesianOrbit.getFrame(), orbitWithOtherFrame.getFrame());
76          Assertions.assertEquals(cartesianOrbit.getDate(), orbitWithOtherFrame.getDate());
77          Assertions.assertEquals(cartesianOrbit.getMu(), orbitWithOtherFrame.getMu());
78          Assertions.assertEquals(cartesianOrbit.getPosition(orbitWithOtherFrame.getFrame()),
79                  orbitWithOtherFrame.getPosition());
80          Assertions.assertEquals(cartesianOrbit.hasNonKeplerianAcceleration(),
81                  orbitWithOtherFrame.hasNonKeplerianAcceleration());
82      }
83  
84      @Test
85      public void testCartesianToCartesian()
86          throws NoSuchMethodException, SecurityException, IllegalAccessException, IllegalArgumentException, InvocationTargetException {
87  
88          Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
89          Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
90          PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
91          double mu = 3.9860047e14;
92  
93          CartesianOrbit p = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
94  
95          Assertions.assertEquals(p.getPosition().getX(), pvCoordinates.getPosition().getX(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getPosition().getX()));
96          Assertions.assertEquals(p.getPosition().getY(), pvCoordinates.getPosition().getY(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getPosition().getY()));
97          Assertions.assertEquals(p.getPosition().getZ(), pvCoordinates.getPosition().getZ(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getPosition().getZ()));
98          Assertions.assertEquals(p.getPVCoordinates().getVelocity().getX(), pvCoordinates.getVelocity().getX(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getVelocity().getX()));
99          Assertions.assertEquals(p.getPVCoordinates().getVelocity().getY(), pvCoordinates.getVelocity().getY(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getVelocity().getY()));
100         Assertions.assertEquals(p.getPVCoordinates().getVelocity().getZ(), pvCoordinates.getVelocity().getZ(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getVelocity().getZ()));
101 
102         Method initPV = CartesianOrbit.class.getDeclaredMethod("initPVCoordinates", new Class[0]);
103         initPV.setAccessible(true);
104         Assertions.assertSame(p.getPVCoordinates(), initPV.invoke(p, new Object[0]));
105 
106     }
107 
108     @Test
109     public void testCartesianToEquinoctial() {
110 
111         Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
112         Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
113         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
114 
115         CartesianOrbit p = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
116 
117         Assertions.assertEquals(42255170.0028257,  p.getA(), Utils.epsilonTest * p.getA());
118         Assertions.assertEquals(0.592732497856475e-03,  p.getEquinoctialEx(), Utils.epsilonE * FastMath.abs(p.getE()));
119         Assertions.assertEquals(-0.206274396964359e-02, p.getEquinoctialEy(), Utils.epsilonE * FastMath.abs(p.getE()));
120         Assertions.assertEquals(FastMath.sqrt(FastMath.pow(0.592732497856475e-03, 2)+FastMath.pow(-0.206274396964359e-02, 2)), p.getE(), Utils.epsilonAngle * FastMath.abs(p.getE()));
121         Assertions.assertEquals(MathUtils.normalizeAngle(2*FastMath.asin(FastMath.sqrt((FastMath.pow(0.128021863908325e-03, 2)+FastMath.pow(-0.352136186881817e-02, 2))/4.)), p.getI()), p.getI(), Utils.epsilonAngle * FastMath.abs(p.getI()));
122         Assertions.assertEquals(MathUtils.normalizeAngle(0.234498139679291e+01, p.getLM()), p.getLM(), Utils.epsilonAngle * FastMath.abs(p.getLM()));
123 
124         // trigger a specific path in copy constructor
125         CartesianOrbit q = new CartesianOrbit(p);
126 
127         Assertions.assertEquals(42255170.0028257,  q.getA(), Utils.epsilonTest * q.getA());
128         Assertions.assertEquals(0.592732497856475e-03,  q.getEquinoctialEx(), Utils.epsilonE * FastMath.abs(q.getE()));
129         Assertions.assertEquals(-0.206274396964359e-02, q.getEquinoctialEy(), Utils.epsilonE * FastMath.abs(q.getE()));
130         Assertions.assertEquals(FastMath.sqrt(FastMath.pow(0.592732497856475e-03, 2)+FastMath.pow(-0.206274396964359e-02, 2)), q.getE(), Utils.epsilonAngle * FastMath.abs(q.getE()));
131         Assertions.assertEquals(MathUtils.normalizeAngle(2*FastMath.asin(FastMath.sqrt((FastMath.pow(0.128021863908325e-03, 2)+FastMath.pow(-0.352136186881817e-02, 2))/4.)), q.getI()), q.getI(), Utils.epsilonAngle * FastMath.abs(q.getI()));
132         Assertions.assertEquals(MathUtils.normalizeAngle(0.234498139679291e+01, q.getLM()), q.getLM(), Utils.epsilonAngle * FastMath.abs(q.getLM()));
133 
134         Assertions.assertFalse(q.hasNonKeplerianAcceleration());
135 
136     }
137 
138     @Test
139     public void testCartesianToKeplerian(){
140 
141         Vector3D position = new Vector3D(-26655470.0, 29881667.0, -113657.0);
142         Vector3D velocity = new Vector3D(-1125.0, -1122.0, 195.0);
143         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
144         double mu = 3.9860047e14;
145 
146         CartesianOrbit p = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
147         KeplerianOrbit kep = new KeplerianOrbit(p);
148 
149         Assertions.assertEquals(22979265.3030773,  p.getA(), Utils.epsilonTest  * p.getA());
150         Assertions.assertEquals(0.743502611664700, p.getE(), Utils.epsilonE     * FastMath.abs(p.getE()));
151         Assertions.assertEquals(0.122182096220906, p.getI(), Utils.epsilonAngle * FastMath.abs(p.getI()));
152         double pa = kep.getPerigeeArgument();
153         Assertions.assertEquals(MathUtils.normalizeAngle(3.09909041016672, pa), pa,
154                      Utils.epsilonAngle * FastMath.abs(pa));
155         double raan = kep.getRightAscensionOfAscendingNode();
156         Assertions.assertEquals(MathUtils.normalizeAngle(2.32231010979999, raan), raan,
157                      Utils.epsilonAngle * FastMath.abs(raan));
158         double m = kep.getMeanAnomaly();
159         Assertions.assertEquals(MathUtils.normalizeAngle(3.22888977629034, m), m,
160                      Utils.epsilonAngle * FastMath.abs(FastMath.abs(m)));
161     }
162 
163     @Test
164     public void testPositionVelocityNorms(){
165 
166         Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
167         Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
168         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
169 
170         CartesianOrbit p = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
171 
172         double e       = p.getE();
173         double v       = new KeplerianOrbit(p).getTrueAnomaly();
174         double ksi     = 1 + e * FastMath.cos(v);
175         double nu      = e * FastMath.sin(v);
176         double epsilon = FastMath.sqrt((1 - e) * (1 + e));
177 
178         double a  = p.getA();
179         double na = FastMath.sqrt(mu / a);
180 
181         // validation of: r = a .(1 - e2) / (1 + e.cos(v))
182         Assertions.assertEquals(a * epsilon * epsilon / ksi,
183                      p.getPosition().getNorm(),
184                      Utils.epsilonTest * FastMath.abs(p.getPosition().getNorm()));
185 
186         // validation of: V = sqrt(mu.(1+2e.cos(v)+e2)/a.(1-e2) )
187         Assertions.assertEquals(na * FastMath.sqrt(ksi * ksi + nu * nu) / epsilon,
188                      p.getPVCoordinates().getVelocity().getNorm(),
189                      Utils.epsilonTest * FastMath.abs(p.getPVCoordinates().getVelocity().getNorm()));
190 
191     }
192 
193     @Test
194     public void testGeometry() {
195 
196         Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
197         Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
198         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
199 
200         Vector3D momentum = pvCoordinates.getMomentum().normalize();
201 
202         EquinoctialOrbit p = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
203 
204         double apogeeRadius  = p.getA() * (1 + p.getE());
205         double perigeeRadius = p.getA() * (1 - p.getE());
206 
207         for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI/100.) {
208             p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(), p.getEquinoctialEy(),
209                                           p.getHx(), p.getHy(), lv, PositionAngleType.TRUE, p.getFrame(), date, mu);
210             position = p.getPosition();
211 
212             // test if the norm of the position is in the range [perigee radius, apogee radius]
213             // Warning: these tests are without absolute value by choice
214             Assertions.assertTrue((position.getNorm() - apogeeRadius)  <= (  apogeeRadius * Utils.epsilonTest));
215             Assertions.assertTrue((position.getNorm() - perigeeRadius) >= (- perigeeRadius * Utils.epsilonTest));
216             // Assertions.assertTrue(position.getNorm() <= apogeeRadius);
217             // Assertions.assertTrue(position.getNorm() >= perigeeRadius);
218 
219             position= position.normalize();
220             velocity = p.getPVCoordinates().getVelocity().normalize();
221 
222             // at this stage of computation, all the vectors (position, velocity and momemtum) are normalized here
223 
224             // test of orthogonality between position and momentum
225             Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest);
226             // test of orthogonality between velocity and momentum
227             Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest);
228         }
229     }
230 
231     @Test
232     public void testHyperbola1() {
233         CartesianOrbit orbit = new CartesianOrbit(new KeplerianOrbit(-10000000.0, 2.5, 0.3, 0, 0, 0.0,
234                                                                      PositionAngleType.TRUE,
235                                                                      FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
236                                                                      mu));
237         Vector3D perigeeP  = orbit.getPosition();
238         Vector3D u = perigeeP.normalize();
239         Vector3D focus1 = Vector3D.ZERO;
240         Vector3D focus2 = new Vector3D(-2 * orbit.getA() * orbit.getE(), u);
241         for (double dt = -5000; dt < 5000; dt += 60) {
242             PVCoordinates pv = orbit.shiftedBy(dt).getPVCoordinates();
243             double d1 = Vector3D.distance(pv.getPosition(), focus1);
244             double d2 = Vector3D.distance(pv.getPosition(), focus2);
245             Assertions.assertEquals(-2 * orbit.getA(), FastMath.abs(d1 - d2), 1.0e-6);
246             CartesianOrbit rebuilt =
247                 new CartesianOrbit(pv, orbit.getFrame(), orbit.getDate().shiftedBy(dt), mu);
248             Assertions.assertEquals(-10000000.0, rebuilt.getA(), 1.0e-6);
249             Assertions.assertEquals(2.5, rebuilt.getE(), 1.0e-13);
250         }
251     }
252 
253     @Test
254     public void testHyperbola2() {
255         CartesianOrbit orbit = new CartesianOrbit(new KeplerianOrbit(-10000000.0, 1.2, 0.3, 0, 0, -1.75,
256                                                                      PositionAngleType.MEAN,
257                                                                      FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
258                                                                      mu));
259         Vector3D perigeeP  = new KeplerianOrbit(-10000000.0, 1.2, 0.3, 0, 0, 0.0, PositionAngleType.TRUE, orbit.getFrame(),
260                                                 orbit.getDate(), orbit.getMu()).getPosition();
261         Vector3D u = perigeeP.normalize();
262         Vector3D focus1 = Vector3D.ZERO;
263         Vector3D focus2 = new Vector3D(-2 * orbit.getA() * orbit.getE(), u);
264         for (double dt = -5000; dt < 5000; dt += 60) {
265             PVCoordinates pv = orbit.shiftedBy(dt).getPVCoordinates();
266             double d1 = Vector3D.distance(pv.getPosition(), focus1);
267             double d2 = Vector3D.distance(pv.getPosition(), focus2);
268             Assertions.assertEquals(-2 * orbit.getA(), FastMath.abs(d1 - d2), 1.0e-6);
269             CartesianOrbit rebuilt =
270                 new CartesianOrbit(pv, orbit.getFrame(), orbit.getDate().shiftedBy(dt), mu);
271             Assertions.assertEquals(-10000000.0, rebuilt.getA(), 1.0e-6);
272             Assertions.assertEquals(1.2, rebuilt.getE(), 1.0e-13);
273         }
274     }
275 
276     @Test
277     public void testNumericalIssue25() {
278         Vector3D position = new Vector3D(3782116.14107698, 416663.11924914, 5875541.62103057);
279         Vector3D velocity = new Vector3D(-6349.7848910501, 288.4061811651, 4066.9366759691);
280         CartesianOrbit orbit = new CartesianOrbit(new PVCoordinates(position, velocity),
281                                                   FramesFactory.getEME2000(),
282                                                   new AbsoluteDate("2004-01-01T23:00:00.000",
283                                                                    TimeScalesFactory.getUTC()),
284                                                                    3.986004415E14);
285         Assertions.assertEquals(0.0, orbit.getE(), 2.0e-14);
286     }
287 
288     @Test
289     public void testDerivativesConversionSymmetry() {
290         final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:01:20.000", TimeScalesFactory.getUTC());
291         Vector3D position     = new Vector3D(6893443.400234382, 1886406.1073757345, -589265.1150359757);
292         Vector3D velocity     = new Vector3D(-281.1261461082365, -1231.6165642450928, -7348.756363469432);
293         Vector3D acceleration = new Vector3D(-7.460341170581685, -2.0415957334584527, 0.6393322823627762);
294         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity, acceleration);
295         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(),
296                                                   date, Constants.EIGEN5C_EARTH_MU);
297         Assertions.assertTrue(orbit.hasNonKeplerianAcceleration());
298         double r2 = position.getNormSq();
299         double r  = FastMath.sqrt(r2);
300         Vector3D keplerianAcceleration = new Vector3D(-orbit.getMu() / (r2 * r), position);
301         Assertions.assertEquals(0.0101, Vector3D.distance(keplerianAcceleration, acceleration), 1.0e-4);
302 
303         for (OrbitType type : OrbitType.values()) {
304             Orbit converted = type.convertType(orbit);
305             Assertions.assertTrue(converted.hasNonKeplerianAcceleration());
306             CartesianOrbit rebuilt = (CartesianOrbit) OrbitType.CARTESIAN.convertType(converted);
307             Assertions.assertTrue(rebuilt.hasNonKeplerianAcceleration());
308             Assertions.assertEquals(0, Vector3D.distance(rebuilt.getPosition(),     position),     2.0e-9);
309             Assertions.assertEquals(0, Vector3D.distance(rebuilt.getPVCoordinates().getVelocity(),     velocity),     2.5e-12);
310             Assertions.assertEquals(0, Vector3D.distance(rebuilt.getPVCoordinates().getAcceleration(), acceleration), 4.9e-15);
311         }
312 
313     }
314 
315     @Test
316     public void testDerivativesConversionSymmetryHyperbolic() {
317         final AbsoluteDate date         = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
318         final Vector3D     position     = new Vector3D(224267911.905821, 290251613.109399, 45534292.777492);
319         final Vector3D     velocity     = new Vector3D(-1494.068165293, 1124.771027677, 526.915286134);
320         final Vector3D     acceleration = new Vector3D(-0.001295920501, -0.002233045187, -0.000349906292);
321         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity, acceleration);
322         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(),
323                                                   date, Constants.EIGEN5C_EARTH_MU);
324         Assertions.assertTrue(orbit.hasNonKeplerianAcceleration());
325         double r2 = position.getNormSq();
326         double r  = FastMath.sqrt(r2);
327         Vector3D keplerianAcceleration = new Vector3D(-orbit.getMu() / (r2 * r), position);
328         Assertions.assertEquals(4.78e-4, Vector3D.distance(keplerianAcceleration, acceleration), 1.0e-6);
329 
330         OrbitType type = OrbitType.KEPLERIAN;
331         Orbit converted = type.convertType(orbit);
332         Assertions.assertTrue(converted.hasNonKeplerianAcceleration());
333         CartesianOrbit rebuilt = (CartesianOrbit) OrbitType.CARTESIAN.convertType(converted);
334         Assertions.assertTrue(rebuilt.hasNonKeplerianAcceleration());
335         Assertions.assertEquals(0, Vector3D.distance(rebuilt.getPosition(),     position),     1.0e-15);
336         Assertions.assertEquals(0, Vector3D.distance(rebuilt.getPVCoordinates().getVelocity(),     velocity),     1.0e-15);
337         Assertions.assertEquals(0, Vector3D.distance(rebuilt.getPVCoordinates().getAcceleration(), acceleration), 1.0e-15);
338 
339     }
340 
341     @Test
342     public void testShiftElliptic() {
343         Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
344         Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
345         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
346         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
347         testShift(orbit, new KeplerianOrbit(orbit), 1.0e-13);
348     }
349 
350     @Test
351     public void testShiftCircular() {
352         Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
353         Vector3D velocity = new Vector3D(FastMath.sqrt(mu / position.getNorm()), position.orthogonal());
354         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
355         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
356         testShift(orbit, new CircularOrbit(orbit), 1.0e-15);
357     }
358 
359     @Test
360     public void testShiftEquinoctial() {
361         Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
362         Vector3D velocity = new Vector3D(FastMath.sqrt(mu / position.getNorm()), position.orthogonal());
363         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
364         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
365         testShift(orbit, new EquinoctialOrbit(orbit), 5.0e-14);
366     }
367 
368     @Test
369     public void testShiftHyperbolic() {
370         Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
371         Vector3D velocity = new Vector3D(3 * FastMath.sqrt(mu / position.getNorm()), position.orthogonal());
372         PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
373         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
374         testShift(orbit, new KeplerianOrbit(orbit), 2.0e-15);
375     }
376 
377     @Test
378     public void testNumericalIssue135() {
379         Vector3D position = new Vector3D(-6.7884943832e7, -2.1423006112e7, -3.1603915377e7);
380         Vector3D velocity = new Vector3D(-4732.55, -2472.086, -3022.177);
381         PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
382         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date,
383                                                   324858598826460.);
384         testShift(orbit, new KeplerianOrbit(orbit), 6.0e-15);
385     }
386 
387     @Test
388     public void testNumericalIssue1015() {
389         Vector3D position = new Vector3D(-1466739.735988, 1586390.713569, 6812901.677773);
390         Vector3D velocity = new Vector3D(-9532.812, -4321.894, -1409.018);
391         PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
392         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, 3.986004415E14);
393         testShift(orbit, new KeplerianOrbit(orbit), 1.0e-10);
394     }
395 
396     private void testShift(CartesianOrbit tested, Orbit reference, double threshold) {
397         for (double dt = - 1000; dt < 1000; dt += 10.0) {
398 
399             PVCoordinates pvTested    = tested.shiftedBy(dt).getPVCoordinates();
400             Vector3D      pTested     = pvTested.getPosition();
401             Vector3D      vTested     = pvTested.getVelocity();
402 
403             PVCoordinates pvReference = reference.shiftedBy(dt).getPVCoordinates();
404             Vector3D      pReference  = pvReference.getPosition();
405             Vector3D      vReference  = pvReference.getVelocity();
406 
407             Assertions.assertEquals(0, pTested.subtract(pReference).getNorm(), threshold * pReference.getNorm());
408             Assertions.assertEquals(0, vTested.subtract(vReference).getNorm(), threshold * vReference.getNorm());
409 
410         }
411     }
412 
413     @Test
414     public void testNonInertialFrame() throws IllegalArgumentException {
415         Assertions.assertThrows(IllegalArgumentException.class, () -> {
416             Vector3D position = new Vector3D(-26655470.0, 29881667.0, -113657.0);
417             Vector3D velocity = new Vector3D(-1125.0, -1122.0, 195.0);
418             PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
419             double mu = 3.9860047e14;
420             new CartesianOrbit(pvCoordinates,
421                     new Frame(FramesFactory.getEME2000(), Transform.IDENTITY, "non-inertial", false),
422                     date, mu);
423         });
424     }
425 
426     @Test
427     public void testJacobianReference() {
428 
429         Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
430         Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
431         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
432         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
433 
434         double[][] jacobian = new double[6][6];
435         orbit.getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
436 
437         for (int i = 0; i < jacobian.length; i++) {
438             double[] row    = jacobian[i];
439             for (int j = 0; j < row.length; j++) {
440                 Assertions.assertEquals((i == j) ? 1 : 0, row[j], 1.0e-15);
441             }
442         }
443 
444         double[][] invJacobian = new double[6][6];
445         orbit.getJacobianWrtParameters(PositionAngleType.MEAN, invJacobian);
446         MatrixUtils.createRealMatrix(jacobian).
447                         multiply(MatrixUtils.createRealMatrix(invJacobian)).
448         walkInRowOrder(new RealMatrixPreservingVisitor() {
449             public void start(int rows, int columns,
450                               int startRow, int endRow, int startColumn, int endColumn) {
451             }
452 
453             public void visit(int row, int column, double value) {
454                 Assertions.assertEquals(row == column ? 1.0 : 0.0, value, 1.0e-15);
455             }
456 
457             public double end() {
458                 return Double.NaN;
459             }
460         });
461 
462     }
463 
464     @Test
465     public void testNonKeplerianDerivatives() {
466         final AbsoluteDate date         = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
467         final Vector3D     position     = new Vector3D(6896874.444705,  1956581.072644,  -147476.245054);
468         final Vector3D     velocity     = new Vector3D(166.816407662, -1106.783301861, -7372.745712770);
469         final Vector3D     acceleration = new Vector3D(-7.466182457944, -2.118153357345,  0.160004048437);
470         final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, position, velocity, acceleration);
471         final Frame frame = FramesFactory.getEME2000();
472         final double mu   = Constants.EIGEN5C_EARTH_MU;
473         final CartesianOrbit orbit = new CartesianOrbit(pv, frame, mu);
474 
475         Assertions.assertEquals(differentiate(pv, frame, mu, CartesianOrbit::getA),
476                             orbit.getADot(),
477                             4.3e-8);
478         Assertions.assertEquals(differentiate(pv, frame, mu, CartesianOrbit::getEquinoctialEx),
479                             orbit.getEquinoctialExDot(),
480                             2.1e-15);
481         Assertions.assertEquals(differentiate(pv, frame, mu, CartesianOrbit::getEquinoctialEy),
482                             orbit.getEquinoctialEyDot(),
483                             5.3e-16);
484         Assertions.assertEquals(differentiate(pv, frame, mu, CartesianOrbit::getHx),
485                             orbit.getHxDot(),
486                             4.4e-15);
487         Assertions.assertEquals(differentiate(pv, frame, mu, CartesianOrbit::getHy),
488                             orbit.getHyDot(),
489                             8.0e-16);
490         Assertions.assertEquals(differentiate(pv, frame, mu, CartesianOrbit::getLv),
491                             orbit.getLvDot(),
492                             1.2e-15);
493         Assertions.assertEquals(differentiate(pv, frame, mu, CartesianOrbit::getLE),
494                             orbit.getLEDot(),
495                             7.8e-16);
496         Assertions.assertEquals(differentiate(pv, frame, mu, CartesianOrbit::getLM),
497                             orbit.getLMDot(),
498                             8.8e-16);
499         Assertions.assertEquals(differentiate(pv, frame, mu, CartesianOrbit::getE),
500                             orbit.getEDot(),
501                             7.0e-16);
502         Assertions.assertEquals(differentiate(pv, frame, mu, CartesianOrbit::getI),
503                             orbit.getIDot(),
504                             5.7e-16);
505 
506     }
507 
508     private <S extends Function<CartesianOrbit, Double>>
509     double differentiate(TimeStampedPVCoordinates pv, Frame frame, double mu, S picker) {
510         final DSFactory factory = new DSFactory(1, 1);
511         FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
512         UnivariateDifferentiableFunction diff = differentiator.differentiate(
513             (UnivariateFunction) dt -> picker.apply(new CartesianOrbit(pv.shiftedBy(dt), frame, mu)));
514         return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
515     }
516 
517     @Test
518     public void testEquatorialRetrograde() {
519         Vector3D position = new Vector3D(10000000.0, 0.0, 0.0);
520         Vector3D velocity = new Vector3D(0.0, -6500.0, 0.0);
521         double r2 = position.getNormSq();
522         double r  = FastMath.sqrt(r2);
523         Vector3D acceleration = new Vector3D(-mu / (r * r2), position,
524                                              1, new Vector3D(-0.1, 0.2, 0.3));
525         PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
526         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
527         Assertions.assertEquals(10637829.465, orbit.getA(), 1.0e-3);
528         Assertions.assertEquals(-738.145, orbit.getADot(), 1.0e-3);
529         Assertions.assertEquals(0.05995861, orbit.getE(), 1.0e-8);
530         Assertions.assertEquals(-6.523e-5, orbit.getEDot(), 1.0e-8);
531         Assertions.assertEquals(FastMath.PI, orbit.getI(), 1.0e-15);
532         Assertions.assertTrue(Double.isNaN(orbit.getIDot()));
533         Assertions.assertTrue(Double.isNaN(orbit.getHx()));
534         Assertions.assertTrue(Double.isNaN(orbit.getHxDot()));
535         Assertions.assertTrue(Double.isNaN(orbit.getHy()));
536         Assertions.assertTrue(Double.isNaN(orbit.getHyDot()));
537     }
538 
539     @Test
540     public void testToString() {
541         Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
542         Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
543         PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
544         CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
545         Assertions.assertEquals("Cartesian parameters: {P(-2.9536113E7, 3.0329259E7, -100125.0), V(-2194.0, -2141.0, -8.0)}",
546                             orbit.toString());
547     }
548 
549     @Test
550     public void testCopyNonKeplerianAcceleration() {
551 
552         final Frame eme2000     = FramesFactory.getEME2000();
553 
554         // Define GEO satellite position
555         final Vector3D position = new Vector3D(42164140, 0, 0);
556         // Build PVCoodrinates starting from its position and computing the corresponding circular velocity
557         final PVCoordinates pv  = new PVCoordinates(position,
558                                        new Vector3D(0, FastMath.sqrt(mu / position.getNorm()), 0));
559         // Build a KeplerianOrbit in eme2000
560         final Orbit orbit = new CartesianOrbit(pv, eme2000, date, mu);
561 
562         // Build another KeplerianOrbit as a copy of the first one
563         final Orbit orbitCopy = new CartesianOrbit(orbit);
564 
565         // Shift the orbit of a time-interval
566         final Orbit shiftedOrbit = orbit.shiftedBy(10); // This works good
567         final Orbit shiftedOrbitCopy = orbitCopy.shiftedBy(10); // This does not work
568 
569         Assertions.assertEquals(0.0,
570                             Vector3D.distance(shiftedOrbit.getPosition(),
571                                               shiftedOrbitCopy.getPosition()),
572                             1.0e-10);
573         Assertions.assertEquals(0.0,
574                             Vector3D.distance(shiftedOrbit.getPVCoordinates().getVelocity(),
575                                               shiftedOrbitCopy.getPVCoordinates().getVelocity()),
576                             1.0e-10);
577 
578     }
579 
580     @Test
581     public void testNormalize() {
582         final Vector3D position = new Vector3D(42164140, 0, 0);
583         final PVCoordinates pv  = new PVCoordinates(position,
584                                        new Vector3D(0, FastMath.sqrt(mu / position.getNorm()), 0));
585         final Orbit orbit = new CartesianOrbit(pv, FramesFactory.getEME2000(), date, mu);
586         Assertions.assertSame(orbit, orbit.getType().normalize(orbit, null));
587     }
588 
589     @Test
590     public void testIssue1139() {
591 
592         // Create
593         Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
594         Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
595         PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
596 
597         CartesianOrbit p = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
598 
599         double dt = 60.0;
600         AbsoluteDate shiftedEpoch = date.shiftedBy(dt);
601 
602         CartesianOrbit p2 = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), shiftedEpoch, mu);
603 
604         // Verify
605         Assertions.assertEquals(dt, shiftedEpoch.durationFrom(date));
606         Assertions.assertEquals(dt, p2.durationFrom(p));
607         Assertions.assertEquals(dt, p2.getDate().durationFrom(p));
608         Assertions.assertEquals(dt, p2.durationFrom(p.getDate()));
609         Assertions.assertEquals(dt, p2.getDate().durationFrom(p.getDate()));
610         Assertions.assertEquals(-dt, p.durationFrom(p2));
611         
612     }
613 
614     @BeforeEach
615     public void setUp() {
616 
617         Utils.setDataRoot("regular-data");
618 
619         // Computation date
620         date = AbsoluteDate.J2000_EPOCH;
621 
622         // Body mu
623         mu = 3.9860047e14;
624     }
625 
626     @AfterEach
627     public void tearDown() {
628         date = null;
629     }
630 
631 }
632