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