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