1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.utils;
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.DerivativeStructure;
23 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
24 import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
25 import org.hipparchus.analysis.differentiation.UnivariateDerivative2;
26 import org.hipparchus.analysis.interpolation.HermiteInterpolator;
27 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
28 import org.hipparchus.geometry.euclidean.threed.Vector3D;
29 import org.hipparchus.random.RandomGenerator;
30 import org.hipparchus.random.Well19937a;
31 import org.hipparchus.util.FastMath;
32 import org.junit.jupiter.api.Assertions;
33 import org.junit.jupiter.api.BeforeEach;
34 import org.junit.jupiter.api.Test;
35 import org.orekit.OrekitMatchers;
36 import org.orekit.Utils;
37 import org.orekit.errors.OrekitException;
38 import org.orekit.errors.OrekitMessages;
39 import org.orekit.frames.FramesFactory;
40 import org.orekit.orbits.CartesianOrbit;
41 import org.orekit.time.AbsoluteDate;
42
43
44 public class PVCoordinatesTest {
45
46 @Test
47 public void testDefaultConstructor() {
48 Assertions.assertEquals("{P(0.0, 0.0, 0.0), V(0.0, 0.0, 0.0), A(0.0, 0.0, 0.0)}", new PVCoordinates().toString());
49 }
50
51 @Test
52 public void testLinearConstructors() {
53 PVCoordinates pv1 = new PVCoordinates(new Vector3D( 1, 0.1, 10),
54 new Vector3D(-1, -0.1, -10));
55 PVCoordinates pv2 = new PVCoordinates(new Vector3D( 2, 0.2, 20),
56 new Vector3D(-2, -0.2, -20));
57 PVCoordinates pv3 = new PVCoordinates(new Vector3D( 3, 0.3, 30),
58 new Vector3D(-3, -0.3, -30));
59 PVCoordinates pv4 = new PVCoordinates(new Vector3D( 4, 0.4, 40),
60 new Vector3D(-4, -0.4, -40));
61 checkPV(pv4, new PVCoordinates(4, pv1), 1.0e-15);
62 checkPV(pv2, new PVCoordinates(pv1, pv3), 1.0e-15);
63 checkPV(pv3, new PVCoordinates(1, pv1, 1, pv2), 1.0e-15);
64 checkPV(new PVCoordinates(2, pv4), new PVCoordinates(3, pv1, 1, pv2, 1, pv3), 1.0e-15);
65 checkPV(new PVCoordinates(3, pv3), new PVCoordinates(3, pv1, 1, pv2, 1, pv4), 1.0e-15);
66 checkPV(new PVCoordinates(5, pv4), new PVCoordinates(4, pv1, 3, pv2, 2, pv3, 1, pv4), 1.0e-15);
67 }
68
69 @Test
70 public void testToDerivativeStructureVectorNeg() {
71 try {
72 PVCoordinates.ZERO.toDerivativeStructureVector(-1);
73 Assertions.fail("an exception should have been thrown");
74 } catch (OrekitException oe) {
75 Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
76 Assertions.assertEquals(-1, ((Integer) (oe.getParts()[0])).intValue());
77 }
78 }
79
80 @Test
81 public void testToDerivativeStructureVector3() {
82 try {
83 PVCoordinates.ZERO.toDerivativeStructureVector(3);
84 Assertions.fail("an exception should have been thrown");
85 } catch (OrekitException oe) {
86 Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
87 Assertions.assertEquals(3, ((Integer) (oe.getParts()[0])).intValue());
88 }
89 }
90
91 @Test
92 public void testToDerivativeStructureVector0() {
93 FieldVector3D<DerivativeStructure> fv =
94 new PVCoordinates(new Vector3D( 1, 0.1, 10),
95 new Vector3D(-1, -0.1, -10),
96 new Vector3D(10, -1.0, -100)).toDerivativeStructureVector(0);
97 Assertions.assertEquals(1, fv.getX().getFreeParameters());
98 Assertions.assertEquals(0, fv.getX().getOrder());
99 Assertions.assertEquals( 1.0, fv.getX().getReal(), 1.0e-10);
100 Assertions.assertEquals( 0.1, fv.getY().getReal(), 1.0e-10);
101 Assertions.assertEquals( 10.0, fv.getZ().getReal(), 1.0e-10);
102 checkPV(new PVCoordinates(new Vector3D( 1, 0.1, 10),
103 Vector3D.ZERO,
104 Vector3D.ZERO),
105 new PVCoordinates(fv), 1.0e-15);
106 }
107
108 @Test
109 public void testToDerivativeStructureVector1() {
110 FieldVector3D<DerivativeStructure> fv =
111 new PVCoordinates(new Vector3D( 1, 0.1, 10),
112 new Vector3D(-1, -0.1, -10),
113 new Vector3D(10, -1.0, -100)).toDerivativeStructureVector(1);
114 Assertions.assertEquals(1, fv.getX().getFreeParameters());
115 Assertions.assertEquals(1, fv.getX().getOrder());
116 Assertions.assertEquals( 1.0, fv.getX().getReal(), 1.0e-10);
117 Assertions.assertEquals( 0.1, fv.getY().getReal(), 1.0e-10);
118 Assertions.assertEquals( 10.0, fv.getZ().getReal(), 1.0e-10);
119 Assertions.assertEquals( -1.0, fv.getX().getPartialDerivative(1), 1.0e-15);
120 Assertions.assertEquals( -0.1, fv.getY().getPartialDerivative(1), 1.0e-15);
121 Assertions.assertEquals( -10.0, fv.getZ().getPartialDerivative(1), 1.0e-15);
122 checkPV(new PVCoordinates(new Vector3D( 1, 0.1, 10),
123 new Vector3D(-1, -0.1, -10),
124 Vector3D.ZERO),
125 new PVCoordinates(fv), 1.0e-15);
126 }
127
128 @Test
129 public void testUnivariateDerivative1Vector() {
130 FieldVector3D<UnivariateDerivative1> fv =
131 new PVCoordinates(new Vector3D( 1, 0.1, 10),
132 new Vector3D(-1, -0.1, -10),
133 new Vector3D(10, -1.0, -100)).toUnivariateDerivative1Vector();
134 Assertions.assertEquals(1, fv.getX().getOrder());
135 Assertions.assertEquals( 1.0, fv.getX().getReal(), 1.0e-10);
136 Assertions.assertEquals( 0.1, fv.getY().getReal(), 1.0e-10);
137 Assertions.assertEquals( 10.0, fv.getZ().getReal(), 1.0e-10);
138 Assertions.assertEquals( -1.0, fv.getX().getDerivative(1), 1.0e-15);
139 Assertions.assertEquals( -0.1, fv.getY().getDerivative(1), 1.0e-15);
140 Assertions.assertEquals( -10.0, fv.getZ().getDerivative(1), 1.0e-15);
141
142 PVCoordinates pv = new PVCoordinates(fv);
143 Assertions.assertEquals( 1.0, pv.getPosition().getX(), 1.0e-10);
144 Assertions.assertEquals( 0.1, pv.getPosition().getY(), 1.0e-10);
145 Assertions.assertEquals( 10.0, pv.getPosition().getZ(), 1.0e-10);
146 Assertions.assertEquals( -1.0, pv.getVelocity().getX(), 1.0e-15);
147 Assertions.assertEquals( -0.1, pv.getVelocity().getY(), 1.0e-15);
148 Assertions.assertEquals( -10.0, pv.getVelocity().getZ(), 1.0e-15);
149
150 }
151
152 @Test
153 public void testToDerivativeStructureVector2() {
154 FieldVector3D<DerivativeStructure> fv =
155 new PVCoordinates(new Vector3D( 1, 0.1, 10),
156 new Vector3D(-1, -0.1, -10),
157 new Vector3D(10, -1.0, -100)).toDerivativeStructureVector(2);
158 Assertions.assertEquals(1, fv.getX().getFreeParameters());
159 Assertions.assertEquals(2, fv.getX().getOrder());
160 Assertions.assertEquals( 1.0, fv.getX().getReal(), 1.0e-10);
161 Assertions.assertEquals( 0.1, fv.getY().getReal(), 1.0e-10);
162 Assertions.assertEquals( 10.0, fv.getZ().getReal(), 1.0e-10);
163 Assertions.assertEquals( -1.0, fv.getX().getPartialDerivative(1), 1.0e-15);
164 Assertions.assertEquals( -0.1, fv.getY().getPartialDerivative(1), 1.0e-15);
165 Assertions.assertEquals( -10.0, fv.getZ().getPartialDerivative(1), 1.0e-15);
166 Assertions.assertEquals( 10.0, fv.getX().getPartialDerivative(2), 1.0e-15);
167 Assertions.assertEquals( -1.0, fv.getY().getPartialDerivative(2), 1.0e-15);
168 Assertions.assertEquals(-100.0, fv.getZ().getPartialDerivative(2), 1.0e-15);
169 checkPV(new PVCoordinates(new Vector3D( 1, 0.1, 10),
170 new Vector3D(-1, -0.1, -10),
171 new Vector3D(10, -1.0, -100)),
172 new PVCoordinates(fv), 1.0e-15);
173
174 for (double dt = 0; dt < 10; dt += 0.125) {
175 Vector3D p = new PVCoordinates(new Vector3D( 1, 0.1, 10),
176 new Vector3D(-1, -0.1, -10),
177 new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition();
178 Assertions.assertEquals(p.getX(), fv.getX().taylor(dt), 1.0e-14);
179 Assertions.assertEquals(p.getY(), fv.getY().taylor(dt), 1.0e-14);
180 Assertions.assertEquals(p.getZ(), fv.getZ().taylor(dt), 1.0e-14);
181 }
182
183 PVCoordinates pv = new PVCoordinates(fv);
184 Assertions.assertEquals( 1.0, pv.getPosition().getX(), 1.0e-10);
185 Assertions.assertEquals( 0.1, pv.getPosition().getY(), 1.0e-10);
186 Assertions.assertEquals( 10.0, pv.getPosition().getZ(), 1.0e-10);
187 Assertions.assertEquals( -1.0, pv.getVelocity().getX(), 1.0e-15);
188 Assertions.assertEquals( -0.1, pv.getVelocity().getY(), 1.0e-15);
189 Assertions.assertEquals( -10.0, pv.getVelocity().getZ(), 1.0e-15);
190 Assertions.assertEquals( 10.0, pv.getAcceleration().getX(), 1.0e-15);
191 Assertions.assertEquals( -1.0, pv.getAcceleration().getY(), 1.0e-15);
192 Assertions.assertEquals(-100.0, pv.getAcceleration().getZ(), 1.0e-15);
193
194 }
195
196 @Test
197 public void testUnivariateDerivative2Vector() {
198 FieldVector3D<UnivariateDerivative2> fv =
199 new PVCoordinates(new Vector3D( 1, 0.1, 10),
200 new Vector3D(-1, -0.1, -10),
201 new Vector3D(10, -1.0, -100)).toUnivariateDerivative2Vector();
202 Assertions.assertEquals(2, fv.getX().getOrder());
203 Assertions.assertEquals( 1.0, fv.getX().getReal(), 1.0e-10);
204 Assertions.assertEquals( 0.1, fv.getY().getReal(), 1.0e-10);
205 Assertions.assertEquals( 10.0, fv.getZ().getReal(), 1.0e-10);
206 Assertions.assertEquals( -1.0, fv.getX().getDerivative(1), 1.0e-15);
207 Assertions.assertEquals( -0.1, fv.getY().getDerivative(1), 1.0e-15);
208 Assertions.assertEquals( -10.0, fv.getZ().getDerivative(1), 1.0e-15);
209 Assertions.assertEquals( 10.0, fv.getX().getDerivative(2), 1.0e-15);
210 Assertions.assertEquals( -1.0, fv.getY().getDerivative(2), 1.0e-15);
211 Assertions.assertEquals(-100.0, fv.getZ().getDerivative(2), 1.0e-15);
212
213 for (double dt = 0; dt < 10; dt += 0.125) {
214 Vector3D p = new PVCoordinates(new Vector3D( 1, 0.1, 10),
215 new Vector3D(-1, -0.1, -10),
216 new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition();
217 Assertions.assertEquals(p.getX(), fv.getX().taylor(dt), 1.0e-14);
218 Assertions.assertEquals(p.getY(), fv.getY().taylor(dt), 1.0e-14);
219 Assertions.assertEquals(p.getZ(), fv.getZ().taylor(dt), 1.0e-14);
220 }
221
222 PVCoordinates pv = new PVCoordinates(fv);
223 Assertions.assertEquals( 1.0, pv.getPosition().getX(), 1.0e-10);
224 Assertions.assertEquals( 0.1, pv.getPosition().getY(), 1.0e-10);
225 Assertions.assertEquals( 10.0, pv.getPosition().getZ(), 1.0e-10);
226 Assertions.assertEquals( -1.0, pv.getVelocity().getX(), 1.0e-15);
227 Assertions.assertEquals( -0.1, pv.getVelocity().getY(), 1.0e-15);
228 Assertions.assertEquals( -10.0, pv.getVelocity().getZ(), 1.0e-15);
229 Assertions.assertEquals( 10.0, pv.getAcceleration().getX(), 1.0e-15);
230 Assertions.assertEquals( -1.0, pv.getAcceleration().getY(), 1.0e-15);
231 Assertions.assertEquals(-100.0, pv.getAcceleration().getZ(), 1.0e-15);
232
233 }
234
235 @Test
236 public void testToDerivativeStructurePVNeg() {
237 try {
238 PVCoordinates.ZERO.toDerivativeStructurePV(-1);
239 Assertions.fail("an exception should have been thrown");
240 } catch (OrekitException oe) {
241 Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
242 Assertions.assertEquals(-1, ((Integer) (oe.getParts()[0])).intValue());
243 }
244 }
245
246 @Test
247 public void testToDerivativeStructurePV3() {
248 try {
249 PVCoordinates.ZERO.toDerivativeStructurePV(3);
250 Assertions.fail("an exception should have been thrown");
251 } catch (OrekitException oe) {
252 Assertions.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
253 Assertions.assertEquals(3, ((Integer) (oe.getParts()[0])).intValue());
254 }
255 }
256
257 @Test
258 public void testToDerivativeStructurePV0() {
259 FieldPVCoordinates<DerivativeStructure> fv =
260 new PVCoordinates(new Vector3D( 1, 0.1, 10),
261 new Vector3D(-1, -0.1, -10),
262 new Vector3D(10, -1.0, -100)).toDerivativeStructurePV(0);
263 Assertions.assertEquals(1, fv.getPosition().getX().getFreeParameters());
264 Assertions.assertEquals(0, fv.getPosition().getX().getOrder());
265 Assertions.assertEquals( 1.0, fv.getPosition().getX().getReal(), 1.0e-10);
266 Assertions.assertEquals( 0.1, fv.getPosition().getY().getReal(), 1.0e-10);
267 Assertions.assertEquals( 10.0, fv.getPosition().getZ().getReal(), 1.0e-10);
268 Assertions.assertEquals( -1.0, fv.getVelocity().getX().getReal(), 1.0e-10);
269 Assertions.assertEquals( -0.1, fv.getVelocity().getY().getReal(), 1.0e-10);
270 Assertions.assertEquals( -10.0, fv.getVelocity().getZ().getReal(), 1.0e-10);
271 Assertions.assertEquals( 10.0, fv.getAcceleration().getX().getReal(), 1.0e-10);
272 Assertions.assertEquals( -1.0, fv.getAcceleration().getY().getReal(), 1.0e-10);
273 Assertions.assertEquals(-100.0, fv.getAcceleration().getZ().getReal(), 1.0e-10);
274 }
275
276 @Test
277 public void testToDerivativeStructurePV1() {
278 FieldPVCoordinates<DerivativeStructure> fv =
279 new PVCoordinates(new Vector3D( 1, 0.1, 10),
280 new Vector3D(-1, -0.1, -10),
281 new Vector3D(10, -1.0, -100)).toDerivativeStructurePV(1);
282 Assertions.assertEquals(1, fv.getPosition().getX().getFreeParameters());
283 Assertions.assertEquals(1, fv.getPosition().getX().getOrder());
284 Assertions.assertEquals( 1.0, fv.getPosition().getX().getReal(), 1.0e-10);
285 Assertions.assertEquals( 0.1, fv.getPosition().getY().getReal(), 1.0e-10);
286 Assertions.assertEquals( 10.0, fv.getPosition().getZ().getReal(), 1.0e-10);
287 Assertions.assertEquals( -1.0, fv.getVelocity().getX().getReal(), 1.0e-10);
288 Assertions.assertEquals( -0.1, fv.getVelocity().getY().getReal(), 1.0e-10);
289 Assertions.assertEquals( -10.0, fv.getVelocity().getZ().getReal(), 1.0e-10);
290 Assertions.assertEquals( 10.0, fv.getAcceleration().getX().getReal(), 1.0e-10);
291 Assertions.assertEquals( -1.0, fv.getAcceleration().getY().getReal(), 1.0e-10);
292 Assertions.assertEquals(-100.0, fv.getAcceleration().getZ().getReal(), 1.0e-10);
293
294 Assertions.assertEquals(fv.getVelocity().getX().getReal(), fv.getPosition().getX().getPartialDerivative(1), 1.0e-10);
295 Assertions.assertEquals(fv.getVelocity().getY().getReal(), fv.getPosition().getY().getPartialDerivative(1), 1.0e-10);
296 Assertions.assertEquals(fv.getVelocity().getZ().getReal(), fv.getPosition().getZ().getPartialDerivative(1), 1.0e-10);
297 Assertions.assertEquals(fv.getAcceleration().getX().getReal(), fv.getVelocity().getX().getPartialDerivative(1), 1.0e-10);
298 Assertions.assertEquals(fv.getAcceleration().getY().getReal(), fv.getVelocity().getY().getPartialDerivative(1), 1.0e-10);
299 Assertions.assertEquals(fv.getAcceleration().getZ().getReal(), fv.getVelocity().getZ().getPartialDerivative(1), 1.0e-10);
300
301 }
302
303 @Test
304 public void testToUnivariateDerivative1PV() {
305 FieldPVCoordinates<UnivariateDerivative1> fv =
306 new PVCoordinates(new Vector3D( 1, 0.1, 10),
307 new Vector3D(-1, -0.1, -10),
308 new Vector3D(10, -1.0, -100)).toUnivariateDerivative1PV();
309 Assertions.assertEquals(1, fv.getPosition().getX().getOrder());
310 Assertions.assertEquals( 1.0, fv.getPosition().getX().getReal(), 1.0e-10);
311 Assertions.assertEquals( 0.1, fv.getPosition().getY().getReal(), 1.0e-10);
312 Assertions.assertEquals( 10.0, fv.getPosition().getZ().getReal(), 1.0e-10);
313 Assertions.assertEquals( -1.0, fv.getVelocity().getX().getReal(), 1.0e-10);
314 Assertions.assertEquals( -0.1, fv.getVelocity().getY().getReal(), 1.0e-10);
315 Assertions.assertEquals( -10.0, fv.getVelocity().getZ().getReal(), 1.0e-10);
316 Assertions.assertEquals( 10.0, fv.getAcceleration().getX().getReal(), 1.0e-10);
317 Assertions.assertEquals( -1.0, fv.getAcceleration().getY().getReal(), 1.0e-10);
318 Assertions.assertEquals(-100.0, fv.getAcceleration().getZ().getReal(), 1.0e-10);
319
320 Assertions.assertEquals(fv.getVelocity().getX().getReal(), fv.getPosition().getX().getDerivative(1), 1.0e-10);
321 Assertions.assertEquals(fv.getVelocity().getY().getReal(), fv.getPosition().getY().getDerivative(1), 1.0e-10);
322 Assertions.assertEquals(fv.getVelocity().getZ().getReal(), fv.getPosition().getZ().getDerivative(1), 1.0e-10);
323 Assertions.assertEquals(fv.getAcceleration().getX().getReal(), fv.getVelocity().getX().getDerivative(1), 1.0e-10);
324 Assertions.assertEquals(fv.getAcceleration().getY().getReal(), fv.getVelocity().getY().getDerivative(1), 1.0e-10);
325 Assertions.assertEquals(fv.getAcceleration().getZ().getReal(), fv.getVelocity().getZ().getDerivative(1), 1.0e-10);
326
327 }
328
329 @Test
330 public void testToDerivativeStructurePV2() {
331 FieldPVCoordinates<DerivativeStructure> fv =
332 new PVCoordinates(new Vector3D( 1, 0.1, 10),
333 new Vector3D(-1, -0.1, -10),
334 new Vector3D(10, -1.0, -100)).toDerivativeStructurePV(2);
335 Assertions.assertEquals(1, fv.getPosition().getX().getFreeParameters());
336 Assertions.assertEquals(2, fv.getPosition().getX().getOrder());
337 Assertions.assertEquals( 1.0, fv.getPosition().getX().getReal(), 1.0e-10);
338 Assertions.assertEquals( 0.1, fv.getPosition().getY().getReal(), 1.0e-10);
339 Assertions.assertEquals( 10.0, fv.getPosition().getZ().getReal(), 1.0e-10);
340 Assertions.assertEquals( -1.0, fv.getVelocity().getX().getReal(), 1.0e-10);
341 Assertions.assertEquals( -0.1, fv.getVelocity().getY().getReal(), 1.0e-10);
342 Assertions.assertEquals( -10.0, fv.getVelocity().getZ().getReal(), 1.0e-10);
343 Assertions.assertEquals( 10.0, fv.getAcceleration().getX().getReal(), 1.0e-10);
344 Assertions.assertEquals( -1.0, fv.getAcceleration().getY().getReal(), 1.0e-10);
345 Assertions.assertEquals(-100.0, fv.getAcceleration().getZ().getReal(), 1.0e-10);
346
347 Assertions.assertEquals(fv.getVelocity().getX().getReal(), fv.getPosition().getX().getPartialDerivative(1), 1.0e-10);
348 Assertions.assertEquals(fv.getVelocity().getY().getReal(), fv.getPosition().getY().getPartialDerivative(1), 1.0e-10);
349 Assertions.assertEquals(fv.getVelocity().getZ().getReal(), fv.getPosition().getZ().getPartialDerivative(1), 1.0e-10);
350 Assertions.assertEquals(fv.getAcceleration().getX().getReal(), fv.getPosition().getX().getPartialDerivative(2), 1.0e-10);
351 Assertions.assertEquals(fv.getAcceleration().getY().getReal(), fv.getPosition().getY().getPartialDerivative(2), 1.0e-10);
352 Assertions.assertEquals(fv.getAcceleration().getZ().getReal(), fv.getPosition().getZ().getPartialDerivative(2), 1.0e-10);
353 Assertions.assertEquals(fv.getAcceleration().getX().getReal(), fv.getVelocity().getX().getPartialDerivative(1), 1.0e-10);
354 Assertions.assertEquals(fv.getAcceleration().getY().getReal(), fv.getVelocity().getY().getPartialDerivative(1), 1.0e-10);
355 Assertions.assertEquals(fv.getAcceleration().getZ().getReal(), fv.getVelocity().getZ().getPartialDerivative(1), 1.0e-10);
356 Assertions.assertEquals(fv.getAcceleration().getX().getPartialDerivative(1), fv.getVelocity().getX().getPartialDerivative(2), 1.0e-10);
357 Assertions.assertEquals(fv.getAcceleration().getY().getPartialDerivative(1), fv.getVelocity().getY().getPartialDerivative(2), 1.0e-10);
358 Assertions.assertEquals(fv.getAcceleration().getZ().getPartialDerivative(1), fv.getVelocity().getZ().getPartialDerivative(2), 1.0e-10);
359
360 for (double dt = 0; dt < 10; dt += 0.125) {
361 Vector3D p = new PVCoordinates(new Vector3D( 1, 0.1, 10),
362 new Vector3D(-1, -0.1, -10),
363 new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition();
364 Assertions.assertEquals(p.getX(), fv.getPosition().getX().taylor(dt), 1.0e-14);
365 Assertions.assertEquals(p.getY(), fv.getPosition().getY().taylor(dt), 1.0e-14);
366 Assertions.assertEquals(p.getZ(), fv.getPosition().getZ().taylor(dt), 1.0e-14);
367 }
368
369 }
370
371 @Test
372 public void testToUnivariateDerivative2PV() {
373 FieldPVCoordinates<UnivariateDerivative2> fv =
374 new PVCoordinates(new Vector3D( 1, 0.1, 10),
375 new Vector3D(-1, -0.1, -10),
376 new Vector3D(10, -1.0, -100)).toUnivariateDerivative2PV();
377 Assertions.assertEquals(2, fv.getPosition().getX().getOrder());
378 Assertions.assertEquals( 1.0, fv.getPosition().getX().getReal(), 1.0e-10);
379 Assertions.assertEquals( 0.1, fv.getPosition().getY().getReal(), 1.0e-10);
380 Assertions.assertEquals( 10.0, fv.getPosition().getZ().getReal(), 1.0e-10);
381 Assertions.assertEquals( -1.0, fv.getVelocity().getX().getReal(), 1.0e-10);
382 Assertions.assertEquals( -0.1, fv.getVelocity().getY().getReal(), 1.0e-10);
383 Assertions.assertEquals( -10.0, fv.getVelocity().getZ().getReal(), 1.0e-10);
384 Assertions.assertEquals( 10.0, fv.getAcceleration().getX().getReal(), 1.0e-10);
385 Assertions.assertEquals( -1.0, fv.getAcceleration().getY().getReal(), 1.0e-10);
386 Assertions.assertEquals(-100.0, fv.getAcceleration().getZ().getReal(), 1.0e-10);
387
388 Assertions.assertEquals(fv.getVelocity().getX().getReal(), fv.getPosition().getX().getDerivative(1), 1.0e-10);
389 Assertions.assertEquals(fv.getVelocity().getY().getReal(), fv.getPosition().getY().getDerivative(1), 1.0e-10);
390 Assertions.assertEquals(fv.getVelocity().getZ().getReal(), fv.getPosition().getZ().getDerivative(1), 1.0e-10);
391 Assertions.assertEquals(fv.getAcceleration().getX().getReal(), fv.getPosition().getX().getDerivative(2), 1.0e-10);
392 Assertions.assertEquals(fv.getAcceleration().getY().getReal(), fv.getPosition().getY().getDerivative(2), 1.0e-10);
393 Assertions.assertEquals(fv.getAcceleration().getZ().getReal(), fv.getPosition().getZ().getDerivative(2), 1.0e-10);
394 Assertions.assertEquals(fv.getAcceleration().getX().getReal(), fv.getVelocity().getX().getDerivative(1), 1.0e-10);
395 Assertions.assertEquals(fv.getAcceleration().getY().getReal(), fv.getVelocity().getY().getDerivative(1), 1.0e-10);
396 Assertions.assertEquals(fv.getAcceleration().getZ().getReal(), fv.getVelocity().getZ().getDerivative(1), 1.0e-10);
397 Assertions.assertEquals(fv.getAcceleration().getX().getDerivative(1), fv.getVelocity().getX().getDerivative(2), 1.0e-10);
398 Assertions.assertEquals(fv.getAcceleration().getY().getDerivative(1), fv.getVelocity().getY().getDerivative(2), 1.0e-10);
399 Assertions.assertEquals(fv.getAcceleration().getZ().getDerivative(1), fv.getVelocity().getZ().getDerivative(2), 1.0e-10);
400
401 for (double dt = 0; dt < 10; dt += 0.125) {
402 Vector3D p = new PVCoordinates(new Vector3D( 1, 0.1, 10),
403 new Vector3D(-1, -0.1, -10),
404 new Vector3D(10, -1.0, -100)).shiftedBy(dt).getPosition();
405 Assertions.assertEquals(p.getX(), fv.getPosition().getX().taylor(dt), 1.0e-14);
406 Assertions.assertEquals(p.getY(), fv.getPosition().getY().taylor(dt), 1.0e-14);
407 Assertions.assertEquals(p.getZ(), fv.getPosition().getZ().taylor(dt), 1.0e-14);
408 }
409 }
410
411 @Test
412 public void testJerkIsVelocitySecondDerivative() {
413 final CartesianOrbit orbit = new CartesianOrbit(new PVCoordinates(new Vector3D(-4947831., -3765382., -3708221.),
414 new Vector3D(-2079., 5291., -7842.)),
415 FramesFactory.getEME2000(),
416 AbsoluteDate.J2000_EPOCH,
417 Constants.EIGEN5C_EARTH_MU);
418 FieldPVCoordinates<DerivativeStructure> fv = orbit.getPVCoordinates().toDerivativeStructurePV(2);
419 Vector3D numericalJerk = differentiate(orbit, o -> o.getPVCoordinates().getAcceleration());
420 Assertions.assertEquals(numericalJerk.getX(),
421 fv.getVelocity().getX().getPartialDerivative(2),
422 3.0e-13);
423 Assertions.assertEquals(numericalJerk.getY(),
424 fv.getVelocity().getY().getPartialDerivative(2),
425 3.0e-13);
426 Assertions.assertEquals(numericalJerk.getZ(),
427 fv.getVelocity().getZ().getPartialDerivative(2),
428 3.0e-13);
429
430 }
431
432 @Test
433 public void testJerkIsAccelerationDerivative() {
434 final CartesianOrbit orbit = new CartesianOrbit(new PVCoordinates(new Vector3D(-4947831., -3765382., -3708221.),
435 new Vector3D(-2079., 5291., -7842.)),
436 FramesFactory.getEME2000(),
437 AbsoluteDate.J2000_EPOCH,
438 Constants.EIGEN5C_EARTH_MU);
439
440 FieldPVCoordinates<DerivativeStructure> fv1 = orbit.getPVCoordinates().toDerivativeStructurePV(1);
441 Vector3D numericalJerk = differentiate(orbit, o -> o.getPVCoordinates().getAcceleration());
442 Assertions.assertEquals(numericalJerk.getX(),
443 fv1.getAcceleration().getX().getPartialDerivative(1),
444 3.0e-13);
445 Assertions.assertEquals(numericalJerk.getY(),
446 fv1.getAcceleration().getY().getPartialDerivative(1),
447 3.0e-13);
448 Assertions.assertEquals(numericalJerk.getZ(),
449 fv1.getAcceleration().getZ().getPartialDerivative(1),
450 3.0e-13);
451
452 FieldPVCoordinates<DerivativeStructure> fv2 = orbit.getPVCoordinates().toDerivativeStructurePV(2);
453 Assertions.assertEquals(numericalJerk.getX(),
454 fv2.getAcceleration().getX().getPartialDerivative(1),
455 3.0e-13);
456 Assertions.assertEquals(numericalJerk.getY(),
457 fv2.getAcceleration().getY().getPartialDerivative(1),
458 3.0e-13);
459 Assertions.assertEquals(numericalJerk.getZ(),
460 fv2.getAcceleration().getZ().getPartialDerivative(1),
461 3.0e-13);
462
463 }
464
465 @Test
466 public void testJounceIsAccelerationSecondDerivative() {
467 final CartesianOrbit orbit = new CartesianOrbit(new PVCoordinates(new Vector3D(-4947831., -3765382., -3708221.),
468 new Vector3D(-2079., 5291., -7842.)),
469 FramesFactory.getEME2000(),
470 AbsoluteDate.J2000_EPOCH,
471 Constants.EIGEN5C_EARTH_MU);
472 FieldPVCoordinates<DerivativeStructure> fv = orbit.getPVCoordinates().toDerivativeStructurePV(2);
473 Vector3D numericalJounce = differentiate(orbit, o -> {
474 FieldVector3D<DerivativeStructure> a = o.getPVCoordinates().toDerivativeStructurePV(1).getAcceleration();
475 return new Vector3D(a.getX().getPartialDerivative(1),
476 a.getY().getPartialDerivative(1),
477 a.getZ().getPartialDerivative(1));
478 });
479 Assertions.assertEquals(numericalJounce.getX(),
480 fv.getAcceleration().getX().getPartialDerivative(2),
481 1.0e-15);
482 Assertions.assertEquals(numericalJounce.getY(),
483 fv.getAcceleration().getY().getPartialDerivative(2),
484 1.0e-15);
485 Assertions.assertEquals(numericalJounce.getZ(),
486 fv.getAcceleration().getZ().getPartialDerivative(2),
487 1.0e-15);
488
489 }
490
491 @Test
492 public void testMomentumDerivative() {
493 final PVCoordinates pva =
494 new PVCoordinates(new Vector3D(-4947831., -3765382., -3708221.),
495 new Vector3D(-2079., 5291., -7842.));
496 final Vector3D p = pva.getPosition();
497 final Vector3D v = pva.getVelocity();
498 final Vector3D a = pva.getAcceleration();
499 final double r2 = p.getNormSq();
500 final double r = FastMath.sqrt(r2);
501 final Vector3D keplerianJerk = new Vector3D(-3 * Vector3D.dotProduct(p, v) / r2, a, -a.getNorm() / r, v);
502 final PVCoordinates velocity = new PVCoordinates(v, a, keplerianJerk);
503 final Vector3D momentumRef = pva.getMomentum();
504 final Vector3D momentumDotRef = PVCoordinates.crossProduct(pva, velocity).getVelocity();
505
506 final FieldVector3D<DerivativeStructure> momentumDot = pva.toDerivativeStructurePV(1).getMomentum();
507 Assertions.assertEquals(momentumRef.getX(), momentumDot.getX().getReal(), 1.0e-15);
508 Assertions.assertEquals(momentumRef.getY(), momentumDot.getY().getReal(), 1.0e-15);
509 Assertions.assertEquals(momentumRef.getZ(), momentumDot.getZ().getReal(), 1.0e-15);
510 Assertions.assertEquals(momentumDotRef.getX(), momentumDot.getX().getPartialDerivative(1), 1.0e-15);
511 Assertions.assertEquals(momentumDotRef.getY(), momentumDot.getY().getPartialDerivative(1), 1.0e-15);
512 Assertions.assertEquals(momentumDotRef.getZ(), momentumDot.getZ().getPartialDerivative(1), 1.0e-15);
513
514 }
515
516 @Test
517 public void testShift() {
518 Vector3D p1 = new Vector3D( 1, 0.1, 10);
519 Vector3D p2 = new Vector3D( 2, 0.2, 20);
520 Vector3D v = new Vector3D(-1, -0.1, -10);
521 checkPV(new PVCoordinates(p2, v), new PVCoordinates(p1, v).shiftedBy(-1.0), 1.0e-15);
522 Assertions.assertEquals(0.0, PVCoordinates.estimateVelocity(p1, p2, -1.0).subtract(v).getNorm(), 1.0e-15);
523 MatcherAssert.assertThat(
524 new PVCoordinates(p1, v).positionShiftedBy(-1.0),
525 OrekitMatchers.vectorCloseTo(p2, 1e-15));
526 }
527
528 @Test
529 public void testToString() {
530 PVCoordinates pv =
531 new PVCoordinates(new Vector3D( 1, 0.1, 10), new Vector3D(-1, -0.1, -10));
532 Assertions.assertEquals("{P(1.0, 0.1, 10.0), V(-1.0, -0.1, -10.0), A(0.0, 0.0, 0.0)}", pv.toString());
533 }
534
535 @Test
536 public void testGetMomentum() {
537
538 Vector3D p = new Vector3D(1, -2, 3);
539 Vector3D v = new Vector3D(-9, 8, -7);
540
541
542 Assertions.assertEquals(new PVCoordinates(p, v).getMomentum(), p.crossProduct(v));
543
544 Assertions.assertEquals(
545 new PVCoordinates(Vector3D.PLUS_I, Vector3D.MINUS_I).getMomentum(),
546 Vector3D.ZERO);
547 Assertions.assertEquals(
548 new PVCoordinates(Vector3D.PLUS_I, Vector3D.PLUS_J).getMomentum(),
549 Vector3D.PLUS_K);
550 }
551
552 @Test
553 public void testGetAngularVelocity() {
554
555 Vector3D p = new Vector3D(1, -2, 3);
556 Vector3D v = new Vector3D(-9, 8, -7);
557
558
559 Assertions.assertEquals(
560 new PVCoordinates(p, v).getAngularVelocity(),
561 p.crossProduct(v).scalarMultiply(1.0 / p.getNormSq()));
562
563 Assertions.assertEquals(
564 new PVCoordinates(Vector3D.PLUS_I, Vector3D.MINUS_I).getAngularVelocity(),
565 Vector3D.ZERO);
566 Assertions.assertEquals(
567 new PVCoordinates(new Vector3D(2, 0, 0), Vector3D.PLUS_J).getAngularVelocity(),
568 Vector3D.PLUS_K.scalarMultiply(0.5));
569 }
570
571 @Test
572 public void testNormalize() {
573 DSFactory factory = new DSFactory(1, 2);
574 RandomGenerator generator = new Well19937a(0xb2011ffd25412067l);
575 FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(5, 1.0e-3);
576 for (int i = 0; i < 200; ++i) {
577 final PVCoordinates pv = randomPVCoordinates(generator, 1e6, 1e3, 1.0);
578 DerivativeStructure x =
579 differentiator.differentiate(new UnivariateFunction() {
580 public double value(double t) {
581 return pv.shiftedBy(t).getPosition().normalize().getX();
582 }
583 }).value(factory.variable(0, 0.0));
584 DerivativeStructure y =
585 differentiator.differentiate(new UnivariateFunction() {
586 public double value(double t) {
587 return pv.shiftedBy(t).getPosition().normalize().getY();
588 }
589 }).value(factory.variable(0, 0.0));
590 DerivativeStructure z =
591 differentiator.differentiate(new UnivariateFunction() {
592 public double value(double t) {
593 return pv.shiftedBy(t).getPosition().normalize().getZ();
594 }
595 }).value(factory.variable(0, 0.0));
596 PVCoordinates normalized = pv.normalize();
597 Assertions.assertEquals(x.getValue(), normalized.getPosition().getX(), 1.0e-16);
598 Assertions.assertEquals(y.getValue(), normalized.getPosition().getY(), 1.0e-16);
599 Assertions.assertEquals(z.getValue(), normalized.getPosition().getZ(), 1.0e-16);
600 Assertions.assertEquals(x.getPartialDerivative(1), normalized.getVelocity().getX(), 3.0e-13);
601 Assertions.assertEquals(y.getPartialDerivative(1), normalized.getVelocity().getY(), 3.0e-13);
602 Assertions.assertEquals(z.getPartialDerivative(1), normalized.getVelocity().getZ(), 3.0e-13);
603 Assertions.assertEquals(x.getPartialDerivative(2), normalized.getAcceleration().getX(), 6.0e-10);
604 Assertions.assertEquals(y.getPartialDerivative(2), normalized.getAcceleration().getY(), 6.0e-10);
605 Assertions.assertEquals(z.getPartialDerivative(2), normalized.getAcceleration().getZ(), 6.0e-10);
606 }
607 }
608
609 @Test
610 public void testCrossProduct() {
611 DSFactory factory = new DSFactory(1, 2);
612 RandomGenerator generator = new Well19937a(0x85c592b3be733d23l);
613 FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(5, 1.0e-3);
614 for (int i = 0; i < 200; ++i) {
615 final PVCoordinates pv1 = randomPVCoordinates(generator, 1.0, 1.0, 1.0);
616 final PVCoordinates pv2 = randomPVCoordinates(generator, 1.0, 1.0, 1.0);
617 DerivativeStructure x =
618 differentiator.differentiate(new UnivariateFunction() {
619 public double value(double t) {
620 return Vector3D.crossProduct(pv1.shiftedBy(t).getPosition(),
621 pv2.shiftedBy(t).getPosition()).getX();
622 }
623 }).value(factory.variable(0, 0.0));
624 DerivativeStructure y =
625 differentiator.differentiate(new UnivariateFunction() {
626 public double value(double t) {
627 return Vector3D.crossProduct(pv1.shiftedBy(t).getPosition(),
628 pv2.shiftedBy(t).getPosition()).getY();
629 }
630 }).value(factory.variable(0, 0.0));
631 DerivativeStructure z =
632 differentiator.differentiate(new UnivariateFunction() {
633 public double value(double t) {
634 return Vector3D.crossProduct(pv1.shiftedBy(t).getPosition(),
635 pv2.shiftedBy(t).getPosition()).getZ();
636 }
637 }).value(factory.variable(0, 0.0));
638 PVCoordinates product = PVCoordinates.crossProduct(pv1, pv2);
639 Assertions.assertEquals(x.getValue(), product.getPosition().getX(), 1.0e-16);
640 Assertions.assertEquals(y.getValue(), product.getPosition().getY(), 1.0e-16);
641 Assertions.assertEquals(z.getValue(), product.getPosition().getZ(), 1.0e-16);
642 Assertions.assertEquals(x.getPartialDerivative(1), product.getVelocity().getX(), 9.0e-10);
643 Assertions.assertEquals(y.getPartialDerivative(1), product.getVelocity().getY(), 9.0e-10);
644 Assertions.assertEquals(z.getPartialDerivative(1), product.getVelocity().getZ(), 9.0e-10);
645 Assertions.assertEquals(x.getPartialDerivative(2), product.getAcceleration().getX(), 3.0e-9);
646 Assertions.assertEquals(y.getPartialDerivative(2), product.getAcceleration().getY(), 3.0e-9);
647 Assertions.assertEquals(z.getPartialDerivative(2), product.getAcceleration().getZ(), 3.0e-9);
648 }
649 }
650
651 private Vector3D randomVector(RandomGenerator random, double norm) {
652 double n = random.nextDouble() * norm;
653 double x = random.nextDouble();
654 double y = random.nextDouble();
655 double z = random.nextDouble();
656 return new Vector3D(n, new Vector3D(x, y, z).normalize());
657 }
658
659 private PVCoordinates randomPVCoordinates(RandomGenerator random,
660 double norm0, double norm1, double norm2) {
661 Vector3D p0 = randomVector(random, norm0);
662 Vector3D p1 = randomVector(random, norm1);
663 Vector3D p2 = randomVector(random, norm2);
664 return new PVCoordinates(p0, p1, p2);
665 }
666
667 private void checkPV(PVCoordinates expected, PVCoordinates real, double epsilon) {
668 Assertions.assertEquals(expected.getPosition().getX(), real.getPosition().getX(), epsilon);
669 Assertions.assertEquals(expected.getPosition().getY(), real.getPosition().getY(), epsilon);
670 Assertions.assertEquals(expected.getPosition().getZ(), real.getPosition().getZ(), epsilon);
671 Assertions.assertEquals(expected.getVelocity().getX(), real.getVelocity().getX(), epsilon);
672 Assertions.assertEquals(expected.getVelocity().getY(), real.getVelocity().getY(), epsilon);
673 Assertions.assertEquals(expected.getVelocity().getZ(), real.getVelocity().getZ(), epsilon);
674 }
675
676 private interface OrbitFunction {
677 Vector3D apply(final CartesianOrbit o);
678 }
679
680 private Vector3D differentiate(CartesianOrbit orbit, OrbitFunction picker) {
681 try {
682 HermiteInterpolator interpolator = new HermiteInterpolator();
683 final double step = 0.01;
684 for (int i = -4; i < 4; ++i) {
685 double dt = i * step;
686 interpolator.addSamplePoint(dt, picker.apply(orbit.shiftedBy(dt)).toArray());
687 }
688 return new Vector3D(interpolator.derivatives(0.0, 1)[1]);
689 } catch (OrekitException oe) {
690 return null;
691 }
692 }
693
694 @BeforeEach
695 public void setUp() {
696 Utils.setDataRoot("regular-data");
697 }
698
699 }