1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import java.io.ByteArrayInputStream;
20 import java.io.ByteArrayOutputStream;
21 import java.io.IOException;
22 import java.io.ObjectInputStream;
23 import java.io.ObjectOutputStream;
24 import java.lang.reflect.InvocationTargetException;
25 import java.lang.reflect.Method;
26 import java.util.ArrayList;
27 import java.util.List;
28 import java.util.function.Function;
29
30 import org.hipparchus.analysis.UnivariateFunction;
31 import org.hipparchus.analysis.differentiation.DSFactory;
32 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
33 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
34 import org.hipparchus.geometry.euclidean.threed.Vector3D;
35 import org.hipparchus.linear.MatrixUtils;
36 import org.hipparchus.linear.RealMatrixPreservingVisitor;
37 import org.hipparchus.util.FastMath;
38 import org.hipparchus.util.MathUtils;
39 import org.junit.After;
40 import org.junit.Assert;
41 import org.junit.Before;
42 import org.junit.Test;
43 import org.orekit.Utils;
44 import org.orekit.frames.Frame;
45 import org.orekit.frames.FramesFactory;
46 import org.orekit.frames.Transform;
47 import org.orekit.propagation.analytical.EcksteinHechlerPropagator;
48 import org.orekit.time.AbsoluteDate;
49 import org.orekit.time.TimeScalesFactory;
50 import org.orekit.utils.Constants;
51 import org.orekit.utils.PVCoordinates;
52 import org.orekit.utils.TimeStampedPVCoordinates;
53
54
55 public class CartesianOrbitTest {
56
57
58 private AbsoluteDate date;
59
60
61 private double mu;
62
63 @Test
64 public void testCartesianToCartesian()
65 throws NoSuchMethodException, SecurityException, IllegalAccessException, IllegalArgumentException, InvocationTargetException {
66
67 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
68 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
69 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
70 double mu = 3.9860047e14;
71
72 CartesianOrbit p = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
73
74 Assert.assertEquals(p.getPVCoordinates().getPosition().getX(), pvCoordinates.getPosition().getX(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getPosition().getX()));
75 Assert.assertEquals(p.getPVCoordinates().getPosition().getY(), pvCoordinates.getPosition().getY(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getPosition().getY()));
76 Assert.assertEquals(p.getPVCoordinates().getPosition().getZ(), pvCoordinates.getPosition().getZ(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getPosition().getZ()));
77 Assert.assertEquals(p.getPVCoordinates().getVelocity().getX(), pvCoordinates.getVelocity().getX(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getVelocity().getX()));
78 Assert.assertEquals(p.getPVCoordinates().getVelocity().getY(), pvCoordinates.getVelocity().getY(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getVelocity().getY()));
79 Assert.assertEquals(p.getPVCoordinates().getVelocity().getZ(), pvCoordinates.getVelocity().getZ(), Utils.epsilonTest * FastMath.abs(pvCoordinates.getVelocity().getZ()));
80
81 Method initPV = CartesianOrbit.class.getDeclaredMethod("initPVCoordinates", new Class[0]);
82 initPV.setAccessible(true);
83 Assert.assertSame(p.getPVCoordinates(), initPV.invoke(p, new Object[0]));
84
85 }
86
87 @Test
88 public void testCartesianToEquinoctial() {
89
90 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
91 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
92 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
93
94 CartesianOrbit p = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
95
96 Assert.assertEquals(42255170.0028257, p.getA(), Utils.epsilonTest * p.getA());
97 Assert.assertEquals(0.592732497856475e-03, p.getEquinoctialEx(), Utils.epsilonE * FastMath.abs(p.getE()));
98 Assert.assertEquals(-0.206274396964359e-02, p.getEquinoctialEy(), Utils.epsilonE * FastMath.abs(p.getE()));
99 Assert.assertEquals(FastMath.sqrt(FastMath.pow(0.592732497856475e-03, 2)+FastMath.pow(-0.206274396964359e-02, 2)), p.getE(), Utils.epsilonAngle * FastMath.abs(p.getE()));
100 Assert.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()));
101 Assert.assertEquals(MathUtils.normalizeAngle(0.234498139679291e+01, p.getLM()), p.getLM(), Utils.epsilonAngle * FastMath.abs(p.getLM()));
102
103
104 CartesianOrbit q = new CartesianOrbit(p);
105
106 Assert.assertEquals(42255170.0028257, q.getA(), Utils.epsilonTest * q.getA());
107 Assert.assertEquals(0.592732497856475e-03, q.getEquinoctialEx(), Utils.epsilonE * FastMath.abs(q.getE()));
108 Assert.assertEquals(-0.206274396964359e-02, q.getEquinoctialEy(), Utils.epsilonE * FastMath.abs(q.getE()));
109 Assert.assertEquals(FastMath.sqrt(FastMath.pow(0.592732497856475e-03, 2)+FastMath.pow(-0.206274396964359e-02, 2)), q.getE(), Utils.epsilonAngle * FastMath.abs(q.getE()));
110 Assert.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()));
111 Assert.assertEquals(MathUtils.normalizeAngle(0.234498139679291e+01, q.getLM()), q.getLM(), Utils.epsilonAngle * FastMath.abs(q.getLM()));
112
113 Assert.assertTrue(Double.isNaN(q.getADot()));
114 Assert.assertTrue(Double.isNaN(q.getEquinoctialExDot()));
115 Assert.assertTrue(Double.isNaN(q.getEquinoctialEyDot()));
116 Assert.assertTrue(Double.isNaN(q.getHxDot()));
117 Assert.assertTrue(Double.isNaN(q.getHyDot()));
118 Assert.assertTrue(Double.isNaN(q.getLvDot()));
119 Assert.assertTrue(Double.isNaN(q.getEDot()));
120 Assert.assertTrue(Double.isNaN(q.getIDot()));
121
122 }
123
124 @Test
125 public void testCartesianToKeplerian(){
126
127 Vector3D position = new Vector3D(-26655470.0, 29881667.0, -113657.0);
128 Vector3D velocity = new Vector3D(-1125.0, -1122.0, 195.0);
129 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
130 double mu = 3.9860047e14;
131
132 CartesianOrbit p = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
133 KeplerianOrbit kep = new KeplerianOrbit(p);
134
135 Assert.assertEquals(22979265.3030773, p.getA(), Utils.epsilonTest * p.getA());
136 Assert.assertEquals(0.743502611664700, p.getE(), Utils.epsilonE * FastMath.abs(p.getE()));
137 Assert.assertEquals(0.122182096220906, p.getI(), Utils.epsilonAngle * FastMath.abs(p.getI()));
138 double pa = kep.getPerigeeArgument();
139 Assert.assertEquals(MathUtils.normalizeAngle(3.09909041016672, pa), pa,
140 Utils.epsilonAngle * FastMath.abs(pa));
141 double raan = kep.getRightAscensionOfAscendingNode();
142 Assert.assertEquals(MathUtils.normalizeAngle(2.32231010979999, raan), raan,
143 Utils.epsilonAngle * FastMath.abs(raan));
144 double m = kep.getMeanAnomaly();
145 Assert.assertEquals(MathUtils.normalizeAngle(3.22888977629034, m), m,
146 Utils.epsilonAngle * FastMath.abs(FastMath.abs(m)));
147 }
148
149 @Test
150 public void testPositionVelocityNorms(){
151
152 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
153 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
154 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
155
156 CartesianOrbit p = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
157
158 double e = p.getE();
159 double v = new KeplerianOrbit(p).getTrueAnomaly();
160 double ksi = 1 + e * FastMath.cos(v);
161 double nu = e * FastMath.sin(v);
162 double epsilon = FastMath.sqrt((1 - e) * (1 + e));
163
164 double a = p.getA();
165 double na = FastMath.sqrt(mu / a);
166
167
168 Assert.assertEquals(a * epsilon * epsilon / ksi,
169 p.getPVCoordinates().getPosition().getNorm(),
170 Utils.epsilonTest * FastMath.abs(p.getPVCoordinates().getPosition().getNorm()));
171
172
173 Assert.assertEquals(na * FastMath.sqrt(ksi * ksi + nu * nu) / epsilon,
174 p.getPVCoordinates().getVelocity().getNorm(),
175 Utils.epsilonTest * FastMath.abs(p.getPVCoordinates().getVelocity().getNorm()));
176
177 }
178
179 @Test
180 public void testGeometry() {
181
182 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
183 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
184 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
185
186 Vector3D momentum = pvCoordinates.getMomentum().normalize();
187
188 EquinoctialOrbit p = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
189
190 double apogeeRadius = p.getA() * (1 + p.getE());
191 double perigeeRadius = p.getA() * (1 - p.getE());
192
193 for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI/100.) {
194 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(), p.getEquinoctialEy(),
195 p.getHx(), p.getHy(), lv, PositionAngle.TRUE, p.getFrame(), date, mu);
196 position = p.getPVCoordinates().getPosition();
197
198
199
200 Assert.assertTrue((position.getNorm() - apogeeRadius) <= ( apogeeRadius * Utils.epsilonTest));
201 Assert.assertTrue((position.getNorm() - perigeeRadius) >= (- perigeeRadius * Utils.epsilonTest));
202
203
204
205 position= position.normalize();
206 velocity = p.getPVCoordinates().getVelocity().normalize();
207
208
209
210
211 Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest);
212
213 Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest);
214 }
215 }
216
217 @Test
218 public void testHyperbola1() {
219 CartesianOrbit orbit = new CartesianOrbit(new KeplerianOrbit(-10000000.0, 2.5, 0.3, 0, 0, 0.0,
220 PositionAngle.TRUE,
221 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
222 mu));
223 Vector3D perigeeP = orbit.getPVCoordinates().getPosition();
224 Vector3D u = perigeeP.normalize();
225 Vector3D focus1 = Vector3D.ZERO;
226 Vector3D focus2 = new Vector3D(-2 * orbit.getA() * orbit.getE(), u);
227 for (double dt = -5000; dt < 5000; dt += 60) {
228 PVCoordinates pv = orbit.shiftedBy(dt).getPVCoordinates();
229 double d1 = Vector3D.distance(pv.getPosition(), focus1);
230 double d2 = Vector3D.distance(pv.getPosition(), focus2);
231 Assert.assertEquals(-2 * orbit.getA(), FastMath.abs(d1 - d2), 1.0e-6);
232 CartesianOrbit rebuilt =
233 new CartesianOrbit(pv, orbit.getFrame(), orbit.getDate().shiftedBy(dt), mu);
234 Assert.assertEquals(-10000000.0, rebuilt.getA(), 1.0e-6);
235 Assert.assertEquals(2.5, rebuilt.getE(), 1.0e-13);
236 }
237 }
238
239 @Test
240 public void testHyperbola2() {
241 CartesianOrbit orbit = new CartesianOrbit(new KeplerianOrbit(-10000000.0, 1.2, 0.3, 0, 0, -1.75,
242 PositionAngle.MEAN,
243 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
244 mu));
245 Vector3D perigeeP = new KeplerianOrbit(-10000000.0, 1.2, 0.3, 0, 0, 0.0, PositionAngle.TRUE, orbit.getFrame(),
246 orbit.getDate(), orbit.getMu()).getPVCoordinates().getPosition();
247 Vector3D u = perigeeP.normalize();
248 Vector3D focus1 = Vector3D.ZERO;
249 Vector3D focus2 = new Vector3D(-2 * orbit.getA() * orbit.getE(), u);
250 for (double dt = -5000; dt < 5000; dt += 60) {
251 PVCoordinates pv = orbit.shiftedBy(dt).getPVCoordinates();
252 double d1 = Vector3D.distance(pv.getPosition(), focus1);
253 double d2 = Vector3D.distance(pv.getPosition(), focus2);
254 Assert.assertEquals(-2 * orbit.getA(), FastMath.abs(d1 - d2), 1.0e-6);
255 CartesianOrbit rebuilt =
256 new CartesianOrbit(pv, orbit.getFrame(), orbit.getDate().shiftedBy(dt), mu);
257 Assert.assertEquals(-10000000.0, rebuilt.getA(), 1.0e-6);
258 Assert.assertEquals(1.2, rebuilt.getE(), 1.0e-13);
259 }
260 }
261
262 @Test
263 public void testNumericalIssue25() {
264 Vector3D position = new Vector3D(3782116.14107698, 416663.11924914, 5875541.62103057);
265 Vector3D velocity = new Vector3D(-6349.7848910501, 288.4061811651, 4066.9366759691);
266 CartesianOrbit orbit = new CartesianOrbit(new PVCoordinates(position, velocity),
267 FramesFactory.getEME2000(),
268 new AbsoluteDate("2004-01-01T23:00:00.000",
269 TimeScalesFactory.getUTC()),
270 3.986004415E14);
271 Assert.assertEquals(0.0, orbit.getE(), 2.0e-14);
272 }
273
274 @Test
275 public void testSerialization()
276 throws IOException, ClassNotFoundException {
277 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
278 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
279 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
280 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
281 Assert.assertEquals(42255170.003, orbit.getA(), 1.0e-3);
282
283 ByteArrayOutputStream bos = new ByteArrayOutputStream();
284 ObjectOutputStream oos = new ObjectOutputStream(bos);
285 oos.writeObject(orbit);
286
287 Assert.assertTrue(bos.size() > 270);
288 Assert.assertTrue(bos.size() < 320);
289
290 ByteArrayInputStream bis = new ByteArrayInputStream(bos.toByteArray());
291 ObjectInputStream ois = new ObjectInputStream(bis);
292 CartesianOrbit deserialized = (CartesianOrbit) ois.readObject();
293 PVCoordinates dpv = new PVCoordinates(orbit.getPVCoordinates(), deserialized.getPVCoordinates());
294 Assert.assertEquals(0.0, dpv.getPosition().getNorm(), 1.0e-10);
295 Assert.assertEquals(0.0, dpv.getVelocity().getNorm(), 1.0e-10);
296 Assert.assertTrue(Double.isNaN(orbit.getADot()) && Double.isNaN(deserialized.getADot()));
297 Assert.assertTrue(Double.isNaN(orbit.getEDot()) && Double.isNaN(deserialized.getEDot()));
298 Assert.assertTrue(Double.isNaN(orbit.getIDot()) && Double.isNaN(deserialized.getIDot()));
299 Assert.assertTrue(Double.isNaN(orbit.getEquinoctialExDot()) && Double.isNaN(deserialized.getEquinoctialExDot()));
300 Assert.assertTrue(Double.isNaN(orbit.getEquinoctialEyDot()) && Double.isNaN(deserialized.getEquinoctialEyDot()));
301 Assert.assertTrue(Double.isNaN(orbit.getHxDot()) && Double.isNaN(deserialized.getHxDot()));
302 Assert.assertTrue(Double.isNaN(orbit.getHyDot()) && Double.isNaN(deserialized.getHyDot()));
303 Assert.assertTrue(Double.isNaN(orbit.getLvDot()) && Double.isNaN(deserialized.getLvDot()));
304 Assert.assertEquals(orbit.getDate(), deserialized.getDate());
305 Assert.assertEquals(orbit.getMu(), deserialized.getMu(), 1.0e-10);
306 Assert.assertEquals(orbit.getFrame().getName(), deserialized.getFrame().getName());
307
308 }
309
310 @Test
311 public void testSerializationWithDerivatives()
312 throws IOException, ClassNotFoundException {
313 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
314 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
315 double r2 = position.getNormSq();
316 double r = FastMath.sqrt(r2);
317 Vector3D acceleration = new Vector3D(-mu / (r * r2), position,
318 1, new Vector3D(-0.1, 0.2, 0.3));
319 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
320 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
321 Assert.assertEquals(42255170.003, orbit.getA(), 1.0e-3);
322
323 ByteArrayOutputStream bos = new ByteArrayOutputStream();
324 ObjectOutputStream oos = new ObjectOutputStream(bos);
325 oos.writeObject(orbit);
326
327 Assert.assertTrue(bos.size() > 320);
328 Assert.assertTrue(bos.size() < 370);
329
330 ByteArrayInputStream bis = new ByteArrayInputStream(bos.toByteArray());
331 ObjectInputStream ois = new ObjectInputStream(bis);
332 CartesianOrbit deserialized = (CartesianOrbit) ois.readObject();
333 PVCoordinates dpv = new PVCoordinates(orbit.getPVCoordinates(), deserialized.getPVCoordinates());
334 Assert.assertEquals(0.0, dpv.getPosition().getNorm(), 1.0e-10);
335 Assert.assertEquals(0.0, dpv.getVelocity().getNorm(), 1.0e-10);
336 Assert.assertEquals(orbit.getADot(), deserialized.getADot(), 1.0e-10);
337 Assert.assertEquals(orbit.getEDot(), deserialized.getEDot(), 1.0e-10);
338 Assert.assertEquals(orbit.getIDot(), deserialized.getIDot(), 1.0e-10);
339 Assert.assertEquals(orbit.getEquinoctialExDot(), deserialized.getEquinoctialExDot(), 1.0e-10);
340 Assert.assertEquals(orbit.getEquinoctialEyDot(), deserialized.getEquinoctialEyDot(), 1.0e-10);
341 Assert.assertEquals(orbit.getHxDot(), deserialized.getHxDot(), 1.0e-10);
342 Assert.assertEquals(orbit.getHyDot(), deserialized.getHyDot(), 1.0e-10);
343 Assert.assertEquals(orbit.getLvDot(), deserialized.getLvDot(), 1.0e-10);
344 Assert.assertEquals(orbit.getDate(), deserialized.getDate());
345 Assert.assertEquals(orbit.getMu(), deserialized.getMu(), 1.0e-10);
346 Assert.assertEquals(orbit.getFrame().getName(), deserialized.getFrame().getName());
347
348 }
349
350 @Test
351 public void testDerivativesConversionSymmetry() {
352 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:01:20.000", TimeScalesFactory.getUTC());
353 Vector3D position = new Vector3D(6893443.400234382, 1886406.1073757345, -589265.1150359757);
354 Vector3D velocity = new Vector3D(-281.1261461082365, -1231.6165642450928, -7348.756363469432);
355 Vector3D acceleration = new Vector3D(-7.460341170581685, -2.0415957334584527, 0.6393322823627762);
356 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity, acceleration);
357 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(),
358 date, Constants.EIGEN5C_EARTH_MU);
359 Assert.assertTrue(orbit.hasDerivatives());
360 double r2 = position.getNormSq();
361 double r = FastMath.sqrt(r2);
362 Vector3D keplerianAcceleration = new Vector3D(-orbit.getMu() / (r2 * r), position);
363 Assert.assertEquals(0.0101, Vector3D.distance(keplerianAcceleration, acceleration), 1.0e-4);
364
365 for (OrbitType type : OrbitType.values()) {
366 Orbit converted = type.convertType(orbit);
367 Assert.assertTrue(converted.hasDerivatives());
368 CartesianOrbit rebuilt = (CartesianOrbit) OrbitType.CARTESIAN.convertType(converted);
369 Assert.assertTrue(rebuilt.hasDerivatives());
370 Assert.assertEquals(0, Vector3D.distance(rebuilt.getPVCoordinates().getPosition(), position), 2.0e-9);
371 Assert.assertEquals(0, Vector3D.distance(rebuilt.getPVCoordinates().getVelocity(), velocity), 2.5e-12);
372 Assert.assertEquals(0, Vector3D.distance(rebuilt.getPVCoordinates().getAcceleration(), acceleration), 4.9e-15);
373 }
374
375 }
376
377 @Test
378 public void testDerivativesConversionSymmetryHyperbolic() {
379 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
380 final Vector3D position = new Vector3D(224267911.905821, 290251613.109399, 45534292.777492);
381 final Vector3D velocity = new Vector3D(-1494.068165293, 1124.771027677, 526.915286134);
382 final Vector3D acceleration = new Vector3D(-0.001295920501, -0.002233045187, -0.000349906292);
383 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity, acceleration);
384 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(),
385 date, Constants.EIGEN5C_EARTH_MU);
386 Assert.assertTrue(orbit.hasDerivatives());
387 double r2 = position.getNormSq();
388 double r = FastMath.sqrt(r2);
389 Vector3D keplerianAcceleration = new Vector3D(-orbit.getMu() / (r2 * r), position);
390 Assert.assertEquals(4.78e-4, Vector3D.distance(keplerianAcceleration, acceleration), 1.0e-6);
391
392 OrbitType type = OrbitType.KEPLERIAN;
393 Orbit converted = type.convertType(orbit);
394 Assert.assertTrue(converted.hasDerivatives());
395 CartesianOrbit rebuilt = (CartesianOrbit) OrbitType.CARTESIAN.convertType(converted);
396 Assert.assertTrue(rebuilt.hasDerivatives());
397 Assert.assertEquals(0, Vector3D.distance(rebuilt.getPVCoordinates().getPosition(), position), 1.0e-15);
398 Assert.assertEquals(0, Vector3D.distance(rebuilt.getPVCoordinates().getVelocity(), velocity), 1.0e-15);
399 Assert.assertEquals(0, Vector3D.distance(rebuilt.getPVCoordinates().getAcceleration(), acceleration), 1.0e-15);
400
401 }
402
403 @Test
404 public void testShiftElliptic() {
405 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
406 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
407 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
408 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
409 testShift(orbit, new KeplerianOrbit(orbit), 1.0e-13);
410 }
411
412 @Test
413 public void testShiftCircular() {
414 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
415 Vector3D velocity = new Vector3D(FastMath.sqrt(mu / position.getNorm()), position.orthogonal());
416 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
417 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
418 testShift(orbit, new CircularOrbit(orbit), 1.0e-15);
419 }
420
421 @Test
422 public void testShiftEquinoctial() {
423 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
424 Vector3D velocity = new Vector3D(FastMath.sqrt(mu / position.getNorm()), position.orthogonal());
425 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
426 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
427 testShift(orbit, new EquinoctialOrbit(orbit), 5.0e-14);
428 }
429
430 @Test
431 public void testShiftHyperbolic() {
432 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
433 Vector3D velocity = new Vector3D(3 * FastMath.sqrt(mu / position.getNorm()), position.orthogonal());
434 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
435 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
436 testShift(orbit, new KeplerianOrbit(orbit), 2.0e-15);
437 }
438
439 @Test
440 public void testNumericalIssue135() {
441 Vector3D position = new Vector3D(-6.7884943832e7, -2.1423006112e7, -3.1603915377e7);
442 Vector3D velocity = new Vector3D(-4732.55, -2472.086, -3022.177);
443 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
444 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date,
445 324858598826460.);
446 testShift(orbit, new KeplerianOrbit(orbit), 6.0e-15);
447 }
448
449 private void testShift(CartesianOrbit tested, Orbit reference, double threshold) {
450 for (double dt = - 1000; dt < 1000; dt += 10.0) {
451
452 PVCoordinates pvTested = tested.shiftedBy(dt).getPVCoordinates();
453 Vector3D pTested = pvTested.getPosition();
454 Vector3D vTested = pvTested.getVelocity();
455
456 PVCoordinates pvReference = reference.shiftedBy(dt).getPVCoordinates();
457 Vector3D pReference = pvReference.getPosition();
458 Vector3D vReference = pvReference.getVelocity();
459
460 Assert.assertEquals(0, pTested.subtract(pReference).getNorm(), threshold * pReference.getNorm());
461 Assert.assertEquals(0, vTested.subtract(vReference).getNorm(), threshold * vReference.getNorm());
462
463 }
464 }
465
466 @Test(expected=IllegalArgumentException.class)
467 public void testNonInertialFrame() throws IllegalArgumentException {
468
469 Vector3D position = new Vector3D(-26655470.0, 29881667.0, -113657.0);
470 Vector3D velocity = new Vector3D(-1125.0, -1122.0, 195.0);
471 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
472 double mu = 3.9860047e14;
473 new CartesianOrbit(pvCoordinates,
474 new Frame(FramesFactory.getEME2000(), Transform.IDENTITY, "non-inertial", false),
475 date, mu);
476 }
477
478 @Test
479 public void testJacobianReference() {
480
481 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
482 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
483 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
484 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
485
486 double[][] jacobian = new double[6][6];
487 orbit.getJacobianWrtCartesian(PositionAngle.MEAN, jacobian);
488
489 for (int i = 0; i < jacobian.length; i++) {
490 double[] row = jacobian[i];
491 for (int j = 0; j < row.length; j++) {
492 Assert.assertEquals((i == j) ? 1 : 0, row[j], 1.0e-15);
493 }
494 }
495
496 double[][] invJacobian = new double[6][6];
497 orbit.getJacobianWrtParameters(PositionAngle.MEAN, invJacobian);
498 MatrixUtils.createRealMatrix(jacobian).
499 multiply(MatrixUtils.createRealMatrix(invJacobian)).
500 walkInRowOrder(new RealMatrixPreservingVisitor() {
501 public void start(int rows, int columns,
502 int startRow, int endRow, int startColumn, int endColumn) {
503 }
504
505 public void visit(int row, int column, double value) {
506 Assert.assertEquals(row == column ? 1.0 : 0.0, value, 1.0e-15);
507 }
508
509 public double end() {
510 return Double.NaN;
511 }
512 });
513
514 }
515
516 @Test
517 public void testInterpolationWithDerivatives() {
518 doTestInterpolation(true,
519 394, 2.28e-8, 3.21, 1.39e-9,
520 2474, 6842, 6.55, 186);
521 }
522
523 @Test
524 public void testInterpolationWithoutDerivatives() {
525 doTestInterpolation(false,
526 394, 2.61, 3.21, 0.154,
527 2474, 2.28e12, 6.55, 6.22e10);
528 }
529
530 private void doTestInterpolation(boolean useDerivatives,
531 double shiftPositionErrorWithin, double interpolationPositionErrorWithin,
532 double shiftVelocityErrorWithin, double interpolationVelocityErrorWithin,
533 double shiftPositionErrorFarPast, double interpolationPositionErrorFarPast,
534 double shiftVelocityErrorFarPast, double interpolationVelocityErrorFarPast)
535 {
536
537 final double ehMu = 3.9860047e14;
538 final double ae = 6.378137e6;
539 final double c20 = -1.08263e-3;
540 final double c30 = 2.54e-6;
541 final double c40 = 1.62e-6;
542 final double c50 = 2.3e-7;
543 final double c60 = -5.5e-7;
544
545 final AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
546 final Vector3D position = new Vector3D(3220103., 69623., 6449822.);
547 final Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
548 final CartesianOrbit initialOrbit = new CartesianOrbit(new PVCoordinates(position, velocity),
549 FramesFactory.getEME2000(), date, ehMu);
550
551 EcksteinHechlerPropagator propagator =
552 new EcksteinHechlerPropagator(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);
553
554
555 List<Orbit> sample = new ArrayList<Orbit>();
556 for (double dt = 0; dt < 251.0; dt += 60.0) {
557 Orbit orbit = propagator.propagate(date.shiftedBy(dt)).getOrbit();
558 if (!useDerivatives) {
559
560 double[] stateVector = new double[6];
561 orbit.getType().mapOrbitToArray(orbit, PositionAngle.TRUE, stateVector, null);
562 orbit = orbit.getType().mapArrayToOrbit(stateVector, null, PositionAngle.TRUE,
563 orbit.getDate(), orbit.getMu(), orbit.getFrame());
564 }
565 sample.add(orbit);
566 }
567
568
569
570
571
572 double maxShiftPError = 0;
573 double maxInterpolationPError = 0;
574 double maxShiftVError = 0;
575 double maxInterpolationVError = 0;
576 for (double dt = 0; dt < 240.0; dt += 1.0) {
577 AbsoluteDate t = initialOrbit.getDate().shiftedBy(dt);
578 PVCoordinates propagated = propagator.propagate(t).getPVCoordinates();
579 PVCoordinates shiftError = new PVCoordinates(propagated,
580 initialOrbit.shiftedBy(dt).getPVCoordinates());
581 PVCoordinates interpolationError = new PVCoordinates(propagated,
582 initialOrbit.interpolate(t, sample).getPVCoordinates());
583 maxShiftPError = FastMath.max(maxShiftPError,
584 shiftError.getPosition().getNorm());
585 maxInterpolationPError = FastMath.max(maxInterpolationPError,
586 interpolationError.getPosition().getNorm());
587 maxShiftVError = FastMath.max(maxShiftVError,
588 shiftError.getVelocity().getNorm());
589 maxInterpolationVError = FastMath.max(maxInterpolationVError,
590 interpolationError.getVelocity().getNorm());
591 }
592 Assert.assertEquals(shiftPositionErrorWithin, maxShiftPError, 0.01 * shiftPositionErrorWithin);
593 Assert.assertEquals(interpolationPositionErrorWithin, maxInterpolationPError, 0.01 * interpolationPositionErrorWithin);
594 Assert.assertEquals(shiftVelocityErrorWithin, maxShiftVError, 0.01 * shiftVelocityErrorWithin);
595 Assert.assertEquals(interpolationVelocityErrorWithin, maxInterpolationVError, 0.01 * interpolationVelocityErrorWithin);
596
597
598 maxShiftPError = 0;
599 maxInterpolationPError = 0;
600 maxShiftVError = 0;
601 maxInterpolationVError = 0;
602 for (double dt = 500.0; dt < 650.0; dt += 1.0) {
603 AbsoluteDate t = initialOrbit.getDate().shiftedBy(dt);
604 PVCoordinates propagated = propagator.propagate(t).getPVCoordinates();
605 PVCoordinates shiftError = new PVCoordinates(propagated,
606 initialOrbit.shiftedBy(dt).getPVCoordinates());
607 PVCoordinates interpolationError = new PVCoordinates(propagated,
608 initialOrbit.interpolate(t, sample).getPVCoordinates());
609 maxShiftPError = FastMath.max(maxShiftPError,
610 shiftError.getPosition().getNorm());
611 maxInterpolationPError = FastMath.max(maxInterpolationPError,
612 interpolationError.getPosition().getNorm());
613 maxShiftVError = FastMath.max(maxShiftVError,
614 shiftError.getVelocity().getNorm());
615 maxInterpolationVError = FastMath.max(maxInterpolationVError,
616 interpolationError.getVelocity().getNorm());
617 }
618 Assert.assertEquals(shiftPositionErrorFarPast, maxShiftPError, 0.01 * shiftPositionErrorFarPast);
619 Assert.assertEquals(interpolationPositionErrorFarPast, maxInterpolationPError, 0.01 * interpolationPositionErrorFarPast);
620 Assert.assertEquals(shiftVelocityErrorFarPast, maxShiftVError, 0.01 * shiftVelocityErrorFarPast);
621 Assert.assertEquals(interpolationVelocityErrorFarPast, maxInterpolationVError, 0.01 * interpolationVelocityErrorFarPast);
622
623 }
624
625 @Test
626 public void testNonKeplerianDerivatives() {
627 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
628 final Vector3D position = new Vector3D(6896874.444705, 1956581.072644, -147476.245054);
629 final Vector3D velocity = new Vector3D(166.816407662, -1106.783301861, -7372.745712770);
630 final Vector3D acceleration = new Vector3D(-7.466182457944, -2.118153357345, 0.160004048437);
631 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, position, velocity, acceleration);
632 final Frame frame = FramesFactory.getEME2000();
633 final double mu = Constants.EIGEN5C_EARTH_MU;
634 final CartesianOrbit orbit = new CartesianOrbit(pv, frame, mu);
635
636 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getA()),
637 orbit.getADot(),
638 4.3e-8);
639 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEx()),
640 orbit.getEquinoctialExDot(),
641 2.1e-15);
642 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEy()),
643 orbit.getEquinoctialEyDot(),
644 5.3e-16);
645 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHx()),
646 orbit.getHxDot(),
647 4.4e-15);
648 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHy()),
649 orbit.getHyDot(),
650 8.0e-16);
651 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLv()),
652 orbit.getLvDot(),
653 1.2e-15);
654 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLE()),
655 orbit.getLEDot(),
656 7.8e-16);
657 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLM()),
658 orbit.getLMDot(),
659 8.8e-16);
660 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getE()),
661 orbit.getEDot(),
662 7.0e-16);
663 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getI()),
664 orbit.getIDot(),
665 5.7e-16);
666
667 }
668
669 private <S extends Function<CartesianOrbit, Double>>
670 double differentiate(TimeStampedPVCoordinates pv, Frame frame, double mu, S picker) {
671 final DSFactory factory = new DSFactory(1, 1);
672 FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
673 UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {
674 public double value(double dt) {
675 return picker.apply(new CartesianOrbit(pv.shiftedBy(dt), frame, mu));
676 }
677 });
678 return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
679 }
680
681 @Test
682 public void testEquatorialRetrograde() {
683 Vector3D position = new Vector3D(10000000.0, 0.0, 0.0);
684 Vector3D velocity = new Vector3D(0.0, -6500.0, 0.0);
685 double r2 = position.getNormSq();
686 double r = FastMath.sqrt(r2);
687 Vector3D acceleration = new Vector3D(-mu / (r * r2), position,
688 1, new Vector3D(-0.1, 0.2, 0.3));
689 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
690 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
691 Assert.assertEquals(10637829.465, orbit.getA(), 1.0e-3);
692 Assert.assertEquals(-738.145, orbit.getADot(), 1.0e-3);
693 Assert.assertEquals(0.05995861, orbit.getE(), 1.0e-8);
694 Assert.assertEquals(-6.523e-5, orbit.getEDot(), 1.0e-8);
695 Assert.assertEquals(FastMath.PI, orbit.getI(), 1.0e-15);
696 Assert.assertTrue(Double.isNaN(orbit.getIDot()));
697 Assert.assertTrue(Double.isNaN(orbit.getHx()));
698 Assert.assertTrue(Double.isNaN(orbit.getHxDot()));
699 Assert.assertTrue(Double.isNaN(orbit.getHy()));
700 Assert.assertTrue(Double.isNaN(orbit.getHyDot()));
701 }
702
703 @Test
704 public void testToString() {
705 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
706 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
707 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
708 CartesianOrbit orbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
709 Assert.assertEquals("Cartesian parameters: {P(-2.9536113E7, 3.0329259E7, -100125.0), V(-2194.0, -2141.0, -8.0)}",
710 orbit.toString());
711 }
712
713 @Test
714 public void testCopyNonKeplerianAcceleration() {
715
716 final Frame eme2000 = FramesFactory.getEME2000();
717
718
719 final Vector3D position = new Vector3D(42164140, 0, 0);
720
721 final PVCoordinates pv = new PVCoordinates(position,
722 new Vector3D(0, FastMath.sqrt(mu / position.getNorm()), 0));
723
724 final Orbit orbit = new CartesianOrbit(pv, eme2000, date, mu);
725
726
727 final Orbit orbitCopy = new CartesianOrbit(orbit);
728
729
730 final Orbit shiftedOrbit = orbit.shiftedBy(10);
731 final Orbit shiftedOrbitCopy = orbitCopy.shiftedBy(10);
732
733 Assert.assertEquals(0.0,
734 Vector3D.distance(shiftedOrbit.getPVCoordinates().getPosition(),
735 shiftedOrbitCopy.getPVCoordinates().getPosition()),
736 1.0e-10);
737 Assert.assertEquals(0.0,
738 Vector3D.distance(shiftedOrbit.getPVCoordinates().getVelocity(),
739 shiftedOrbitCopy.getPVCoordinates().getVelocity()),
740 1.0e-10);
741
742 }
743
744 @Test
745 public void testNormalize() {
746 final Vector3D position = new Vector3D(42164140, 0, 0);
747 final PVCoordinates pv = new PVCoordinates(position,
748 new Vector3D(0, FastMath.sqrt(mu / position.getNorm()), 0));
749 final Orbit orbit = new CartesianOrbit(pv, FramesFactory.getEME2000(), date, mu);
750 Assert.assertSame(orbit, orbit.getType().normalize(orbit, null));
751 }
752
753 @Before
754 public void setUp() {
755
756 Utils.setDataRoot("regular-data");
757
758
759 date = AbsoluteDate.J2000_EPOCH;
760
761
762 mu = 3.9860047e14;
763 }
764
765 @After
766 public void tearDown() {
767 date = null;
768 }
769
770 }
771