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