1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import org.hamcrest.MatcherAssert;
20 import org.hipparchus.analysis.UnivariateFunction;
21 import org.hipparchus.analysis.differentiation.DSFactory;
22 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
23 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.linear.MatrixUtils;
26 import org.hipparchus.linear.RealMatrixPreservingVisitor;
27 import org.hipparchus.util.FastMath;
28 import org.hipparchus.util.MathUtils;
29 import org.junit.jupiter.api.*;
30 import org.junit.jupiter.params.ParameterizedTest;
31 import org.junit.jupiter.params.provider.EnumSource;
32 import org.orekit.Utils;
33 import org.orekit.errors.OrekitIllegalArgumentException;
34 import org.orekit.errors.OrekitMessages;
35 import org.orekit.frames.Frame;
36 import org.orekit.frames.FramesFactory;
37 import org.orekit.frames.Transform;
38 import org.orekit.time.AbsoluteDate;
39 import org.orekit.time.TimeScalesFactory;
40 import org.orekit.utils.Constants;
41 import org.orekit.utils.PVCoordinates;
42 import org.orekit.utils.TimeStampedPVCoordinates;
43
44 import java.util.function.Function;
45
46 import static org.orekit.OrekitMatchers.relativelyCloseTo;
47
48
49 public class EquinoctialOrbitTest {
50
51
52 private AbsoluteDate date;
53
54
55 private double mu;
56
57 @ParameterizedTest
58 @EnumSource(PositionAngleType.class)
59 void testWithCachedPositionAngleType(final PositionAngleType positionAngleType) {
60
61 final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
62 final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
63 final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
64 final double muEarth = 3.9860047e14;
65 final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, muEarth);
66 final EquinoctialOrbit equinoctialOrbit = new EquinoctialOrbit(cartesianOrbit);
67
68 final EquinoctialOrbit orbit = equinoctialOrbit.withCachedPositionAngleType(positionAngleType);
69
70 Assertions.assertEquals(equinoctialOrbit.getFrame(), orbit.getFrame());
71 Assertions.assertEquals(equinoctialOrbit.getDate(), orbit.getDate());
72 Assertions.assertEquals(equinoctialOrbit.getMu(), orbit.getMu());
73 final Vector3D relativePosition = equinoctialOrbit.getPosition(orbit.getFrame()).subtract(
74 orbit.getPosition());
75 Assertions.assertEquals(0., relativePosition.getNorm(), 1e-6);
76 Assertions.assertEquals(equinoctialOrbit.hasNonKeplerianAcceleration(),
77 orbit.hasNonKeplerianAcceleration());
78 }
79
80 @ParameterizedTest
81 @EnumSource(PositionAngleType.class)
82 void testInFrameKeplerian(final PositionAngleType positionAngleType) {
83 testTemplateInFrame(Vector3D.ZERO, positionAngleType);
84 }
85
86 @Test
87 void testInFrameNonKeplerian() {
88 testTemplateInFrame(Vector3D.PLUS_K, PositionAngleType.TRUE);
89 }
90
91 private void testTemplateInFrame(final Vector3D acceleration, final PositionAngleType positionAngleType) {
92
93 final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
94 final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
95 final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
96 final double muEarth = 3.9860047e14;
97 final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, muEarth);
98 final EquinoctialOrbit equinoctialOrbit = new EquinoctialOrbit(cartesianOrbit).withCachedPositionAngleType(positionAngleType);
99
100 final EquinoctialOrbit orbitWithOtherFrame = equinoctialOrbit.inFrame(FramesFactory.getGCRF());
101
102 Assertions.assertNotEquals(equinoctialOrbit.getFrame(), orbitWithOtherFrame.getFrame());
103 Assertions.assertEquals(equinoctialOrbit.getDate(), orbitWithOtherFrame.getDate());
104 Assertions.assertEquals(equinoctialOrbit.getMu(), orbitWithOtherFrame.getMu());
105 final Vector3D relativePosition = equinoctialOrbit.getPosition(orbitWithOtherFrame.getFrame()).subtract(
106 orbitWithOtherFrame.getPosition());
107 Assertions.assertEquals(0., relativePosition.getNorm(), 1e-6);
108 Assertions.assertEquals(equinoctialOrbit.hasNonKeplerianAcceleration(),
109 orbitWithOtherFrame.hasNonKeplerianAcceleration());
110 }
111
112 @Test
113 void testEquinoctialToEquinoctialEll() {
114
115 double ix = 1.200e-04;
116 double iy = -1.16e-04;
117 double inc = 2 * FastMath.asin(FastMath.sqrt((ix * ix + iy * iy) / 4.));
118 double hx = FastMath.tan(inc / 2.) * ix / (2 * FastMath.sin(inc / 2.));
119 double hy = FastMath.tan(inc / 2.) * iy / (2 * FastMath.sin(inc / 2.));
120
121
122 EquinoctialOrbit equi =
123 new EquinoctialOrbit(42166712.0, 0.5, -0.5, hx, hy,
124 5.300, PositionAngleType.MEAN,
125 FramesFactory.getEME2000(), date, mu);
126 Vector3D pos = equi.getPosition();
127 Vector3D vit = equi.getPVCoordinates().getVelocity();
128
129 PVCoordinates pvCoordinates = new PVCoordinates(pos, vit);
130
131 EquinoctialOrbit param = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
132 Assertions.assertEquals(param.getA(), equi.getA(), Utils.epsilonTest * equi.getA());
133 Assertions.assertEquals(param.getEquinoctialEx(), equi.getEquinoctialEx(),
134 Utils.epsilonE * FastMath.abs(equi.getE()));
135 Assertions.assertEquals(param.getEquinoctialEy(), equi.getEquinoctialEy(),
136 Utils.epsilonE * FastMath.abs(equi.getE()));
137 Assertions.assertEquals(param.getHx(), equi.getHx(), Utils.epsilonAngle
138 * FastMath.abs(equi.getI()));
139 Assertions.assertEquals(param.getHy(), equi.getHy(), Utils.epsilonAngle
140 * FastMath.abs(equi.getI()));
141 Assertions.assertEquals(MathUtils.normalizeAngle(param.getLv(), equi.getLv()), equi.getLv(),
142 Utils.epsilonAngle * FastMath.abs(equi.getLv()));
143
144 }
145
146 @Test
147 void testEquinoctialToEquinoctialCirc() {
148
149 double ix = 1.200e-04;
150 double iy = -1.16e-04;
151 double inc = 2 * FastMath.asin(FastMath.sqrt((ix * ix + iy * iy) / 4.));
152 double hx = FastMath.tan(inc / 2.) * ix / (2 * FastMath.sin(inc / 2.));
153 double hy = FastMath.tan(inc / 2.) * iy / (2 * FastMath.sin(inc / 2.));
154
155
156 EquinoctialOrbit equiCir =
157 new EquinoctialOrbit(42166712.0, 0.1e-10, -0.1e-10, hx, hy,
158 5.300, PositionAngleType.MEAN,
159 FramesFactory.getEME2000(), date, mu);
160 Vector3D posCir = equiCir.getPosition();
161 Vector3D vitCir = equiCir.getPVCoordinates().getVelocity();
162
163 PVCoordinates pvCoordinates = new PVCoordinates(posCir, vitCir);
164
165 EquinoctialOrbit paramCir = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(),
166 date, mu);
167 Assertions.assertEquals(paramCir.getA(), equiCir.getA(), Utils.epsilonTest
168 * equiCir.getA());
169 Assertions.assertEquals(paramCir.getEquinoctialEx(), equiCir.getEquinoctialEx(),
170 Utils.epsilonEcir * FastMath.abs(equiCir.getE()));
171 Assertions.assertEquals(paramCir.getEquinoctialEy(), equiCir.getEquinoctialEy(),
172 Utils.epsilonEcir * FastMath.abs(equiCir.getE()));
173 Assertions.assertEquals(paramCir.getHx(), equiCir.getHx(), Utils.epsilonAngle
174 * FastMath.abs(equiCir.getI()));
175 Assertions.assertEquals(paramCir.getHy(), equiCir.getHy(), Utils.epsilonAngle
176 * FastMath.abs(equiCir.getI()));
177 Assertions.assertEquals(MathUtils.normalizeAngle(paramCir.getLv(), equiCir.getLv()), equiCir
178 .getLv(), Utils.epsilonAngle * FastMath.abs(equiCir.getLv()));
179
180 }
181
182 @Test
183 void testEquinoctialToCartesian() {
184
185 double ix = 1.200e-04;
186 double iy = -1.16e-04;
187 double inc = 2 * FastMath.asin(FastMath.sqrt((ix * ix + iy * iy) / 4.));
188 double hx = FastMath.tan(inc / 2.) * ix / (2 * FastMath.sin(inc / 2.));
189 double hy = FastMath.tan(inc / 2.) * iy / (2 * FastMath.sin(inc / 2.));
190
191 EquinoctialOrbit equi =
192 new EquinoctialOrbit(42166712.0, -7.900e-06, 1.100e-04, hx, hy,
193 5.300, PositionAngleType.MEAN,
194 FramesFactory.getEME2000(), date, mu);
195 Vector3D pos = equi.getPosition();
196 Vector3D vit = equi.getPVCoordinates().getVelocity();
197
198
199 double oneovera = (2. / pos.getNorm()) - vit.getNorm() * vit.getNorm() / mu;
200 Assertions.assertEquals(oneovera, 1. / equi.getA(), 1.0e-7);
201
202 Assertions.assertEquals( 0.233745668678733e+08, pos.getX(), Utils.epsilonTest * FastMath.abs(pos.getX()));
203 Assertions.assertEquals(-0.350998914352669e+08, pos.getY(), Utils.epsilonTest * FastMath.abs(pos.getY()));
204 Assertions.assertEquals(-0.150053723123334e+04, pos.getZ(), Utils.epsilonTest * FastMath.abs(pos.getZ()));
205
206 Assertions.assertEquals(2558.7096558809967, vit.getX(), Utils.epsilonTest * FastMath.abs(vit.getX()));
207 Assertions.assertEquals(1704.1586039092576, vit.getY(), Utils.epsilonTest * FastMath.abs(vit.getY()));
208 Assertions.assertEquals( 0.5013093577879, vit.getZ(), Utils.epsilonTest * FastMath.abs(vit.getZ()));
209
210 }
211
212 @Test
213 void testEquinoctialToKeplerian() {
214
215 double ix = 1.20e-4;
216 double iy = -1.16e-4;
217 double i = 2 * FastMath.asin(FastMath.sqrt((ix * ix + iy * iy) / 4));
218 double hx = FastMath.tan(i / 2) * ix / (2 * FastMath.sin(i / 2));
219 double hy = FastMath.tan(i / 2) * iy / (2 * FastMath.sin(i / 2));
220
221 EquinoctialOrbit equi =
222 new EquinoctialOrbit(42166712.0, -7.900e-6, 1.100e-4, hx, hy,
223 5.300, PositionAngleType.MEAN,
224 FramesFactory.getEME2000(), date, mu);
225 KeplerianOrbit kep = new KeplerianOrbit(equi);
226
227 Assertions.assertEquals(42166712.000, equi.getA(), Utils.epsilonTest * kep.getA());
228 Assertions.assertEquals(0.110283316961361e-03, kep.getE(), Utils.epsilonE
229 * FastMath.abs(kep.getE()));
230 Assertions.assertEquals(0.166901168553917e-03, kep.getI(), Utils.epsilonAngle
231 * FastMath.abs(kep.getI()));
232 Assertions.assertEquals(MathUtils.normalizeAngle(-3.87224326008837, kep.getPerigeeArgument()),
233 kep.getPerigeeArgument(), Utils.epsilonTest
234 * FastMath.abs(kep.getPerigeeArgument()));
235 Assertions.assertEquals(MathUtils.normalizeAngle(5.51473467358854, kep
236 .getRightAscensionOfAscendingNode()), kep
237 .getRightAscensionOfAscendingNode(), Utils.epsilonTest
238 * FastMath.abs(kep.getRightAscensionOfAscendingNode()));
239 Assertions.assertEquals(MathUtils.normalizeAngle(3.65750858649982, kep.getMeanAnomaly()), kep
240 .getMeanAnomaly(), Utils.epsilonTest * FastMath.abs(kep.getMeanAnomaly()));
241
242 }
243
244 @Test
245 void testHyperbolic1() {
246 try {
247 new EquinoctialOrbit(-42166712.0, 1.9, 0.5, 0.01, -0.02, 5.300,
248 PositionAngleType.MEAN, FramesFactory.getEME2000(), date, mu);
249 Assertions.fail("an exception should have been thrown");
250 } catch (OrekitIllegalArgumentException oe) {
251 Assertions.assertEquals(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, oe.getSpecifier());
252 }
253 }
254
255
256 @Test
257 void testHyperbolic2() {
258 Orbit orbit = new KeplerianOrbit(-42166712.0, 1.9, 0.5, 0.01, -0.02, 5.300,
259 PositionAngleType.MEAN, FramesFactory.getEME2000(), date, mu);
260 try {
261 new EquinoctialOrbit(orbit.getPVCoordinates(), orbit.getFrame(), orbit.getMu());
262 Assertions.fail("an exception should have been thrown");
263 } catch (OrekitIllegalArgumentException oe) {
264 Assertions.assertEquals(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS, oe.getSpecifier());
265 }
266 }
267
268 @Test
269 void testNumericalIssue25() {
270 Vector3D position = new Vector3D(3782116.14107698, 416663.11924914, 5875541.62103057);
271 Vector3D velocity = new Vector3D(-6349.7848910501, 288.4061811651, 4066.9366759691);
272 EquinoctialOrbit orbit = new EquinoctialOrbit(new PVCoordinates(position, velocity),
273 FramesFactory.getEME2000(),
274 new AbsoluteDate("2004-01-01T23:00:00.000",
275 TimeScalesFactory.getUTC()),
276 3.986004415E14);
277 Assertions.assertEquals(0.0, orbit.getE(), 2.0e-14);
278 }
279
280
281 @Test
282 void testAnomaly() {
283
284
285 Vector3D position = new Vector3D(7.0e6, 1.0e6, 4.0e6);
286 Vector3D velocity = new Vector3D(-500.0, 8000.0, 1000.0);
287
288 EquinoctialOrbit p = new EquinoctialOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), date, mu);
289 KeplerianOrbit kep = new KeplerianOrbit(p);
290
291 double e = p.getE();
292 double eRatio = FastMath.sqrt((1 - e) / (1 + e));
293 double paPraan = kep.getPerigeeArgument()
294 + kep.getRightAscensionOfAscendingNode();
295
296 double lv = 1.1;
297
298 double lE = 2 * FastMath.atan(eRatio * FastMath.tan((lv - paPraan) / 2)) + paPraan;
299 double lM = lE - e * FastMath.sin(lE - paPraan);
300
301 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
302 p.getEquinoctialEy() , p.getHx(), p.getHy() , lv , PositionAngleType.TRUE,
303 p.getFrame(), p.getDate(), p.getMu());
304 Assertions.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
305 Assertions.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
306 Assertions.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
307 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
308 p.getEquinoctialEy() , p.getHx(), p.getHy() , 0 , PositionAngleType.TRUE,
309 p.getFrame(), p.getDate(), p.getMu());
310
311 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
312 p.getEquinoctialEy() , p.getHx(), p.getHy() , lE , PositionAngleType.ECCENTRIC,
313 p.getFrame(), p.getDate(), p.getMu());
314 Assertions.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
315 Assertions.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
316 Assertions.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
317 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
318 p.getEquinoctialEy() , p.getHx(), p.getHy() , 0 , PositionAngleType.TRUE,
319 p.getFrame(), p.getDate(), p.getMu());
320
321 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
322 p.getEquinoctialEy() , p.getHx(), p.getHy() , lM , PositionAngleType.MEAN,
323 p.getFrame(), p.getDate(), p.getMu());
324 Assertions.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
325 Assertions.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
326 Assertions.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
327
328
329 p = new EquinoctialOrbit(p.getA(), 0 ,
330 0, p.getHx(), p.getHy() , p.getLv() , PositionAngleType.TRUE,
331 p.getFrame(), p.getDate(), p.getMu());
332
333 lE = lv;
334 lM = lE;
335
336 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
337 p.getEquinoctialEy() , p.getHx(), p.getHy() , lv , PositionAngleType.TRUE,
338 p.getFrame(), p.getDate(), p.getMu());
339 Assertions.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
340 Assertions.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
341 Assertions.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
342 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
343 p.getEquinoctialEy() , p.getHx(), p.getHy() , 0 , PositionAngleType.TRUE,
344 p.getFrame(), p.getDate(), p.getMu());
345
346 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
347 p.getEquinoctialEy() , p.getHx(), p.getHy() , lE , PositionAngleType.ECCENTRIC,
348 p.getFrame(), p.getDate(), p.getMu());
349 Assertions.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
350 Assertions.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
351 Assertions.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
352 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
353 p.getEquinoctialEy() , p.getHx(), p.getHy() , 0 , PositionAngleType.TRUE,
354 p.getFrame(), p.getDate(), p.getMu());
355
356 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
357 p.getEquinoctialEy() , p.getHx(), p.getHy() , lM , PositionAngleType.MEAN, p.getFrame(), p.getDate(), p.getMu());
358 Assertions.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
359 Assertions.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
360 Assertions.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
361 }
362
363 @Test
364 void testPositionVelocityNorms() {
365
366
367 EquinoctialOrbit p =
368 new EquinoctialOrbit(42166712.0, 0.5, -0.5, 1.200, 2.1,
369 0.67, PositionAngleType.TRUE,
370 FramesFactory.getEME2000(), date, mu);
371
372 double ex = p.getEquinoctialEx();
373 double ey = p.getEquinoctialEy();
374 double lv = p.getLv();
375 double ksi = 1 + ex * FastMath.cos(lv) + ey * FastMath.sin(lv);
376 double nu = ex * FastMath.sin(lv) - ey * FastMath.cos(lv);
377 double epsilon = FastMath.sqrt(1 - ex * ex - ey * ey);
378
379 double a = p.getA();
380 double na = FastMath.sqrt(p.getMu() / a);
381
382 Assertions.assertEquals(a * epsilon * epsilon / ksi, p.getPosition().getNorm(),
383 Utils.epsilonTest * FastMath.abs(p.getPosition().getNorm()));
384 Assertions.assertEquals(na * FastMath.sqrt(ksi * ksi + nu * nu) / epsilon, p
385 .getPVCoordinates().getVelocity().getNorm(), Utils.epsilonTest
386 * FastMath.abs(p.getPVCoordinates().getVelocity().getNorm()));
387
388
389 EquinoctialOrbit pCirEqua =
390 new EquinoctialOrbit(42166712.0, 0.1e-8, 0.1e-8, 0.1e-8, 0.1e-8,
391 0.67, PositionAngleType.TRUE,
392 FramesFactory.getEME2000(), date, mu);
393
394 ex = pCirEqua.getEquinoctialEx();
395 ey = pCirEqua.getEquinoctialEy();
396 lv = pCirEqua.getLv();
397 ksi = 1 + ex * FastMath.cos(lv) + ey * FastMath.sin(lv);
398 nu = ex * FastMath.sin(lv) - ey * FastMath.cos(lv);
399 epsilon = FastMath.sqrt(1 - ex * ex - ey * ey);
400
401 a = pCirEqua.getA();
402 na = FastMath.sqrt(pCirEqua.getMu() / a);
403
404 Assertions.assertEquals(a * epsilon * epsilon / ksi, pCirEqua.getPosition()
405 .getNorm(), Utils.epsilonTest
406 * FastMath.abs(pCirEqua.getPosition().getNorm()));
407 Assertions.assertEquals(na * FastMath.sqrt(ksi * ksi + nu * nu) / epsilon, pCirEqua
408 .getPVCoordinates().getVelocity().getNorm(), Utils.epsilonTest
409 * FastMath.abs(pCirEqua.getPVCoordinates().getVelocity().getNorm()));
410 }
411
412 @Test
413 void testGeometry() {
414
415
416 EquinoctialOrbit p =
417 new EquinoctialOrbit(42166712.0, 0.5, -0.5, 1.200, 2.1,
418 0.67, PositionAngleType.TRUE,
419 FramesFactory.getEME2000(), date, mu);
420
421 Vector3D position = p.getPosition();
422 Vector3D velocity = p.getPVCoordinates().getVelocity();
423 Vector3D momentum = p.getPVCoordinates().getMomentum().normalize();
424
425 double apogeeRadius = p.getA() * (1 + p.getE());
426 double perigeeRadius = p.getA() * (1 - p.getE());
427
428 for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI / 100.) {
429 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
430 p.getEquinoctialEy() , p.getHx(), p.getHy() , lv , PositionAngleType.TRUE,
431 p.getFrame(), p.getDate(), p.getMu());
432 position = p.getPosition();
433
434
435
436
437 Assertions.assertTrue((position.getNorm() - apogeeRadius) <= (apogeeRadius * Utils.epsilonTest));
438 Assertions.assertTrue((position.getNorm() - perigeeRadius) >= (-perigeeRadius * Utils.epsilonTest));
439
440 position = position.normalize();
441 velocity = p.getPVCoordinates().getVelocity();
442 velocity = velocity.normalize();
443
444
445
446
447
448 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest);
449
450 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest);
451 }
452
453
454 EquinoctialOrbit pCirEqua =
455 new EquinoctialOrbit(42166712.0, 0.1e-8, 0.1e-8, 0.1e-8, 0.1e-8,
456 0.67, PositionAngleType.TRUE,
457 FramesFactory.getEME2000(), date, mu);
458
459 position = pCirEqua.getPosition();
460 velocity = pCirEqua.getPVCoordinates().getVelocity();
461
462 momentum = Vector3D.crossProduct(position, velocity).normalize();
463
464 apogeeRadius = pCirEqua.getA() * (1 + pCirEqua.getE());
465 perigeeRadius = pCirEqua.getA() * (1 - pCirEqua.getE());
466
467 Assertions.assertEquals(perigeeRadius, apogeeRadius, 1.e+4 * Utils.epsilonTest
468 * apogeeRadius);
469
470 for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI / 100.) {
471 pCirEqua = new EquinoctialOrbit(pCirEqua.getA(), pCirEqua.getEquinoctialEx(),
472 pCirEqua.getEquinoctialEy() , pCirEqua.getHx(), pCirEqua.getHy() , lv , PositionAngleType.TRUE,
473 pCirEqua.getFrame(), p.getDate(), p.getMu());
474 position = pCirEqua.getPosition();
475
476
477
478 Assertions.assertTrue((position.getNorm() - apogeeRadius) <= (apogeeRadius * Utils.epsilonTest));
479 Assertions.assertTrue((position.getNorm() - perigeeRadius) >= (-perigeeRadius * Utils.epsilonTest));
480
481 position = position.normalize();
482 velocity = pCirEqua.getPVCoordinates().getVelocity();
483 velocity = velocity.normalize();
484
485
486
487
488
489 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest);
490
491 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest);
492 }
493 }
494
495
496 @Test
497 void testRadiusOfCurvature() {
498
499
500 EquinoctialOrbit p =
501 new EquinoctialOrbit(42166712.0, 0.5, -0.5, 1.200, 2.1,
502 0.67, PositionAngleType.TRUE,
503 FramesFactory.getEME2000(), date, mu);
504
505
506 Vector3D u = p.getPVCoordinates().getMomentum().orthogonal();
507 Vector3D v = Vector3D.crossProduct(p.getPVCoordinates().getMomentum(), u).normalize();
508
509
510 double xDot = Vector3D.dotProduct(p.getPVCoordinates().getVelocity(), u);
511 double yDot = Vector3D.dotProduct(p.getPVCoordinates().getVelocity(), v);
512 double xDotDot = Vector3D.dotProduct(p.getPVCoordinates().getAcceleration(), u);
513 double yDotDot = Vector3D.dotProduct(p.getPVCoordinates().getAcceleration(), v);
514 double dot2 = xDot * xDot + yDot * yDot;
515 double rCart = dot2 * FastMath.sqrt(dot2) /
516 FastMath.abs(xDot * yDotDot - yDot * xDotDot);
517
518
519 double ex = p.getEquinoctialEx();
520 double ey = p.getEquinoctialEy();
521 double f = ex * FastMath.cos(p.getLE()) + ey * FastMath.sin(p.getLE());
522 double oMf2 = 1 - f * f;
523 double rOrb = p.getA() * oMf2 * FastMath.sqrt(oMf2 / (1 - (ex * ex + ey * ey)));
524
525
526 Assertions.assertEquals(rCart, rOrb, 1.0e-15 * p.getA());
527
528
529
530 Assertions.assertEquals(0.8477 * p.getA(), rCart, 1.0e-4 * p.getA());
531
532 }
533
534 @Test
535 void testSymmetry() {
536
537
538 Vector3D position = new Vector3D(4512.9, 18260., -5127.);
539 Vector3D velocity = new Vector3D(134664.6, 90066.8, 72047.6);
540
541 EquinoctialOrbit p = new EquinoctialOrbit(new PVCoordinates(position, velocity),
542 FramesFactory.getEME2000(), date, mu);
543
544 Vector3D positionOffset = p.getPosition().subtract(position);
545 Vector3D velocityOffset = p.getPVCoordinates().getVelocity().subtract(velocity);
546
547 Assertions.assertEquals(0, positionOffset.getNorm(), 7.5e-12);
548 Assertions.assertEquals(0, velocityOffset.getNorm(), 1.0e-15);
549
550
551 position = new Vector3D(33051.2, 26184.9, -1.3E-5);
552 velocity = new Vector3D(-60376.2, 76208., 2.7E-4);
553
554 p = new EquinoctialOrbit(new PVCoordinates(position, velocity),
555 FramesFactory.getEME2000(), date, mu);
556
557 positionOffset = p.getPosition().subtract(position);
558 velocityOffset = p.getPVCoordinates().getVelocity().subtract(velocity);
559
560 Assertions.assertEquals(0, positionOffset.getNorm(), 1.1e-11);
561 Assertions.assertEquals(0, velocityOffset.getNorm(), 1.0e-15);
562 }
563
564 @Test
565 void testNonInertialFrame() throws IllegalArgumentException {
566 Assertions.assertThrows(IllegalArgumentException.class, () -> {
567 Vector3D position = new Vector3D(4512.9, 18260., -5127.);
568 Vector3D velocity = new Vector3D(134664.6, 90066.8, 72047.6);
569 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
570 new EquinoctialOrbit(pvCoordinates,
571 new Frame(FramesFactory.getEME2000(), Transform.IDENTITY, "non-inertial", false),
572 date, mu);
573 });
574 }
575
576 @Test
577 void testJacobianReference() {
578
579 AbsoluteDate dateTca = new AbsoluteDate(2000, 4, 1, 0, 0, 0.000, TimeScalesFactory.getUTC());
580 double mu = 3.986004415e+14;
581 EquinoctialOrbit orbEqu = new EquinoctialOrbit(7000000.0, 0.01, -0.02, 1.2, 2.1,
582 FastMath.toRadians(40.), PositionAngleType.MEAN,
583 FramesFactory.getEME2000(), dateTca, mu);
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654 Vector3D pRef = new Vector3D(2004367.298657628707588, 6575317.978060320019722, -1518024.843913963763043);
655 Vector3D vRef = new Vector3D(5574.048661495634406, -368.839015744295409, 5009.529487849066754);
656 double[][] jRef = {
657 { 0.56305379787310628, 1.8470954710993663, -0.42643364527246025, 1370.4369387322224, -90.682848736736688 , 1231.6441195141242 },
658 { 9.52434720041122055E-008, 9.49704503778007296E-008, 4.46607520107935678E-008, 1.69704446323098610E-004, 7.05603505855828105E-005, 1.14825140460141970E-004 },
659 { -5.41784097802642701E-008, 9.54903765833015538E-008, -8.95815777332234450E-008, 1.01864980963344096E-004, -1.03194262242761416E-004, 1.40668700715197768E-004 },
660 { 1.96680305426455816E-007, -1.12388745957974467E-007, -2.27118924123407353E-007, 2.06472886488132167E-004, -1.17984506564646906E-004, -2.38427023682723818E-004 },
661 { -2.24382495052235118E-007, 1.28218568601277626E-007, 2.59108357381747656E-007, 1.89034327703662092E-004, -1.08019615830663994E-004, -2.18289640324466583E-004 },
662 { -3.04001022071876804E-007, 1.22214683774559989E-007, 1.35141804810132761E-007, -1.34034616931480536E-004, -2.14283975204169379E-004, 1.29018773893081404E-004 }
663 };
664
665 PVCoordinates pv = orbEqu.getPVCoordinates();
666 Assertions.assertEquals(0, pv.getPosition().subtract(pRef).getNorm(), 2.0e-16 * pRef.getNorm());
667 Assertions.assertEquals(0, pv.getVelocity().subtract(vRef).getNorm(), 2.0e-16 * vRef.getNorm());
668
669 double[][] jacobian = new double[6][6];
670 orbEqu.getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
671
672 for (int i = 0; i < jacobian.length; i++) {
673 double[] row = jacobian[i];
674 double[] rowRef = jRef[i];
675 for (int j = 0; j < row.length; j++) {
676 Assertions.assertEquals(0, (row[j] - rowRef[j]) / rowRef[j], 4.0e-15);
677 }
678 }
679
680 }
681
682 @Test
683 void testJacobianFinitedifferences() {
684
685 AbsoluteDate dateTca = new AbsoluteDate(2000, 4, 1, 0, 0, 0.000, TimeScalesFactory.getUTC());
686 double mu = 3.986004415e+14;
687 EquinoctialOrbit orbEqu = new EquinoctialOrbit(7000000.0, 0.01, -0.02, 1.2, 2.1,
688 FastMath.toRadians(40.), PositionAngleType.MEAN,
689 FramesFactory.getEME2000(), dateTca, mu);
690
691 for (PositionAngleType type : PositionAngleType.values()) {
692 double hP = 2.0;
693 double[][] finiteDiffJacobian = finiteDifferencesJacobian(type, orbEqu, hP);
694 double[][] jacobian = new double[6][6];
695 orbEqu.getJacobianWrtCartesian(type, jacobian);
696
697 for (int i = 0; i < jacobian.length; i++) {
698 double[] row = jacobian[i];
699 double[] rowRef = finiteDiffJacobian[i];
700 for (int j = 0; j < row.length; j++) {
701 Assertions.assertEquals(0, (row[j] - rowRef[j]) / rowRef[j], 4.0e-9);
702 }
703 }
704
705 double[][] invJacobian = new double[6][6];
706 orbEqu.getJacobianWrtParameters(type, invJacobian);
707 MatrixUtils.createRealMatrix(jacobian).
708 multiply(MatrixUtils.createRealMatrix(invJacobian)).
709 walkInRowOrder(new RealMatrixPreservingVisitor() {
710 public void start(int rows, int columns,
711 int startRow, int endRow, int startColumn, int endColumn) {
712 }
713
714 public void visit(int row, int column, double value) {
715 Assertions.assertEquals(row == column ? 1.0 : 0.0, value, 7.0e-9);
716 }
717
718 public double end() {
719 return Double.NaN;
720 }
721 });
722
723 }
724
725 }
726
727 private double[][] finiteDifferencesJacobian(PositionAngleType type, EquinoctialOrbit orbit, double hP)
728 {
729 double[][] jacobian = new double[6][6];
730 for (int i = 0; i < 6; ++i) {
731 fillColumn(type, i, orbit, hP, jacobian);
732 }
733 return jacobian;
734 }
735
736 private void fillColumn(PositionAngleType type, int i, EquinoctialOrbit orbit, double hP, double[][] jacobian) {
737
738
739
740 Vector3D p = orbit.getPosition();
741 Vector3D v = orbit.getPVCoordinates().getVelocity();
742 double hV = orbit.getMu() * hP / (v.getNorm() * p.getNormSq());
743
744 double h;
745 Vector3D dP = Vector3D.ZERO;
746 Vector3D dV = Vector3D.ZERO;
747 switch (i) {
748 case 0:
749 h = hP;
750 dP = new Vector3D(hP, 0, 0);
751 break;
752 case 1:
753 h = hP;
754 dP = new Vector3D(0, hP, 0);
755 break;
756 case 2:
757 h = hP;
758 dP = new Vector3D(0, 0, hP);
759 break;
760 case 3:
761 h = hV;
762 dV = new Vector3D(hV, 0, 0);
763 break;
764 case 4:
765 h = hV;
766 dV = new Vector3D(0, hV, 0);
767 break;
768 default:
769 h = hV;
770 dV = new Vector3D(0, 0, hV);
771 break;
772 }
773
774 EquinoctialOrbit oM4h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, -4, dP), new Vector3D(1, v, -4, dV)),
775 orbit.getFrame(), orbit.getDate(), orbit.getMu());
776 EquinoctialOrbit oM3h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, -3, dP), new Vector3D(1, v, -3, dV)),
777 orbit.getFrame(), orbit.getDate(), orbit.getMu());
778 EquinoctialOrbit oM2h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, -2, dP), new Vector3D(1, v, -2, dV)),
779 orbit.getFrame(), orbit.getDate(), orbit.getMu());
780 EquinoctialOrbit oM1h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, -1, dP), new Vector3D(1, v, -1, dV)),
781 orbit.getFrame(), orbit.getDate(), orbit.getMu());
782 EquinoctialOrbit oP1h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, +1, dP), new Vector3D(1, v, +1, dV)),
783 orbit.getFrame(), orbit.getDate(), orbit.getMu());
784 EquinoctialOrbit oP2h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, +2, dP), new Vector3D(1, v, +2, dV)),
785 orbit.getFrame(), orbit.getDate(), orbit.getMu());
786 EquinoctialOrbit oP3h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, +3, dP), new Vector3D(1, v, +3, dV)),
787 orbit.getFrame(), orbit.getDate(), orbit.getMu());
788 EquinoctialOrbit oP4h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, +4, dP), new Vector3D(1, v, +4, dV)),
789 orbit.getFrame(), orbit.getDate(), orbit.getMu());
790
791 jacobian[0][i] = (-3 * (oP4h.getA() - oM4h.getA()) +
792 32 * (oP3h.getA() - oM3h.getA()) -
793 168 * (oP2h.getA() - oM2h.getA()) +
794 672 * (oP1h.getA() - oM1h.getA())) / (840 * h);
795 jacobian[1][i] = (-3 * (oP4h.getEquinoctialEx() - oM4h.getEquinoctialEx()) +
796 32 * (oP3h.getEquinoctialEx() - oM3h.getEquinoctialEx()) -
797 168 * (oP2h.getEquinoctialEx() - oM2h.getEquinoctialEx()) +
798 672 * (oP1h.getEquinoctialEx() - oM1h.getEquinoctialEx())) / (840 * h);
799 jacobian[2][i] = (-3 * (oP4h.getEquinoctialEy() - oM4h.getEquinoctialEy()) +
800 32 * (oP3h.getEquinoctialEy() - oM3h.getEquinoctialEy()) -
801 168 * (oP2h.getEquinoctialEy() - oM2h.getEquinoctialEy()) +
802 672 * (oP1h.getEquinoctialEy() - oM1h.getEquinoctialEy())) / (840 * h);
803 jacobian[3][i] = (-3 * (oP4h.getHx() - oM4h.getHx()) +
804 32 * (oP3h.getHx() - oM3h.getHx()) -
805 168 * (oP2h.getHx() - oM2h.getHx()) +
806 672 * (oP1h.getHx() - oM1h.getHx())) / (840 * h);
807 jacobian[4][i] = (-3 * (oP4h.getHy() - oM4h.getHy()) +
808 32 * (oP3h.getHy() - oM3h.getHy()) -
809 168 * (oP2h.getHy() - oM2h.getHy()) +
810 672 * (oP1h.getHy() - oM1h.getHy())) / (840 * h);
811 jacobian[5][i] = (-3 * (oP4h.getL(type) - oM4h.getL(type)) +
812 32 * (oP3h.getL(type) - oM3h.getL(type)) -
813 168 * (oP2h.getL(type) - oM2h.getL(type)) +
814 672 * (oP1h.getL(type) - oM1h.getL(type))) / (840 * h);
815
816 }
817
818 @Test
819 void testNonKeplerianDerivatives() {
820 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
821 final Vector3D position = new Vector3D(6896874.444705, 1956581.072644, -147476.245054);
822 final Vector3D velocity = new Vector3D(166.816407662, -1106.783301861, -7372.745712770);
823 final Vector3D acceleration = new Vector3D(-7.466182457944, -2.118153357345, 0.160004048437);
824 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, position, velocity, acceleration);
825 final Frame frame = FramesFactory.getEME2000();
826 final double mu = Constants.EIGEN5C_EARTH_MU;
827 final EquinoctialOrbit orbit = new EquinoctialOrbit(pv, frame, mu);
828
829 Assertions.assertEquals(differentiate(pv, frame, mu, EquinoctialOrbit::getA),
830 orbit.getADot(),
831 4.3e-8);
832 Assertions.assertEquals(differentiate(pv, frame, mu, EquinoctialOrbit::getEquinoctialEx),
833 orbit.getEquinoctialExDot(),
834 2.1e-15);
835 Assertions.assertEquals(differentiate(pv, frame, mu, EquinoctialOrbit::getEquinoctialEy),
836 orbit.getEquinoctialEyDot(),
837 5.3e-16);
838 Assertions.assertEquals(differentiate(pv, frame, mu, EquinoctialOrbit::getHx),
839 orbit.getHxDot(),
840 4.4e-15);
841 Assertions.assertEquals(differentiate(pv, frame, mu, EquinoctialOrbit::getHy),
842 orbit.getHyDot(),
843 1.5e-15);
844 Assertions.assertEquals(differentiate(pv, frame, mu, EquinoctialOrbit::getLv),
845 orbit.getLvDot(),
846 1.2e-15);
847 Assertions.assertEquals(differentiate(pv, frame, mu, EquinoctialOrbit::getLE),
848 orbit.getLEDot(),
849 7.7e-16);
850 Assertions.assertEquals(differentiate(pv, frame, mu, EquinoctialOrbit::getLM),
851 orbit.getLMDot(),
852 8.8e-16);
853 Assertions.assertEquals(differentiate(pv, frame, mu, EquinoctialOrbit::getE),
854 orbit.getEDot(),
855 6.9e-16);
856 Assertions.assertEquals(differentiate(pv, frame, mu, EquinoctialOrbit::getI),
857 orbit.getIDot(),
858 3.5e-15);
859 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getL(PositionAngleType.TRUE)),
860 orbit.getLDot(PositionAngleType.TRUE),
861 1.2e-15);
862 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getL(PositionAngleType.ECCENTRIC)),
863 orbit.getLDot(PositionAngleType.ECCENTRIC),
864 7.7e-16);
865 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getL(PositionAngleType.MEAN)),
866 orbit.getLDot(PositionAngleType.MEAN),
867 8.8e-16);
868
869 }
870
871 private <S extends Function<EquinoctialOrbit, Double>>
872 double differentiate(TimeStampedPVCoordinates pv, Frame frame, double mu, S picker) {
873 final DSFactory factory = new DSFactory(1, 1);
874 FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
875 UnivariateDifferentiableFunction diff = differentiator.differentiate((UnivariateFunction) dt ->
876 picker.apply(new EquinoctialOrbit(pv.shiftedBy(dt), frame, mu)));
877 return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
878 }
879
880 @Test
881 void testPositionAngleDerivatives() {
882 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
883 final Vector3D position = new Vector3D(6896874.444705, 1956581.072644, -147476.245054);
884 final Vector3D velocity = new Vector3D(166.816407662, -1106.783301861, -7372.745712770);
885 final Vector3D acceleration = new Vector3D(-7.466182457944, -2.118153357345, 0.160004048437);
886 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, position, velocity, acceleration);
887 final Frame frame = FramesFactory.getEME2000();
888 final double mu = Constants.EIGEN5C_EARTH_MU;
889 final EquinoctialOrbit orbit = new EquinoctialOrbit(pv, frame, mu);
890
891 for (PositionAngleType type : PositionAngleType.values()) {
892 final EquinoctialOrbit rebuilt = new EquinoctialOrbit(orbit.getA(),
893 orbit.getEquinoctialEx(),
894 orbit.getEquinoctialEy(),
895 orbit.getHx(),
896 orbit.getHy(),
897 orbit.getL(type),
898 orbit.getADot(),
899 orbit.getEquinoctialExDot(),
900 orbit.getEquinoctialEyDot(),
901 orbit.getHxDot(),
902 orbit.getHyDot(),
903 orbit.getLDot(type),
904 type, orbit.getFrame(), orbit.getDate(), orbit.getMu());
905 MatcherAssert.assertThat(rebuilt.getA(), relativelyCloseTo(orbit.getA(), 1));
906 MatcherAssert.assertThat(rebuilt.getEquinoctialEx(), relativelyCloseTo(orbit.getEquinoctialEx(), 1));
907 MatcherAssert.assertThat(rebuilt.getEquinoctialEy(), relativelyCloseTo(orbit.getEquinoctialEy(), 1));
908 MatcherAssert.assertThat(rebuilt.getHx(), relativelyCloseTo(orbit.getHx(), 1));
909 MatcherAssert.assertThat(rebuilt.getHy(), relativelyCloseTo(orbit.getHy(), 1));
910 MatcherAssert.assertThat(rebuilt.getADot(), relativelyCloseTo(orbit.getADot(), 1));
911 MatcherAssert.assertThat(rebuilt.getEquinoctialExDot(), relativelyCloseTo(orbit.getEquinoctialExDot(), 1));
912 MatcherAssert.assertThat(rebuilt.getEquinoctialEyDot(), relativelyCloseTo(orbit.getEquinoctialEyDot(), 1));
913 MatcherAssert.assertThat(rebuilt.getHxDot(), relativelyCloseTo(orbit.getHxDot(), 1));
914 MatcherAssert.assertThat(rebuilt.getHyDot(), relativelyCloseTo(orbit.getHyDot(), 1));
915 for (PositionAngleType type2 : PositionAngleType.values()) {
916 MatcherAssert.assertThat(rebuilt.getL(type2), relativelyCloseTo(orbit.getL(type2), 1));
917 MatcherAssert.assertThat(rebuilt.getLDot(type2), relativelyCloseTo(orbit.getLDot(type2), 1));
918 }
919 }
920
921 }
922
923 @Test
924 void testEquatorialRetrograde() {
925 Vector3D position = new Vector3D(10000000.0, 0.0, 0.0);
926 Vector3D velocity = new Vector3D(0.0, -6500.0, 0.0);
927 double r2 = position.getNormSq();
928 double r = FastMath.sqrt(r2);
929 Vector3D acceleration = new Vector3D(-mu / (r * r2), position,
930 1, new Vector3D(-0.1, 0.2, 0.3));
931 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
932
933
934
935 EquinoctialOrbit orbit = new EquinoctialOrbit(new KeplerianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu));
936 Assertions.assertEquals(10637829.465, orbit.getA(), 1.0e-3);
937 Assertions.assertEquals(-738.145, orbit.getADot(), 1.0e-3);
938 Assertions.assertEquals(0.05995861, orbit.getE(), 1.0e-8);
939 Assertions.assertEquals(-6.523e-5, orbit.getEDot(), 1.0e-8);
940 Assertions.assertTrue(Double.isNaN(orbit.getI()));
941 Assertions.assertTrue(Double.isNaN(orbit.getHx()));
942 Assertions.assertTrue(Double.isNaN(orbit.getHxDot()));
943 Assertions.assertTrue(Double.isNaN(orbit.getHy()));
944 Assertions.assertTrue(Double.isNaN(orbit.getHyDot()));
945 }
946
947 @Test
948 void testDerivativesConversionSymmetry() {
949 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:01:20.000", TimeScalesFactory.getUTC());
950 Vector3D position = new Vector3D(6893443.400234382, 1886406.1073757345, -589265.1150359757);
951 Vector3D velocity = new Vector3D(-281.1261461082365, -1231.6165642450928, -7348.756363469432);
952 Vector3D acceleration = new Vector3D(-7.460341170581685, -2.0415957334584527, 0.6393322823627762);
953 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity, acceleration);
954 EquinoctialOrbit orbit = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(),
955 date, Constants.EIGEN5C_EARTH_MU);
956 Assertions.assertTrue(orbit.hasNonKeplerianAcceleration());
957 double r2 = position.getNormSq();
958 double r = FastMath.sqrt(r2);
959 Vector3D keplerianAcceleration = new Vector3D(-orbit.getMu() / (r2 * r), position);
960 Assertions.assertEquals(0.0101, Vector3D.distance(keplerianAcceleration, acceleration), 1.0e-4);
961
962 for (OrbitType type : OrbitType.values()) {
963 Orbit converted = type.convertType(orbit);
964 Assertions.assertTrue(converted.hasNonKeplerianAcceleration());
965 EquinoctialOrbit rebuilt = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(converted);
966 Assertions.assertTrue(rebuilt.hasNonKeplerianAcceleration());
967 Assertions.assertEquals(orbit.getADot(), rebuilt.getADot(), 3.0e-13);
968 Assertions.assertEquals(orbit.getEquinoctialExDot(), rebuilt.getEquinoctialExDot(), 1.0e-15);
969 Assertions.assertEquals(orbit.getEquinoctialEyDot(), rebuilt.getEquinoctialEyDot(), 1.0e-15);
970 Assertions.assertEquals(orbit.getHxDot(), rebuilt.getHxDot(), 1.0e-15);
971 Assertions.assertEquals(orbit.getHyDot(), rebuilt.getHyDot(), 1.0e-15);
972 Assertions.assertEquals(orbit.getLvDot(), rebuilt.getLvDot(), 1.0e-15);
973 }
974
975 }
976
977 @Test
978 void testToString() {
979 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
980 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
981 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
982 EquinoctialOrbit orbit = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
983 Assertions.assertEquals("equinoctial parameters: {a: 4.225517000282565E7; ex: 5.927324978565528E-4; ey: -0.002062743969643666; hx: 6.401103130239252E-5; hy: -0.0017606836670756732; lv: 134.24111947709974;}",
984 orbit.toString());
985 }
986
987 @Test
988 void testWithKeplerianRates() {
989
990 final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
991 final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
992 final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
993 final EquinoctialOrbit orbit = new EquinoctialOrbit(pvCoordinates, FramesFactory.getGCRF(), date, mu);
994
995 final EquinoctialOrbit orbitWithoutRates = orbit.withKeplerianRates();
996
997 Assertions.assertFalse(orbitWithoutRates.hasNonKeplerianRates());
998 Assertions.assertEquals(orbit.getMu(), orbitWithoutRates.getMu());
999 Assertions.assertEquals(orbit.getDate(), orbitWithoutRates.getDate());
1000 Assertions.assertEquals(orbit.getFrame(), orbitWithoutRates.getFrame());
1001 Assertions.assertEquals(orbit.getA(), orbitWithoutRates.getA());
1002 Assertions.assertEquals(orbit.getEquinoctialEx(), orbitWithoutRates.getEquinoctialEx());
1003 Assertions.assertEquals(orbit.getEquinoctialEy(), orbitWithoutRates.getEquinoctialEy());
1004 Assertions.assertEquals(orbit.getHx(), orbitWithoutRates.getHx());
1005 Assertions.assertEquals(orbit.getHy(), orbitWithoutRates.getHy());
1006 Assertions.assertEquals(orbit.getLv(), orbitWithoutRates.getLv());
1007 }
1008
1009 @Test
1010 void testCopyNonKeplerianAcceleration() {
1011
1012 final Frame eme2000 = FramesFactory.getEME2000();
1013
1014
1015 final Vector3D position = new Vector3D(42164140, 0, 0);
1016
1017 final PVCoordinates pv = new PVCoordinates(position,
1018 new Vector3D(0, FastMath.sqrt(mu / position.getNorm()), 0));
1019
1020 final Orbit orbit = new EquinoctialOrbit(pv, eme2000, date, mu);
1021
1022
1023 final Orbit orbitCopy = new EquinoctialOrbit(orbit);
1024
1025
1026 final Orbit shiftedOrbit = orbit.shiftedBy(10);
1027 final Orbit shiftedOrbitCopy = orbitCopy.shiftedBy(10);
1028
1029 Assertions.assertEquals(0.0,
1030 Vector3D.distance(shiftedOrbit.getPosition(),
1031 shiftedOrbitCopy.getPosition()),
1032 1.0e-10);
1033 Assertions.assertEquals(0.0,
1034 Vector3D.distance(shiftedOrbit.getPVCoordinates().getVelocity(),
1035 shiftedOrbitCopy.getPVCoordinates().getVelocity()),
1036 1.0e-10);
1037
1038 }
1039
1040 @Test
1041 void testNormalize() {
1042 EquinoctialOrbit withoutDerivatives =
1043 new EquinoctialOrbit(42166712.0, 0.005, -0.025, 0.17, 0.34,
1044 0.4, PositionAngleType.MEAN,
1045 FramesFactory.getEME2000(), date, mu);
1046 EquinoctialOrbit ref =
1047 new EquinoctialOrbit(24000000.0, -0.012, 0.01, 0.2, 0.1,
1048 -6.28, PositionAngleType.MEAN,
1049 FramesFactory.getEME2000(), date, mu);
1050
1051 EquinoctialOrbit normalized1 = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.normalize(withoutDerivatives, ref);
1052 Assertions.assertFalse(normalized1.hasNonKeplerianAcceleration());
1053 Assertions.assertEquals(0.0, normalized1.getA() - withoutDerivatives.getA(), 1.0e-6);
1054 Assertions.assertEquals(0.0, normalized1.getEquinoctialEx() - withoutDerivatives.getEquinoctialEx(), 1.0e-10);
1055 Assertions.assertEquals(0.0, normalized1.getEquinoctialEy() - withoutDerivatives.getEquinoctialEy(), 1.0e-10);
1056 Assertions.assertEquals(0.0, normalized1.getHx() - withoutDerivatives.getHx(), 1.0e-10);
1057 Assertions.assertEquals(0.0, normalized1.getHy() - withoutDerivatives.getHy(), 1.0e-10);
1058 Assertions.assertEquals(-MathUtils.TWO_PI, normalized1.getLv() - withoutDerivatives.getLv(), 1.0e-10);
1059 Assertions.assertEquals(withoutDerivatives.getADot(), normalized1.getADot());
1060 Assertions.assertEquals(withoutDerivatives.getEquinoctialExDot(), normalized1.getEquinoctialExDot());
1061 Assertions.assertEquals(withoutDerivatives.getEquinoctialEyDot(), normalized1.getEquinoctialEyDot());
1062 Assertions.assertEquals(withoutDerivatives.getHxDot(), normalized1.getHxDot());
1063 Assertions.assertEquals(withoutDerivatives.getHyDot(), normalized1.getHyDot());
1064 Assertions.assertEquals(withoutDerivatives.getLvDot(), normalized1.getLvDot(), 1e-15);
1065
1066 double[] p = new double[6];
1067 OrbitType.EQUINOCTIAL.mapOrbitToArray(withoutDerivatives, PositionAngleType.TRUE, p, null);
1068 EquinoctialOrbit withDerivatives = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.mapArrayToOrbit(p,
1069 new double[] { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 },
1070 PositionAngleType.TRUE,
1071 withoutDerivatives.getDate(),
1072 withoutDerivatives.getMu(),
1073 withoutDerivatives.getFrame());
1074 EquinoctialOrbit normalized2 = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.normalize(withDerivatives, ref);
1075 Assertions.assertTrue(normalized2.hasNonKeplerianAcceleration());
1076 Assertions.assertFalse(normalized1.hasNonKeplerianAcceleration());
1077 Assertions.assertEquals(0.0, normalized1.getA() - withoutDerivatives.getA(), 1.0e-6);
1078 Assertions.assertEquals(0.0, normalized1.getEquinoctialEx() - withoutDerivatives.getEquinoctialEx(), 1.0e-10);
1079 Assertions.assertEquals(0.0, normalized1.getEquinoctialEy() - withoutDerivatives.getEquinoctialEy(), 1.0e-10);
1080 Assertions.assertEquals(0.0, normalized1.getHx() - withoutDerivatives.getHx(), 1.0e-10);
1081 Assertions.assertEquals(0.0, normalized1.getHy() - withoutDerivatives.getHy(), 1.0e-10);
1082 Assertions.assertEquals(-MathUtils.TWO_PI, normalized1.getLv() - withoutDerivatives.getLv(), 1.0e-10);
1083 Assertions.assertEquals(0.0, normalized2.getADot() - withDerivatives.getADot(), 1.0e-10);
1084 Assertions.assertEquals(0.0, normalized2.getEquinoctialExDot() - withDerivatives.getEquinoctialExDot(), 1.0e-10);
1085 Assertions.assertEquals(0.0, normalized2.getEquinoctialEyDot() - withDerivatives.getEquinoctialEyDot(), 1.0e-10);
1086 Assertions.assertEquals(0.0, normalized2.getHxDot() - withDerivatives.getHxDot(), 1.0e-10);
1087 Assertions.assertEquals(0.0, normalized2.getHyDot() - withDerivatives.getHyDot(), 1.0e-10);
1088 Assertions.assertEquals(0.0, normalized2.getLvDot() - withDerivatives.getLvDot(), 1.0e-10);
1089
1090 }
1091
1092 @Test
1093 void testCoverageCachedPositionAngleTypeWithRates() {
1094
1095 final double semiMajorAxis = 1e4;
1096 final double ex = 0.;
1097 final double ey = 0.;
1098 final double expectedL = 0.;
1099 final double expectedLDot = 0.;
1100
1101 for (final PositionAngleType inputPositionAngleType: PositionAngleType.values()) {
1102 for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
1103 final EquinoctialOrbit equinoctialOrbit = new EquinoctialOrbit(semiMajorAxis, ex, ey, 0., 0.,
1104 expectedL, 0., 0., 0., 0., 0., expectedLDot,
1105 inputPositionAngleType, cachedPositionAngleType, FramesFactory.getGCRF(), date, mu);
1106 Assertions.assertEquals(expectedL, equinoctialOrbit.getLv());
1107 Assertions.assertEquals(expectedL, equinoctialOrbit.getLM());
1108 Assertions.assertEquals(expectedL, equinoctialOrbit.getLE());
1109 Assertions.assertEquals(expectedLDot, equinoctialOrbit.getLvDot());
1110 Assertions.assertEquals(expectedLDot, equinoctialOrbit.getLMDot());
1111 Assertions.assertEquals(expectedLDot, equinoctialOrbit.getLEDot());
1112 }
1113 }
1114 }
1115
1116 @Test
1117 void testCachedPositionAngleTypeTrue() {
1118
1119 final double semiMajorAxis = 1e4;
1120 final double ex = 1e-2;
1121 final double ey = -1e-3;
1122 final double expectedLv = 2.;
1123 final PositionAngleType inputPositionAngleType = PositionAngleType.TRUE;
1124
1125 for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
1126 final EquinoctialOrbit equinoctialOrbit = new EquinoctialOrbit(semiMajorAxis, ex, ey, 0., 0.,
1127 expectedLv, inputPositionAngleType, cachedPositionAngleType, FramesFactory.getGCRF(), date, mu);
1128 Assertions.assertEquals(expectedLv, equinoctialOrbit.getLv(), 1e-15);
1129 final double actualL = equinoctialOrbit.getL(cachedPositionAngleType);
1130 switch (cachedPositionAngleType) {
1131
1132 case MEAN:
1133 Assertions.assertEquals(EquinoctialLongitudeArgumentUtility.trueToMean(ex, ey, expectedLv),
1134 actualL);
1135 break;
1136
1137 case ECCENTRIC:
1138 Assertions.assertEquals(EquinoctialLongitudeArgumentUtility.trueToEccentric(ex, ey, expectedLv),
1139 actualL);
1140 break;
1141
1142 case TRUE:
1143 Assertions.assertEquals(expectedLv, actualL);
1144 break;
1145 }
1146 }
1147 }
1148
1149 @Test
1150 void testCachedPositionAngleTypeMean() {
1151
1152 final double semiMajorAxis = 1e4;
1153 final double ex = 1e-2;
1154 final double ey = -1e-3;
1155 final double expectedLM = 2.;
1156 final PositionAngleType inputPositionAngleType = PositionAngleType.MEAN;
1157
1158 for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
1159 final EquinoctialOrbit equinoctialOrbit = new EquinoctialOrbit(semiMajorAxis, ex, ey, 0., 0.,
1160 expectedLM, inputPositionAngleType, cachedPositionAngleType, FramesFactory.getGCRF(), date, mu);
1161 Assertions.assertEquals(expectedLM, equinoctialOrbit.getLM(), 1e-15);
1162 final double actualL = equinoctialOrbit.getL(cachedPositionAngleType);
1163 switch (cachedPositionAngleType) {
1164
1165 case TRUE:
1166 Assertions.assertEquals(EquinoctialLongitudeArgumentUtility.meanToTrue(ex, ey, expectedLM),
1167 actualL);
1168 break;
1169
1170 case ECCENTRIC:
1171 Assertions.assertEquals(EquinoctialLongitudeArgumentUtility.meanToEccentric(ex, ey, expectedLM),
1172 actualL);
1173 break;
1174
1175 case MEAN:
1176 Assertions.assertEquals(expectedLM, actualL);
1177 break;
1178 }
1179 }
1180 }
1181
1182 @Test
1183 void testCachedPositionAngleTypeEccentric() {
1184
1185 final double semiMajorAxis = 1e4;
1186 final double ex = 1e-2;
1187 final double ey = -1e-3;
1188 final double expectedLE = 2.;
1189 final PositionAngleType inputPositionAngleType = PositionAngleType.ECCENTRIC;
1190
1191 for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
1192 final EquinoctialOrbit equinoctialOrbit = new EquinoctialOrbit(semiMajorAxis, ex, ey, 0., 0.,
1193 expectedLE, inputPositionAngleType, cachedPositionAngleType, FramesFactory.getGCRF(), date, mu);
1194 Assertions.assertEquals(expectedLE, equinoctialOrbit.getLE(), 1e-15);
1195 final double actualL = equinoctialOrbit.getL(cachedPositionAngleType);
1196 switch (cachedPositionAngleType) {
1197
1198 case TRUE:
1199 Assertions.assertEquals(EquinoctialLongitudeArgumentUtility.eccentricToTrue(ex, ey, expectedLE),
1200 actualL);
1201 break;
1202
1203 case MEAN:
1204 Assertions.assertEquals(EquinoctialLongitudeArgumentUtility.eccentricToMean(ex, ey, expectedLE),
1205 actualL);
1206 break;
1207
1208 case ECCENTRIC:
1209 Assertions.assertEquals(expectedLE, actualL);
1210 break;
1211 }
1212 }
1213 }
1214
1215 @BeforeEach
1216 public void setUp() {
1217
1218 Utils.setDataRoot("regular-data");
1219
1220
1221 date = AbsoluteDate.J2000_EPOCH;
1222
1223
1224 mu = 3.9860047e14;
1225 }
1226
1227 @AfterEach
1228 public void tearDown() {
1229 date = null;
1230 }
1231
1232 }