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