1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.frames;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.complex.Complex;
22 import org.hipparchus.complex.ComplexField;
23 import org.hipparchus.geometry.euclidean.threed.FieldLine;
24 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Line;
27 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
28 import org.hipparchus.geometry.euclidean.threed.Vector3D;
29 import org.hipparchus.geometry.euclidean.threed.Rotation;
30 import org.hipparchus.linear.FieldMatrix;
31 import org.hipparchus.linear.MatrixUtils;
32 import org.hipparchus.random.RandomGenerator;
33 import org.hipparchus.random.Well19937a;
34 import org.hipparchus.util.Binary64;
35 import org.hipparchus.util.Binary64Field;
36 import org.hipparchus.util.FastMath;
37 import org.hipparchus.util.MathArrays;
38 import org.junit.jupiter.api.Assertions;
39 import org.junit.jupiter.api.Test;
40 import org.orekit.time.AbsoluteDate;
41 import org.orekit.time.FieldAbsoluteDate;
42 import org.orekit.time.FieldTimeInterpolator;
43 import org.orekit.utils.*;
44
45 import java.lang.reflect.Array;
46 import java.util.ArrayList;
47 import java.util.List;
48
49 public class FieldTransformTest {
50
51 @Test
52 void testIdentityTransformVector() {
53
54 final Binary64Field field = Binary64Field.getInstance();
55 final FieldTransform<Binary64> identity = FieldTransform.getIdentity(field);
56 final Vector3D vector3D = new Vector3D(1, 2, 3);
57 final FieldVector3D<Binary64> fieldVector3D = new FieldVector3D<>(field, vector3D);
58
59 final FieldVector3D<Binary64> transformed = identity.transformVector(vector3D);
60
61 Assertions.assertEquals(fieldVector3D, transformed);
62 Assertions.assertEquals(identity.transformVector(vector3D), transformed);
63 Assertions.assertEquals(identity.transformVector(fieldVector3D), transformed);
64 Assertions.assertEquals(identity.transformPosition(vector3D), transformed);
65 Assertions.assertEquals(identity.transformPosition(fieldVector3D), transformed);
66 }
67
68 @Test
69 void testIdentityTransformPVCoordinates() {
70
71 final Binary64Field field = Binary64Field.getInstance();
72 final FieldTransform<Binary64> identity = FieldTransform.getIdentity(field);
73 final Vector3D position = new Vector3D(1, 2, 3);
74 final Vector3D velocity = Vector3D.MINUS_K;
75 final Vector3D acceleration = new Vector3D(-1, 1);
76 final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
77
78 final FieldPVCoordinates<Binary64> transformed = identity.transformPVCoordinates(pvCoordinates);
79
80 final FieldPVCoordinates<Binary64> fieldPVCoordinates = new FieldPVCoordinates<Binary64>(field, pvCoordinates);
81 Assertions.assertEquals(position, transformed.getPosition().toVector3D());
82 Assertions.assertEquals(velocity, transformed.getVelocity().toVector3D());
83 Assertions.assertEquals(acceleration, transformed.getAcceleration().toVector3D());
84 Assertions.assertEquals(identity.transformPVCoordinates(fieldPVCoordinates).getPosition(),
85 transformed.getPosition());
86 Assertions.assertEquals(identity.transformPVCoordinates(fieldPVCoordinates).getVelocity(),
87 transformed.getVelocity());
88 final TimeStampedPVCoordinates timeStampedFieldPVCoordinates = new TimeStampedPVCoordinates(AbsoluteDate.ARBITRARY_EPOCH,
89 pvCoordinates);
90 Assertions.assertEquals(identity.transformPVCoordinates(timeStampedFieldPVCoordinates).getPosition(),
91 transformed.getPosition());
92 Assertions.assertEquals(identity.transformPVCoordinates(timeStampedFieldPVCoordinates).getVelocity(),
93 transformed.getVelocity());
94 }
95
96 @Test
97 void testIdentityTransformOnlyPV() {
98
99 final Binary64Field field = Binary64Field.getInstance();
100 final FieldTransform<Binary64> identity = FieldTransform.getIdentity(field);
101 final Vector3D position = new Vector3D(1, 2, 3);
102 final Vector3D velocity = Vector3D.MINUS_K;
103 final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
104 final FieldPVCoordinates<Binary64> fieldPVCoordinates = new FieldPVCoordinates<>(field, pvCoordinates);
105
106 final FieldPVCoordinates<Binary64> transformed = identity.transformOnlyPV(fieldPVCoordinates);
107
108 Assertions.assertEquals(position, transformed.getPosition().toVector3D());
109 Assertions.assertEquals(velocity, transformed.getVelocity().toVector3D());
110 Assertions.assertEquals(FieldVector3D.getZero(field), transformed.getAcceleration());
111 Assertions.assertEquals(identity.transformPVCoordinates(fieldPVCoordinates).getPosition(),
112 transformed.getPosition());
113 Assertions.assertEquals(identity.transformPVCoordinates(fieldPVCoordinates).getVelocity(),
114 transformed.getVelocity());
115 final TimeStampedFieldPVCoordinates<Binary64> timeStampedFieldPVCoordinates = new TimeStampedFieldPVCoordinates<>(identity.getFieldDate(),
116 fieldPVCoordinates);
117 Assertions.assertEquals(identity.transformPVCoordinates(timeStampedFieldPVCoordinates).getPosition(),
118 transformed.getPosition());
119 Assertions.assertEquals(identity.transformPVCoordinates(timeStampedFieldPVCoordinates).getVelocity(),
120 transformed.getVelocity());
121 }
122
123 @Test
124 void testIdentityShiftedBy() {
125
126 final Binary64Field field = Binary64Field.getInstance();
127 final FieldTransform<Binary64> identity = FieldTransform.getIdentity(field);
128
129 final FieldTransform<Binary64> shiftedIdentity = identity.shiftedBy(1.);
130
131 Assertions.assertEquals(identity, shiftedIdentity);
132 Assertions.assertEquals(identity.shiftedBy(Binary64.ONE), shiftedIdentity);
133 }
134
135 @Test
136 void testIdentityStaticShiftedBy() {
137
138 final Binary64Field field = Binary64Field.getInstance();
139 final FieldTransform<Binary64> identity = FieldTransform.getIdentity(field);
140
141 final FieldStaticTransform<Binary64> shiftedIdentity = identity.staticShiftedBy(Binary64.ONE);
142
143 Assertions.assertEquals(FieldStaticTransform.getIdentity(field).getClass(), shiftedIdentity.getClass());
144 }
145
146 @Test
147 void testIdentityGetStaticInverse() {
148
149 final Binary64Field field = Binary64Field.getInstance();
150 final FieldTransform<Binary64> identity = FieldTransform.getIdentity(field);
151
152 final FieldStaticTransform<Binary64> staticInverse = identity.getStaticInverse();
153
154 Assertions.assertEquals(identity.toStaticTransform().getClass(), staticInverse.getClass());
155 }
156
157 @Test
158 void testIdentityFreeze() {
159
160 final Binary64Field field = Binary64Field.getInstance();
161 final FieldTransform<Binary64> identity = FieldTransform.getIdentity(field);
162
163 final FieldTransform<Binary64> frozen = identity.freeze();
164
165 Assertions.assertEquals(identity, frozen);
166 }
167
168 @Test
169 public void testIdentityTranslation() {
170 doTestIdentityTranslation(Binary64Field.getInstance());
171 }
172
173 private <T extends CalculusFieldElement<T>> void doTestIdentityTranslation(Field<T> field) {
174 checkNoTransform(FieldTransform.getIdentity(field).shiftedBy(12345.0),
175 new Well19937a(0xfd118eac6b5ec136l));
176 }
177
178 @Test
179 public void testIdentityRotation() {
180 doTestIdentityRotation(Binary64Field.getInstance());
181 }
182
183 private <T extends CalculusFieldElement<T>> void doTestIdentityRotation(Field<T> field) {
184 checkNoTransform(FieldTransform.getIdentity(field),
185 new Well19937a(0xfd118eac6b5ec136l));
186 }
187
188 @Test
189 public void testIdentityLine() {
190 doTestIdentityLine(Binary64Field.getInstance());
191 }
192
193 private <T extends CalculusFieldElement<T>> void doTestIdentityLine(Field<T> field) {
194 RandomGenerator random = new Well19937a(0x98603025df70db7cl);
195 FieldVector3D<T> p1 = randomVector(field, 100.0, random);
196 FieldVector3D<T> p2 = randomVector(field, 100.0, random);
197 FieldLine<T> line = new FieldLine<>(p1, p2, 1.0e-6);
198 FieldLine<T> transformed = FieldTransform.getIdentity(field).transformLine(line);
199 Assertions.assertSame(line, transformed);
200 }
201
202 @Test
203 public void testSimpleComposition() {
204 doTestSimpleComposition(Binary64Field.getInstance());
205 }
206
207 private <T extends CalculusFieldElement<T>> void doTestSimpleComposition(Field<T> field) {
208 FieldTransform<T> transform =
209 new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field),
210 new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field),
211 new FieldRotation<>(FieldVector3D.getPlusK(field),
212 field.getZero().add(0.5 * FastMath.PI),
213 RotationConvention.VECTOR_OPERATOR)),
214 new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), FieldVector3D.getPlusI(field)));
215 FieldVector3D<T> u = transform.transformPosition(createVector(field, 1.0, 1.0, 1.0));
216 FieldVector3D<T> v = createVector(field, 0.0, 1.0, 1.0);
217 Assertions.assertEquals(0, u.subtract(v).getNorm().getReal(), 1.0e-15);
218 }
219
220 @Test
221 public void testAcceleration() {
222 doTestAcceleration(Binary64Field.getInstance());
223 }
224
225 private <T extends CalculusFieldElement<T>> void doTestAcceleration(Field<T> field) {
226
227 FieldPVCoordinates<T> initPV = new FieldPVCoordinates<>(createVector(field, 9, 8, 7),
228 createVector(field, 6, 5, 4),
229 createVector(field, 3, 2, 1));
230 for (double dt = 0; dt < 1; dt += 0.01) {
231 FieldPVCoordinates<T> basePV = initPV.shiftedBy(dt);
232 FieldPVCoordinates<T> transformedPV = evolvingTransform(FieldAbsoluteDate.getJ2000Epoch(field), dt).transformPVCoordinates(basePV);
233
234
235 List<TimeStampedFieldPVCoordinates<T>> sample = new ArrayList<TimeStampedFieldPVCoordinates<T>>();
236 double h = 1.0e-2;
237 for (int i = -3; i < 4; ++i) {
238 FieldTransform<T> t = evolvingTransform(FieldAbsoluteDate.getJ2000Epoch(field), dt + i * h);
239 FieldPVCoordinates<T> pv = t.transformPVCoordinates(initPV.shiftedBy(dt + i * h));
240 sample.add(new TimeStampedFieldPVCoordinates<>(t.getDate(), pv.getPosition(), pv.getVelocity(), FieldVector3D.getZero(field)));
241 }
242
243
244 final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<T>, T> interpolator =
245 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(sample.size(), CartesianDerivativesFilter.USE_PV);
246
247 FieldPVCoordinates<T> rebuiltPV = interpolator.interpolate(FieldAbsoluteDate.getJ2000Epoch(field).shiftedBy(dt),
248 sample);
249
250 checkVector(rebuiltPV.getPosition(), transformedPV.getPosition(), 3.0e-15);
251 checkVector(rebuiltPV.getVelocity(), transformedPV.getVelocity(), 2.0e-15);
252 checkVector(rebuiltPV.getAcceleration(), transformedPV.getAcceleration(), 5.0e-10);
253
254 }
255
256 }
257
258 @Test
259 public void testAccelerationComposition() {
260 doTestAccelerationComposition(Binary64Field.getInstance());
261 }
262
263 private <T extends CalculusFieldElement<T>> void doTestAccelerationComposition(Field<T> field) {
264 RandomGenerator random = new Well19937a(0x41fdd07d6c9e9f65l);
265
266 FieldVector3D<T> p1 = randomVector(field, 1.0e3, random);
267 FieldVector3D<T> v1 = randomVector(field, 1.0, random);
268 FieldVector3D<T> a1 = randomVector(field, 1.0e-3, random);
269 FieldRotation<T> r1 = randomRotation(field, random);
270 FieldVector3D<T> o1 = randomVector(field, 0.1, random);
271
272 FieldVector3D<T> p2 = randomVector(field, 1.0e3, random);
273 FieldVector3D<T> v2 = randomVector(field, 1.0, random);
274 FieldVector3D<T> a2 = randomVector(field, 1.0e-3, random);
275 FieldRotation<T> r2 = randomRotation(field, random);
276 FieldVector3D<T> o2 = randomVector(field, 0.1, random);
277
278 FieldTransform<T> t1 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field),
279 new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), p1, v1, a1),
280 new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), r1, o1));
281 FieldTransform<T> t2 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field),
282 new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), p2, v2, a2),
283 new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), r2, o2));
284 FieldTransform<T> t12 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), t1, t2);
285
286 FieldVector3D<T> q = randomVector(field, 1.0e3, random);
287 FieldVector3D<T> qDot = randomVector(field, 1.0, random);
288 FieldVector3D<T> qDotDot = randomVector(field, 1.0e-3, random);
289
290 FieldPVCoordinates<T> pva0 = new FieldPVCoordinates<>(q, qDot, qDotDot);
291 FieldPVCoordinates<T> pva1 = t1.transformPVCoordinates(pva0);
292 FieldPVCoordinates<T> pva2 = t2.transformPVCoordinates(pva1);
293 FieldPVCoordinates<T> pvac = t12.transformPVCoordinates(pva0);
294
295 checkVector(pva2.getPosition(), pvac.getPosition(), 1.0e-15);
296 checkVector(pva2.getVelocity(), pvac.getVelocity(), 1.0e-15);
297 checkVector(pva2.getAcceleration(), pvac.getAcceleration(), 1.0e-15);
298
299
300
301
302 Assertions.assertEquals(0.0, t1.getAngular().getRotationAcceleration().getNorm().getReal(), 1.0e-15);
303 Assertions.assertEquals(0.0, t2.getAngular().getRotationAcceleration().getNorm().getReal(), 1.0e-15);
304 Assertions.assertTrue(t12.getAngular().getRotationAcceleration().getNorm().getReal() > 0.01);
305
306 Assertions.assertEquals(0.0, t12.freeze().getCartesian().getVelocity().getNorm().getReal(), 1.0e-15);
307 Assertions.assertEquals(0.0, t12.freeze().getCartesian().getAcceleration().getNorm().getReal(), 1.0e-15);
308 Assertions.assertEquals(0.0, t12.freeze().getAngular().getRotationRate().getNorm().getReal(), 1.0e-15);
309 Assertions.assertEquals(0.0, t12.freeze().getAngular().getRotationAcceleration().getNorm().getReal(), 1.0e-15);
310 }
311
312 @Test
313 public void testRandomComposition() {
314 doTestRandomComposition(Binary64Field.getInstance());
315 }
316
317 private <T extends CalculusFieldElement<T>> void doTestRandomComposition(Field<T> field) {
318
319 RandomGenerator random = new Well19937a(0x171c79e323a1123l);
320 for (int i = 0; i < 20; ++i) {
321
322
323 int n = random.nextInt(20);
324 @SuppressWarnings("unchecked")
325 FieldTransform<T>[] transforms = (FieldTransform<T>[]) Array.newInstance(FieldTransform.class, n);
326 FieldTransform<T> combined = FieldTransform.getIdentity(field);
327 for (int k = 0; k < n; ++k) {
328 transforms[k] = random.nextBoolean()
329 ? new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), randomVector(field, 1.0e3, random), randomVector(field, 1.0, random), randomVector(field, 1.0e-3, random))
330 : new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), randomRotation(field, random), randomVector(field, 0.01, random), randomVector(field, 1.0e-4, random));
331 combined = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), combined, transforms[k]);
332 }
333
334
335 for (int j = 0; j < 10; ++j) {
336 FieldVector3D<T> a = randomVector(field, 1.0, random);
337 FieldVector3D<T> b = randomVector(field, 1.0e3, random);
338 FieldPVCoordinates<T> c = new FieldPVCoordinates<>(randomVector(field, 1.0e3, random), randomVector(field, 1.0, random), randomVector(field, 1.0e-3, random));
339 FieldVector3D<T> aRef = a;
340 FieldVector3D<T> bRef = b;
341 FieldPVCoordinates<T> cRef = c;
342 for (int k = 0; k < n; ++k) {
343 aRef = transforms[k].transformVector(aRef);
344 bRef = transforms[k].transformPosition(bRef);
345 cRef = transforms[k].transformPVCoordinates(cRef);
346 }
347
348 FieldVector3D<T> aCombined = combined.transformVector(a);
349 FieldVector3D<T> bCombined = combined.transformPosition(b);
350 FieldPVCoordinates<T> cCombined = combined.transformPVCoordinates(c);
351 checkVector(aRef, aCombined, 3.0e-15);
352 checkVector(bRef, bCombined, 5.0e-15);
353 checkVector(cRef.getPosition(), cCombined.getPosition(), 1.0e-14);
354 checkVector(cRef.getVelocity(), cCombined.getVelocity(), 1.0e-14);
355 checkVector(cRef.getAcceleration(), cCombined.getAcceleration(), 1.0e-14);
356
357 }
358 }
359
360 }
361
362 @Test
363 public void testReverse() {
364 doTestReverse(Binary64Field.getInstance());
365 }
366
367 private <T extends CalculusFieldElement<T>> void doTestReverse(Field<T> field) {
368 RandomGenerator random = new Well19937a(0x9f82ba2b2c98dac5l);
369 for (int i = 0; i < 20; ++i) {
370 FieldTransform<T> combined = randomTransform(field, random);
371
372 checkNoTransform(new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), combined, combined.getInverse()), random);
373
374 }
375
376 }
377
378 @Test
379 public void testIdentityJacobianP() {
380 doTestIdentityJacobianP(Binary64Field.getInstance());
381 }
382
383 private <T extends CalculusFieldElement<T>> void doTestIdentityJacobianP(Field<T> field) {
384 doTestIdentityJacobian(field, 3, CartesianDerivativesFilter.USE_P);
385 }
386
387 @Test
388 public void testIdentityJacobianPV() {
389 doTestIdentityJacobianPV(Binary64Field.getInstance());
390 }
391
392 private <T extends CalculusFieldElement<T>> void doTestIdentityJacobianPV(Field<T> field) {
393 doTestIdentityJacobian(field, 6, CartesianDerivativesFilter.USE_PV);
394 }
395
396 @Test
397 public void testIdentityJacobianPVA() {
398 doTestIdentityJacobianPVA(Binary64Field.getInstance());
399 }
400
401 private <T extends CalculusFieldElement<T>> void doTestIdentityJacobianPVA(Field<T> field) {
402 doTestIdentityJacobian(field, 9, CartesianDerivativesFilter.USE_PVA);
403 }
404
405 private <T extends CalculusFieldElement<T>> void doTestIdentityJacobian(Field<T> field, int n, CartesianDerivativesFilter filter) {
406 T[][] jacobian = MathArrays.buildArray(field, n, n);
407 FieldTransform.getIdentity(field).getJacobian(filter, jacobian);
408 for (int i = 0; i < n; ++i) {
409 for (int j = 0; j < n; ++j) {
410 Assertions.assertEquals(i == j ? 1.0 : 0.0, jacobian[i][j].getReal(), 1.0e-15);
411 }
412 }
413 }
414
415 @Test
416 public void testDecomposeAndRebuild() {
417 doTestDecomposeAndRebuild(Binary64Field.getInstance());
418 }
419
420 private <T extends CalculusFieldElement<T>> void doTestDecomposeAndRebuild(Field<T> field) {
421 RandomGenerator random = new Well19937a(0xb8ee9da1b05198c9l);
422 for (int i = 0; i < 20; ++i) {
423 FieldTransform<T> combined = randomTransform(field, random);
424 FieldTransform<T> rebuilt = new FieldTransform<>(combined.getFieldDate(),
425 new FieldTransform<>(combined.getFieldDate(), combined.getTranslation(),
426 combined.getVelocity(), combined.getAcceleration()),
427 new FieldTransform<>(combined.getFieldDate(), combined.getRotation(),
428 combined.getRotationRate(), combined.getRotationAcceleration()));
429
430 checkNoTransform(new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), combined, rebuilt.getInverse()), random);
431
432 }
433
434 }
435
436 @Test
437 public void testTranslation() {
438 doTestTranslation(Binary64Field.getInstance());
439 }
440
441 private <T extends CalculusFieldElement<T>> void doTestTranslation(Field<T> field) {
442 RandomGenerator rnd = new Well19937a(0x7e9d737ba4147787l);
443 for (int i = 0; i < 10; ++i) {
444 FieldVector3D<T> delta = randomVector(field, 1.0e3, rnd);
445 FieldTransform<T> transform = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), delta);
446 for (int j = 0; j < 10; ++j) {
447 FieldVector3D<T> a = createVector(field, rnd.nextDouble(), rnd.nextDouble(), rnd.nextDouble());
448 FieldVector3D<T> b = transform.transformVector(a);
449 Assertions.assertEquals(0, b.subtract(a).getNorm().getReal(), 1.0e-15);
450 FieldVector3D<T> c = transform.transformPosition(a);
451 Assertions.assertEquals(0,
452 c.subtract(a).subtract(delta).getNorm().getReal(),
453 1.0e-14);
454 }
455 }
456 }
457
458 @Test
459 public void testTranslationDouble() {
460 doTestTranslationDouble(Binary64Field.getInstance());
461 }
462
463 private <T extends CalculusFieldElement<T>> void doTestTranslationDouble(Field<T> field) {
464 RandomGenerator rnd = new Well19937a(0x7e9d737ba4147787l);
465 for (int i = 0; i < 10; ++i) {
466 FieldVector3D<T> delta = randomVector(field, 1.0e3, rnd);
467 FieldTransform<T> transform = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), delta);
468 for (int j = 0; j < 10; ++j) {
469 Vector3D a = createVector(field, rnd.nextDouble(), rnd.nextDouble(), rnd.nextDouble()).toVector3D();
470 FieldVector3D<T> b = transform.transformVector(a);
471 Assertions.assertEquals(0, b.subtract(a).getNorm().getReal(), 1.0e-15);
472 FieldVector3D<T> c = transform.transformPosition(a);
473 Assertions.assertEquals(0,
474 c.subtract(a).subtract(delta).getNorm().getReal(),
475 1.0e-14);
476 }
477 }
478 }
479
480 @Test
481 public void testRoughTransPV() {
482 doTestRoughTransPV(Binary64Field.getInstance());
483 }
484
485 private <T extends CalculusFieldElement<T>> void doTestRoughTransPV(Field<T> field) {
486
487 FieldPVCoordinates<T> pointP1 = new FieldPVCoordinates<>(FieldVector3D.getPlusI(field), FieldVector3D.getPlusI(field), FieldVector3D.getPlusI(field));
488
489
490 FieldPVCoordinates<T> pointP2 = new FieldPVCoordinates<>(createVector(field, 0, 0, 0), createVector(field, 0, 0, 0));
491 FieldTransform<T> R1toR2 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), FieldVector3D.getMinusI(field), FieldVector3D.getMinusI(field), FieldVector3D.getMinusI(field));
492 FieldPVCoordinates<T> result1 = R1toR2.transformPVCoordinates(pointP1);
493 checkVector(pointP2.getPosition(), result1.getPosition(), 1.0e-15);
494 checkVector(pointP2.getVelocity(), result1.getVelocity(), 1.0e-15);
495 checkVector(pointP2.getAcceleration(), result1.getAcceleration(), 1.0e-15);
496
497
498 FieldTransform<T> R2toR1 = R1toR2.getInverse();
499 FieldPVCoordinates<T> invResult1 = R2toR1.transformPVCoordinates(pointP2);
500 checkVector(pointP1.getPosition(), invResult1.getPosition(), 1.0e-15);
501 checkVector(pointP1.getVelocity(), invResult1.getVelocity(), 1.0e-15);
502 checkVector(pointP1.getAcceleration(), invResult1.getAcceleration(), 1.0e-15);
503
504
505 FieldPVCoordinates<T> pointP3 = new FieldPVCoordinates<>(FieldVector3D.getPlusJ(field), createVector(field, -2, 1, 0), createVector(field, -4, -3, -1));
506 FieldRotation<T> R = new FieldRotation<>(FieldVector3D.getPlusK(field), field.getZero().add(FastMath.PI / 2), RotationConvention.VECTOR_OPERATOR);
507 FieldTransform<T> R1toR3 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), R, createVector(field, 0, 0, -2), createVector(field, 1, 0, 0));
508 FieldPVCoordinates<T> result2 = R1toR3.transformPVCoordinates(pointP1);
509 checkVector(pointP3.getPosition(), result2.getPosition(), 1.0e-15);
510 checkVector(pointP3.getVelocity(), result2.getVelocity(), 1.0e-15);
511 checkVector(pointP3.getAcceleration(), result2.getAcceleration(), 1.0e-15);
512
513
514 FieldTransform<T> R3toR1 = R1toR3.getInverse();
515 FieldPVCoordinates<T> invResult2 = R3toR1.transformPVCoordinates(pointP3);
516 checkVector(pointP1.getPosition(), invResult2.getPosition(), 1.0e-15);
517 checkVector(pointP1.getVelocity(), invResult2.getVelocity(), 1.0e-15);
518 checkVector(pointP1.getAcceleration(), invResult2.getAcceleration(), 1.0e-15);
519
520
521 FieldTransform<T> R1toR4 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), createVector(field, -2, 0, 0), createVector(field, -2, 0, 0), createVector(field, -2, 0, 0));
522 FieldPVCoordinates<T> pointP4 = new FieldPVCoordinates<>(createVector(field, -1, 0, 0), createVector(field, -1, 0, 0), createVector(field, -1, 0, 0));
523 FieldTransform<T> R2toR4 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), R2toR1, R1toR4);
524 FieldPVCoordinates<T> compResult = R2toR4.transformPVCoordinates(pointP2);
525 checkVector(pointP4.getPosition(), compResult.getPosition(), 1.0e-15);
526 checkVector(pointP4.getVelocity(), compResult.getVelocity(), 1.0e-15);
527 checkVector(pointP4.getAcceleration(), compResult.getAcceleration(), 1.0e-15);
528
529
530 FieldPVCoordinates<T> pointP5 = new FieldPVCoordinates<>(createVector(field, -1, 0, 0), createVector(field, -1, 0, 3), createVector(field, 8, 0, 6));
531 FieldRotation<T> R2 = new FieldRotation<>(createVector(field, 0, 0, 1), field.getZero().add(FastMath.PI), RotationConvention.VECTOR_OPERATOR);
532 FieldTransform<T> R1toR5 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), R2, createVector(field, 0, -3, 0));
533 FieldTransform<T> R3toR5 = new FieldTransform<> (FieldAbsoluteDate.getJ2000Epoch(field), R3toR1, R1toR5);
534 FieldPVCoordinates<T> combResult = R3toR5.transformPVCoordinates(pointP3);
535 checkVector(pointP5.getPosition(), combResult.getPosition(), 1.0e-15);
536 checkVector(pointP5.getVelocity(), combResult.getVelocity(), 1.0e-15);
537 checkVector(pointP5.getAcceleration(), combResult.getAcceleration(), 1.0e-15);
538
539
540 FieldTransform<T> R2toR3 = new FieldTransform<> (FieldAbsoluteDate.getJ2000Epoch(field), R2toR1, R1toR3);
541 FieldPVCoordinates<T> result = R2toR3.transformPVCoordinates(pointP2);
542 checkVector(pointP3.getPosition(), result.getPosition(), 1.0e-15);
543 checkVector(pointP3.getVelocity(), result.getVelocity(), 1.0e-15);
544 checkVector(pointP3.getAcceleration(), result.getAcceleration(), 1.0e-15);
545
546 FieldTransform<T> R3toR2 = new FieldTransform<> (FieldAbsoluteDate.getJ2000Epoch(field), R3toR1, R1toR2);
547 result = R3toR2.transformPVCoordinates(pointP3);
548 checkVector(pointP2.getPosition(), result.getPosition(), 1.0e-15);
549 checkVector(pointP2.getVelocity(), result.getVelocity(), 1.0e-15);
550 checkVector(pointP2.getAcceleration(), result.getAcceleration(), 1.0e-15);
551
552 FieldTransform<T> newR1toR5 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), R1toR2, R2toR3);
553 newR1toR5 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), newR1toR5, R3toR5);
554 result = newR1toR5.transformPVCoordinates(pointP1);
555 checkVector(pointP5.getPosition(), result.getPosition(), 1.0e-15);
556 checkVector(pointP5.getVelocity(), result.getVelocity(), 1.0e-15);
557 checkVector(pointP5.getAcceleration(), result.getAcceleration(), 1.0e-15);
558
559
560 newR1toR5 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), R1toR2, R2toR3);
561 FieldTransform<T> R3toR4 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), R3toR1, R1toR4);
562 newR1toR5 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), newR1toR5, R3toR4);
563 FieldTransform<T> R4toR5 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), R1toR4.getInverse(), R1toR5);
564 newR1toR5 = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), newR1toR5, R4toR5);
565 result = newR1toR5.transformPVCoordinates(pointP1);
566 checkVector(pointP5.getPosition(), result.getPosition(), 1.0e-15);
567 checkVector(pointP5.getVelocity(), result.getVelocity(), 1.0e-15);
568 checkVector(pointP5.getAcceleration(), result.getAcceleration(), 1.0e-15);
569
570 }
571
572 @Test
573 public void testRotPV() {
574 doTestRotPV(Binary64Field.getInstance());
575 }
576
577 private <T extends CalculusFieldElement<T>> void doTestRotPV(Field<T> field) {
578
579 RandomGenerator rnd = new Well19937a(0x73d5554d99427af0l);
580
581
582
583 for (int i = 0; i < 10; ++i) {
584
585
586
587 FieldRotation<T> instantRot = randomRotation(field, rnd);
588 FieldVector3D<T> normAxis = instantRot.getAxis(RotationConvention.VECTOR_OPERATOR);
589 T w = instantRot.getAngle().abs().divide(Constants.JULIAN_DAY);
590
591
592 FieldRotation<T> rot = randomRotation(field, rnd);
593
594
595 FieldTransform<T> tr = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field),
596 rot, new FieldVector3D<>(w, normAxis));
597
598
599 FieldVector3D<T> pos = randomVector(field, 1.0e3, rnd);
600 FieldVector3D<T> vel = randomVector(field, 1.0, rnd);
601 FieldVector3D<T> acc = randomVector(field, 1.0e-3, rnd);
602
603 FieldPVCoordinates<T> pvOne = new FieldPVCoordinates<>(pos, vel, acc);
604
605
606
607 FieldPVCoordinates<T> pvTwo = tr.transformPVCoordinates(pvOne);
608
609
610
611 FieldVector3D<T> resultvel = tr.getInverse().transformPVCoordinates(pvTwo).getVelocity();
612
613 checkVector(resultvel, vel, 1.0e-15);
614
615 }
616
617 }
618
619 @Test
620 public void testTransPV() {
621 doTestTransPV(Binary64Field.getInstance());
622 }
623
624 private <T extends CalculusFieldElement<T>> void doTestTransPV(Field<T> field) {
625
626 RandomGenerator rnd = new Well19937a(0x73d5554d99427af0l);
627
628
629
630 for (int i = 0; i < 10; ++i) {
631
632
633 FieldVector3D<T> pos = randomVector(field, 1.0e3, rnd);
634 FieldVector3D<T> vel = randomVector(field, 1.0, rnd);
635 FieldVector3D<T> acc = randomVector(field, 1.0e-3, rnd);
636 FieldPVCoordinates<T> pvOne = new FieldPVCoordinates<>(pos, vel, acc);
637
638
639 FieldVector3D<T> transPos = randomVector(field, 1.0e3, rnd);
640 FieldVector3D<T> transVel = randomVector(field, 1.0, rnd);
641 FieldVector3D<T> transAcc = randomVector(field, 1.0e-3, rnd);
642 FieldTransform<T> tr = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), transPos, transVel, transAcc);
643
644 double dt = 1;
645
646
647 FieldVector3D<T> good = tr.transformPosition(pos.add(new FieldVector3D<>(dt, vel))).add(new FieldVector3D<>(dt, transVel));
648
649
650 FieldPVCoordinates<T> pvTwo = tr.transformPVCoordinates(pvOne);
651 FieldVector3D<T> result = pvTwo.getPosition().add(new FieldVector3D<>(dt, pvTwo.getVelocity()));
652 checkVector(good, result, 1.0e-15);
653
654
655 FieldVector3D<T> resultvel = tr.getInverse().
656 transformPVCoordinates(pvTwo).getVelocity();
657 checkVector(resultvel, vel, 1.0e-15);
658
659 }
660
661 }
662
663 @Test
664 public void testTransPVDouble() {
665 doTestTransPVDouble(Binary64Field.getInstance());
666 }
667
668 private <T extends CalculusFieldElement<T>> void doTestTransPVDouble(Field<T> field) {
669
670 RandomGenerator rnd = new Well19937a(0x73d5554d99427af0l);
671
672
673
674 for (int i = 0; i < 10; ++i) {
675
676
677 Vector3D pos = randomVector(field, 1.0e3, rnd).toVector3D();
678 Vector3D vel = randomVector(field, 1.0, rnd).toVector3D();
679 Vector3D acc = randomVector(field, 1.0e-3, rnd).toVector3D();
680 PVCoordinates pvOne = new PVCoordinates(pos, vel, acc);
681
682
683 FieldVector3D<T> transPos = randomVector(field, 1.0e3, rnd);
684 FieldVector3D<T> transVel = randomVector(field, 1.0, rnd);
685 FieldVector3D<T> transAcc = randomVector(field, 1.0e-3, rnd);
686 FieldTransform<T> tr = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), transPos, transVel, transAcc);
687
688 T dt = field.getZero().add(1);
689
690
691 FieldVector3D<T> good = tr.transformPosition(new FieldVector3D<>(dt, vel).add(pos)).add(new FieldVector3D<>(dt, transVel));
692
693
694 FieldPVCoordinates<T> pvTwo = tr.transformPVCoordinates(pvOne);
695 FieldVector3D<T> result = pvTwo.getPosition().add(new FieldVector3D<>(dt, pvTwo.getVelocity()));
696 checkVector(good, result, 1.0e-15);
697
698
699 FieldVector3D<T> resultvel = tr.getInverse().
700 transformPVCoordinates(pvTwo).getVelocity();
701 checkVector(resultvel, new FieldVector3D<>(field, vel), 1.0e-15);
702
703 }
704
705 }
706
707 @Test
708 public void testRotation() {
709 doTestRotation(Binary64Field.getInstance());
710 }
711
712 private <T extends CalculusFieldElement<T>> void doTestRotation(Field<T> field) {
713 RandomGenerator rnd = new Well19937a(0x73d5554d99427af0l);
714 for (int i = 0; i < 10; ++i) {
715
716 FieldRotation<T> r = randomRotation(field, rnd);
717 FieldVector3D<T> axis = r.getAxis(RotationConvention.VECTOR_OPERATOR);
718 T angle = r.getAngle();
719
720 FieldTransform<T> transform = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), r);
721 for (int j = 0; j < 10; ++j) {
722 FieldVector3D<T> a = createVector(field, rnd.nextDouble(), rnd.nextDouble(), rnd.nextDouble());
723 FieldVector3D<T> b = transform.transformVector(a);
724 Assertions.assertEquals(FieldVector3D.angle(axis, a).getReal(), FieldVector3D.angle(axis, b).getReal(), 1.0e-14);
725 FieldVector3D<T> aOrtho = FieldVector3D.crossProduct(axis, a);
726 FieldVector3D<T> bOrtho = FieldVector3D.crossProduct(axis, b);
727 Assertions.assertEquals(angle.getReal(), FieldVector3D.angle(aOrtho, bOrtho).getReal(), 1.0e-14);
728 FieldVector3D<T> c = transform.transformPosition(a);
729 Assertions.assertEquals(0, c.subtract(b).getNorm().getReal(), 1.0e-14);
730 }
731
732 }
733 }
734
735 @Test
736 public void testJacobianP() {
737 doTestJacobianP(Binary64Field.getInstance());
738 }
739
740 private <T extends CalculusFieldElement<T>> void doTestJacobianP(Field<T> field) {
741
742
743 @SuppressWarnings("unchecked")
744 FieldPVCoordinates<T>[] directions = (FieldPVCoordinates<T>[]) Array.newInstance(FieldPVCoordinates.class, 3);
745 directions[0] = new FieldPVCoordinates<>(FieldVector3D.getPlusI(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field));
746 directions[1] = new FieldPVCoordinates<>(FieldVector3D.getPlusJ(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field));
747 directions[2] = new FieldPVCoordinates<>(FieldVector3D.getPlusK(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field));
748 double h = 0.01;
749
750 RandomGenerator random = new Well19937a(0x47fd0d6809f4b173l);
751 for (int i = 0; i < 20; ++i) {
752
753
754 FieldTransform<T> combined = randomTransform(field, random);
755
756
757 T[][] jacobian = MathArrays.buildArray(field, 9, 9);
758 for (int l = 0; l < jacobian.length; ++l) {
759 for (int c = 0; c < jacobian[l].length; ++c) {
760 jacobian[l][c] = field.getZero().add(l + 0.1 * c);
761 }
762 }
763 combined.getJacobian(CartesianDerivativesFilter.USE_P, jacobian);
764
765 for (int j = 0; j < 100; ++j) {
766
767 FieldPVCoordinates<T> pv0 = new FieldPVCoordinates<>(randomVector(field, 1e3, random),
768 randomVector(field, 1.0, random),
769 randomVector(field, 1.0e-3, random));
770 double epsilonP = 2.0e-12 * pv0.getPosition().getNorm().getReal();
771
772 for (int c = 0; c < directions.length; ++c) {
773
774
775 FieldPVCoordinates<T> pvm4h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -4 * h, directions[c]));
776 FieldPVCoordinates<T> pvm3h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -3 * h, directions[c]));
777 FieldPVCoordinates<T> pvm2h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -2 * h, directions[c]));
778 FieldPVCoordinates<T> pvm1h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -1 * h, directions[c]));
779 FieldPVCoordinates<T> pvp1h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +1 * h, directions[c]));
780 FieldPVCoordinates<T> pvp2h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +2 * h, directions[c]));
781 FieldPVCoordinates<T> pvp3h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +3 * h, directions[c]));
782 FieldPVCoordinates<T> pvp4h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +4 * h, directions[c]));
783 FieldPVCoordinates<T> d4 = new FieldPVCoordinates<>(pvm4h, pvp4h);
784 FieldPVCoordinates<T> d3 = new FieldPVCoordinates<>(pvm3h, pvp3h);
785 FieldPVCoordinates<T> d2 = new FieldPVCoordinates<>(pvm2h, pvp2h);
786 FieldPVCoordinates<T> d1 = new FieldPVCoordinates<>(pvm1h, pvp1h);
787 double d = 1.0 / (840 * h);
788 FieldPVCoordinates<T> estimatedColumn = new FieldPVCoordinates<>(-3 * d, d4, 32 * d, d3, -168 * d, d2, 672 * d, d1);
789
790
791 Assertions.assertEquals(estimatedColumn.getPosition().getX().getReal(), jacobian[0][c].getReal(), epsilonP);
792 Assertions.assertEquals(estimatedColumn.getPosition().getY().getReal(), jacobian[1][c].getReal(), epsilonP);
793 Assertions.assertEquals(estimatedColumn.getPosition().getZ().getReal(), jacobian[2][c].getReal(), epsilonP);
794
795
796 for (int l = 3; l < jacobian.length; ++l) {
797 Assertions.assertEquals(l + 0.1 * c, jacobian[l][c].getReal(), 1.0e-15);
798 }
799
800 }
801
802
803 for (int c = directions.length; c < jacobian[0].length; ++c) {
804 for (int l = 0; l < jacobian.length; ++l) {
805 Assertions.assertEquals(l + 0.1 * c, jacobian[l][c].getReal(), 1.0e-15);
806 }
807 }
808
809 }
810 }
811
812 }
813
814 @Test
815 public void testJacobianPV() {
816 doTestJacobianPV(Binary64Field.getInstance());
817 }
818
819 private <T extends CalculusFieldElement<T>> void doTestJacobianPV(Field<T> field) {
820
821
822 @SuppressWarnings("unchecked")
823 FieldPVCoordinates<T>[] directions = (FieldPVCoordinates<T>[]) Array.newInstance(FieldPVCoordinates.class, 6);
824 directions[0] = new FieldPVCoordinates<>(FieldVector3D.getPlusI(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field));
825 directions[1] = new FieldPVCoordinates<>(FieldVector3D.getPlusJ(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field));
826 directions[2] = new FieldPVCoordinates<>(FieldVector3D.getPlusK(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field));
827 directions[3] = new FieldPVCoordinates<>(FieldVector3D.getZero(field), FieldVector3D.getPlusI(field), FieldVector3D.getZero(field));
828 directions[4] = new FieldPVCoordinates<>(FieldVector3D.getZero(field), FieldVector3D.getPlusJ(field), FieldVector3D.getZero(field));
829 directions[5] = new FieldPVCoordinates<>(FieldVector3D.getZero(field), FieldVector3D.getPlusK(field), FieldVector3D.getZero(field));
830 double h = 0.01;
831
832 RandomGenerator random = new Well19937a(0xce2bfddfbb9796bel);
833 for (int i = 0; i < 20; ++i) {
834
835
836 FieldTransform<T> combined = randomTransform(field, random);
837
838
839 T[][] jacobian = MathArrays.buildArray(field, 9, 9);
840 for (int l = 0; l < jacobian.length; ++l) {
841 for (int c = 0; c < jacobian[l].length; ++c) {
842 jacobian[l][c] = field.getZero().add(l + 0.1 * c);
843 }
844 }
845 combined.getJacobian(CartesianDerivativesFilter.USE_PV, jacobian);
846
847 for (int j = 0; j < 100; ++j) {
848
849 FieldPVCoordinates<T> pv0 = new FieldPVCoordinates<>(randomVector(field, 1e3, random),
850 randomVector(field, 1.0, random),
851 randomVector(field, 1.0e-3, random));
852 double epsilonP = 2.0e-12 * pv0.getPosition().getNorm().getReal();
853 double epsilonV = 6.0e-11 * pv0.getVelocity().getNorm().getReal();
854
855 for (int c = 0; c < directions.length; ++c) {
856
857
858 FieldPVCoordinates<T> pvm4h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -4 * h, directions[c]));
859 FieldPVCoordinates<T> pvm3h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -3 * h, directions[c]));
860 FieldPVCoordinates<T> pvm2h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -2 * h, directions[c]));
861 FieldPVCoordinates<T> pvm1h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -1 * h, directions[c]));
862 FieldPVCoordinates<T> pvp1h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +1 * h, directions[c]));
863 FieldPVCoordinates<T> pvp2h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +2 * h, directions[c]));
864 FieldPVCoordinates<T> pvp3h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +3 * h, directions[c]));
865 FieldPVCoordinates<T> pvp4h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +4 * h, directions[c]));
866 FieldPVCoordinates<T> d4 = new FieldPVCoordinates<>(pvm4h, pvp4h);
867 FieldPVCoordinates<T> d3 = new FieldPVCoordinates<>(pvm3h, pvp3h);
868 FieldPVCoordinates<T> d2 = new FieldPVCoordinates<>(pvm2h, pvp2h);
869 FieldPVCoordinates<T> d1 = new FieldPVCoordinates<>(pvm1h, pvp1h);
870 double d = 1.0 / (840 * h);
871 FieldPVCoordinates<T> estimatedColumn = new FieldPVCoordinates<>(-3 * d, d4, 32 * d, d3, -168 * d, d2, 672 * d, d1);
872
873
874 Assertions.assertEquals(estimatedColumn.getPosition().getX().getReal(), jacobian[0][c].getReal(), epsilonP);
875 Assertions.assertEquals(estimatedColumn.getPosition().getY().getReal(), jacobian[1][c].getReal(), epsilonP);
876 Assertions.assertEquals(estimatedColumn.getPosition().getZ().getReal(), jacobian[2][c].getReal(), epsilonP);
877 Assertions.assertEquals(estimatedColumn.getVelocity().getX().getReal(), jacobian[3][c].getReal(), epsilonV);
878 Assertions.assertEquals(estimatedColumn.getVelocity().getY().getReal(), jacobian[4][c].getReal(), epsilonV);
879 Assertions.assertEquals(estimatedColumn.getVelocity().getZ().getReal(), jacobian[5][c].getReal(), epsilonV);
880
881
882 for (int l = 6; l < jacobian.length; ++l) {
883 Assertions.assertEquals(l + 0.1 * c, jacobian[l][c].getReal(), 1.0e-15);
884 }
885
886 }
887
888
889 for (int c = directions.length; c < jacobian[0].length; ++c) {
890 for (int l = 0; l < jacobian.length; ++l) {
891 Assertions.assertEquals(l + 0.1 * c, jacobian[l][c].getReal(), 1.0e-15);
892 }
893 }
894
895 }
896 }
897
898 }
899
900 @Test
901 public void testJacobianPVA() {
902 doTestJacobianPVA(Binary64Field.getInstance());
903 }
904
905 private <T extends CalculusFieldElement<T>> void doTestJacobianPVA(Field<T> field) {
906
907
908 @SuppressWarnings("unchecked")
909 FieldPVCoordinates<T>[] directions = (FieldPVCoordinates<T>[]) Array.newInstance(FieldPVCoordinates.class, 9);
910 directions[0] = new FieldPVCoordinates<>(FieldVector3D.getPlusI(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field));
911 directions[1] = new FieldPVCoordinates<>(FieldVector3D.getPlusJ(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field));
912 directions[2] = new FieldPVCoordinates<>(FieldVector3D.getPlusK(field), FieldVector3D.getZero(field), FieldVector3D.getZero(field));
913 directions[3] = new FieldPVCoordinates<>(FieldVector3D.getZero(field), FieldVector3D.getPlusI(field), FieldVector3D.getZero(field));
914 directions[4] = new FieldPVCoordinates<>(FieldVector3D.getZero(field), FieldVector3D.getPlusJ(field), FieldVector3D.getZero(field));
915 directions[5] = new FieldPVCoordinates<>(FieldVector3D.getZero(field), FieldVector3D.getPlusK(field), FieldVector3D.getZero(field));
916 directions[6] = new FieldPVCoordinates<>(FieldVector3D.getZero(field), FieldVector3D.getZero(field), FieldVector3D.getPlusI(field));
917 directions[7] = new FieldPVCoordinates<>(FieldVector3D.getZero(field), FieldVector3D.getZero(field), FieldVector3D.getPlusJ(field));
918 directions[8] = new FieldPVCoordinates<>(FieldVector3D.getZero(field), FieldVector3D.getZero(field), FieldVector3D.getPlusK(field));
919 double h = 0.01;
920
921 RandomGenerator random = new Well19937a(0xd223e88b6232198fl);
922 for (int i = 0; i < 20; ++i) {
923
924
925 FieldTransform<T> combined = randomTransform(field, random);
926
927
928 T[][] jacobian = MathArrays.buildArray(field, 9, 9);
929 for (int l = 0; l < jacobian.length; ++l) {
930 for (int c = 0; c < jacobian[l].length; ++c) {
931 jacobian[l][c] = field.getZero().add(1 + 0.1 * c);
932 }
933 }
934 combined.getJacobian(CartesianDerivativesFilter.USE_PVA, jacobian);
935
936 for (int j = 0; j < 100; ++j) {
937
938 FieldPVCoordinates<T> pv0 = new FieldPVCoordinates<>(randomVector(field, 1e3, random),
939 randomVector(field, 1.0, random),
940 randomVector(field, 1.0e-3, random));
941 double epsilonP = 2.0e-12 * pv0.getPosition().getNorm().getReal();
942 double epsilonV = 6.0e-11 * pv0.getVelocity().getNorm().getReal();
943 double epsilonA = 2.0e-9 * pv0.getAcceleration().getNorm().getReal();
944
945 for (int c = 0; c < directions.length; ++c) {
946
947
948 FieldPVCoordinates<T> pvm4h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -4 * h, directions[c]));
949 FieldPVCoordinates<T> pvm3h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -3 * h, directions[c]));
950 FieldPVCoordinates<T> pvm2h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -2 * h, directions[c]));
951 FieldPVCoordinates<T> pvm1h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, -1 * h, directions[c]));
952 FieldPVCoordinates<T> pvp1h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +1 * h, directions[c]));
953 FieldPVCoordinates<T> pvp2h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +2 * h, directions[c]));
954 FieldPVCoordinates<T> pvp3h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +3 * h, directions[c]));
955 FieldPVCoordinates<T> pvp4h = combined.transformPVCoordinates(new FieldPVCoordinates<>(1.0, pv0, +4 * h, directions[c]));
956 FieldPVCoordinates<T> d4 = new FieldPVCoordinates<>(pvm4h, pvp4h);
957 FieldPVCoordinates<T> d3 = new FieldPVCoordinates<>(pvm3h, pvp3h);
958 FieldPVCoordinates<T> d2 = new FieldPVCoordinates<>(pvm2h, pvp2h);
959 FieldPVCoordinates<T> d1 = new FieldPVCoordinates<>(pvm1h, pvp1h);
960 double d = 1.0 / (840 * h);
961 FieldPVCoordinates<T> estimatedColumn = new FieldPVCoordinates<>(-3 * d, d4, 32 * d, d3, -168 * d, d2, 672 * d, d1);
962
963
964 Assertions.assertEquals(estimatedColumn.getPosition().getX().getReal(), jacobian[0][c].getReal(), epsilonP);
965 Assertions.assertEquals(estimatedColumn.getPosition().getY().getReal(), jacobian[1][c].getReal(), epsilonP);
966 Assertions.assertEquals(estimatedColumn.getPosition().getZ().getReal(), jacobian[2][c].getReal(), epsilonP);
967 Assertions.assertEquals(estimatedColumn.getVelocity().getX().getReal(), jacobian[3][c].getReal(), epsilonV);
968 Assertions.assertEquals(estimatedColumn.getVelocity().getY().getReal(), jacobian[4][c].getReal(), epsilonV);
969 Assertions.assertEquals(estimatedColumn.getVelocity().getZ().getReal(), jacobian[5][c].getReal(), epsilonV);
970 Assertions.assertEquals(estimatedColumn.getAcceleration().getX().getReal(), jacobian[6][c].getReal(), epsilonA);
971 Assertions.assertEquals(estimatedColumn.getAcceleration().getY().getReal(), jacobian[7][c].getReal(), epsilonA);
972 Assertions.assertEquals(estimatedColumn.getAcceleration().getZ().getReal(), jacobian[8][c].getReal(), epsilonA);
973
974 }
975
976 }
977 }
978
979 }
980
981 @Test
982 public void testLine() {
983 doTestLine(Binary64Field.getInstance());
984 }
985
986 private <T extends CalculusFieldElement<T>> void doTestLine(Field<T> field) {
987 RandomGenerator random = new Well19937a(0x4a5ff67426c5731fl);
988 for (int i = 0; i < 100; ++i) {
989 FieldTransform<T> transform = randomTransform(field, random);
990 for (int j = 0; j < 20; ++j) {
991 FieldVector3D<T> p0 = randomVector(field, 1.0e3, random);
992 FieldVector3D<T> p1 = randomVector(field, 1.0e3, random);
993 FieldLine<T> l = new FieldLine<>(p0, p1, 1.0e-10);
994 FieldLine<T> transformed = transform.transformLine(l);
995 for (int k = 0; k < 10; ++k) {
996 FieldVector3D<T> p = l.pointAt(random.nextDouble() * 1.0e6);
997 Assertions.assertEquals(0.0, transformed.distance(transform.transformPosition(p)).getReal(), 1.0e-9);
998 }
999 }
1000 }
1001 }
1002
1003 @Test
1004 public void testLineDouble() {
1005 doTestLineDouble(Binary64Field.getInstance());
1006 }
1007
1008 private <T extends CalculusFieldElement<T>> void doTestLineDouble(Field<T> field) {
1009 RandomGenerator random = new Well19937a(0x4a5ff67426c5731fl);
1010 for (int i = 0; i < 100; ++i) {
1011 FieldTransform<T> transform = randomTransform(field, random);
1012 for (int j = 0; j < 20; ++j) {
1013 Vector3D p0 = randomVector(field, 1.0e3, random).toVector3D();
1014 Vector3D p1 = randomVector(field, 1.0e3, random).toVector3D();
1015 Line l = new Line(p0, p1, 1.0e-10);
1016 FieldLine<T> transformed = transform.transformLine(l);
1017 for (int k = 0; k < 10; ++k) {
1018 Vector3D p = l.pointAt(random.nextDouble() * 1.0e6);
1019 Assertions.assertEquals(0.0, transformed.distance(transform.transformPosition(p)).getReal(), 1.0e-9);
1020 }
1021 }
1022 }
1023 }
1024
1025 @Test
1026 public void testLinear() {
1027 doTestLinear(Binary64Field.getInstance());
1028 }
1029
1030 private <T extends CalculusFieldElement<T>> void doTestLinear(final Field<T> field) {
1031
1032 RandomGenerator random = new Well19937a(0x14f6411217b148d8l);
1033 for (int n = 0; n < 100; ++n) {
1034 FieldTransform<T> t = randomTransform(field, random);
1035
1036
1037 FieldMatrix<T> linearA = MatrixUtils.createFieldMatrix(field, 3, 4);
1038 linearA.setSubMatrix(t.getRotation().getMatrix(), 0, 0);
1039 FieldVector3D<T> rt = t.getRotation().applyTo(t.getTranslation());
1040 linearA.setEntry(0, 3, rt.getX());
1041 linearA.setEntry(1, 3, rt.getY());
1042 linearA.setEntry(2, 3, rt.getZ());
1043
1044
1045 FieldMatrix<T> linearB = MatrixUtils.createFieldMatrix(field, 3, 4);
1046 FieldVector3D<T> p0 = t.transformPosition(FieldVector3D.getZero(field));
1047 FieldVector3D<T> pI = t.transformPosition(FieldVector3D.getPlusI(field)).subtract(p0);
1048 FieldVector3D<T> pJ = t.transformPosition(FieldVector3D.getPlusJ(field)).subtract(p0);
1049 FieldVector3D<T> pK = t.transformPosition(FieldVector3D.getPlusK(field)).subtract(p0);
1050 linearB.setEntry(0, 0, pI.getX());
1051 linearB.setEntry(1, 0, pI.getY());
1052 linearB.setEntry(2, 0, pI.getZ());
1053 linearB.setEntry(0, 1, pJ.getX());
1054 linearB.setEntry(1, 1, pJ.getY());
1055 linearB.setEntry(2, 1, pJ.getZ());
1056 linearB.setEntry(0, 2, pK.getX());
1057 linearB.setEntry(1, 2, pK.getY());
1058 linearB.setEntry(2, 2, pK.getZ());
1059 linearB.setEntry(0, 3, p0.getX());
1060 linearB.setEntry(1, 3, p0.getY());
1061 linearB.setEntry(2, 3, p0.getZ());
1062
1063
1064 FieldMatrix<T> sub = linearB.subtract(linearA);
1065 double refMax = 0;
1066 double diffMax = 0;
1067 for (int i = 0; i < linearA.getRowDimension(); ++i) {
1068 for (int j = 0; j < linearA.getColumnDimension(); ++j) {
1069 refMax = FastMath.max(linearA.getEntry(i, j).getReal(), refMax);
1070 diffMax = FastMath.max(sub.getEntry(i, j).getReal(), diffMax);
1071 }
1072 }
1073 Assertions.assertEquals(0.0, diffMax, 2.0e-12 * refMax);
1074
1075
1076 for (int i = 0; i < 100; ++i) {
1077 FieldVector3D<T> p = randomVector(field, 1.0e3, random);
1078 FieldVector3D<T> q = t.transformPosition(p);
1079
1080 T[] pField = MathArrays.buildArray(field, 4);
1081 pField[0] = p.getX();
1082 pField[1] = p.getY();
1083 pField[2] = p.getZ();
1084 pField[3] = field.getOne();
1085 T[] qA = linearA.operate(pField);
1086 Assertions.assertEquals(q.getX().getReal(), qA[0].getReal(), 1.0e-13 * p.getNorm().getReal());
1087 Assertions.assertEquals(q.getY().getReal(), qA[1].getReal(), 1.0e-13 * p.getNorm().getReal());
1088 Assertions.assertEquals(q.getZ().getReal(), qA[2].getReal(), 1.0e-13 * p.getNorm().getReal());
1089
1090 T[] qB = linearB.operate(pField);
1091 Assertions.assertEquals(q.getX().getReal(), qB[0].getReal(), 1.0e-10 * p.getNorm().getReal());
1092 Assertions.assertEquals(q.getY().getReal(), qB[1].getReal(), 1.0e-10 * p.getNorm().getReal());
1093 Assertions.assertEquals(q.getZ().getReal(), qB[2].getReal(), 1.0e-10 * p.getNorm().getReal());
1094
1095 }
1096
1097 }
1098
1099 }
1100
1101 @Test
1102 public void testShift() {
1103 doTestShift(Binary64Field.getInstance());
1104 }
1105
1106 private <T extends CalculusFieldElement<T>> void doTestShift(Field<T> field) {
1107
1108
1109
1110
1111
1112
1113 FieldAbsoluteDate<T> date = FieldAbsoluteDate.getGalileoEpoch(field);
1114 double alpha0 = 0.5 * FastMath.PI;
1115 double omega = 0.5 * FastMath.PI;
1116 FieldTransform<T> t = new FieldTransform<>(date,
1117 new FieldTransform<>(date, FieldVector3D.getMinusI(field), FieldVector3D.getMinusJ(field)),
1118 new FieldTransform<>(date,
1119 new FieldRotation<>(FieldVector3D.getPlusK(field),
1120 field.getZero().add(alpha0),
1121 RotationConvention.VECTOR_OPERATOR),
1122 new FieldVector3D<>(omega, FieldVector3D.getMinusK(field))));
1123
1124 for (double dt = -10.0; dt < 10.0; dt += 0.125) {
1125
1126 FieldTransform<T> shifted = t.shiftedBy(dt);
1127
1128
1129 FieldPVCoordinates<T> expectedFixedPoint =
1130 shifted.transformPVCoordinates(new FieldPVCoordinates<>(createVector(field, 1, dt, 0),
1131 FieldVector3D.getPlusJ(field),
1132 FieldVector3D.getZero(field)));
1133 checkVector(expectedFixedPoint.getPosition(), FieldVector3D.getZero(field), 1.0e-14);
1134 checkVector(expectedFixedPoint.getVelocity(), FieldVector3D.getZero(field), 1.0e-14);
1135 checkVector(expectedFixedPoint.getAcceleration(), FieldVector3D.getZero(field), 1.0e-14);
1136
1137
1138 FieldPVCoordinates<T> expectedApparentMotion = shifted.transformPVCoordinates(PVCoordinates.ZERO);
1139 double c = FastMath.cos(alpha0 + omega * dt);
1140 double s = FastMath.sin(alpha0 + omega * dt);
1141 FieldVector3D<T> referencePosition = createVector(field,
1142 -c + dt * s,
1143 -s - dt * c,
1144 0);
1145 FieldVector3D<T> referenceVelocity = createVector(field,
1146 (1 + omega) * s + dt * omega * c,
1147 -(1 + omega) * c + dt * omega * s,
1148 0);
1149 FieldVector3D<T> referenceAcceleration = createVector(field,
1150 omega * (2 + omega) * c - dt * omega * omega * s,
1151 omega * (2 + omega) * s + dt * omega * omega * c,
1152 0);
1153 checkVector(expectedApparentMotion.getPosition(), referencePosition, 1.0e-14);
1154 checkVector(expectedApparentMotion.getVelocity(), referenceVelocity, 1.0e-14);
1155 checkVector(expectedApparentMotion.getAcceleration(), referenceAcceleration, 1.0e-14);
1156
1157 }
1158
1159 }
1160
1161 @Test
1162 public void testShiftDerivatives() {
1163 doTestShiftDerivatives(Binary64Field.getInstance());
1164 }
1165
1166 private <T extends CalculusFieldElement<T>> void doTestShiftDerivatives(Field<T> field) {
1167
1168 RandomGenerator random = new Well19937a(0x5acda4f605aadce7l);
1169 for (int i = 0; i < 10; ++i) {
1170 FieldTransform<T> t = randomTransform(field, random);
1171
1172 for (double dtD = -10.0; dtD < 10.0; dtD += 0.125) {
1173
1174 T dt = field.getZero().add(dtD);
1175 FieldTransform<T> t0 = t.shiftedBy(dt);
1176 T v = t0.getVelocity().getNorm();
1177 T a = t0.getAcceleration().getNorm();
1178 T omega = t0.getRotationRate().getNorm();
1179 T omegaDot = t0.getRotationAcceleration().getNorm();
1180
1181
1182 T h = omega.reciprocal().multiply(0.01);
1183 FieldTransform<T> tm4h = t.shiftedBy(dt.subtract(h.multiply(4)));
1184 FieldTransform<T> tm3h = t.shiftedBy(dt.subtract(h.multiply(3)));
1185 FieldTransform<T> tm2h = t.shiftedBy(dt.subtract(h.multiply(2)));
1186 FieldTransform<T> tm1h = t.shiftedBy(dt.subtract(h.multiply(1)));
1187 FieldTransform<T> tp1h = t.shiftedBy(dt.add(h.multiply(1)));
1188 FieldTransform<T> tp2h = t.shiftedBy(dt.add(h.multiply(2)));
1189 FieldTransform<T> tp3h = t.shiftedBy(dt.add(h.multiply(3)));
1190 FieldTransform<T> tp4h = t.shiftedBy(dt.add(h.multiply(4)));
1191 T numXDot = derivative(h,
1192 tm4h.getTranslation().getX(), tm3h.getTranslation().getX(),
1193 tm2h.getTranslation().getX(), tm1h.getTranslation().getX(),
1194 tp1h.getTranslation().getX(), tp2h.getTranslation().getX(),
1195 tp3h.getTranslation().getX(), tp4h.getTranslation().getX());
1196 T numYDot = derivative(h,
1197 tm4h.getTranslation().getY(), tm3h.getTranslation().getY(),
1198 tm2h.getTranslation().getY(), tm1h.getTranslation().getY(),
1199 tp1h.getTranslation().getY(), tp2h.getTranslation().getY(),
1200 tp3h.getTranslation().getY(), tp4h.getTranslation().getY());
1201 T numZDot = derivative(h,
1202 tm4h.getTranslation().getZ(), tm3h.getTranslation().getZ(),
1203 tm2h.getTranslation().getZ(), tm1h.getTranslation().getZ(),
1204 tp1h.getTranslation().getZ(), tp2h.getTranslation().getZ(),
1205 tp3h.getTranslation().getZ(), tp4h.getTranslation().getZ());
1206 T numXDot2 = derivative(h,
1207 tm4h.getVelocity().getX(), tm3h.getVelocity().getX(),
1208 tm2h.getVelocity().getX(), tm1h.getVelocity().getX(),
1209 tp1h.getVelocity().getX(), tp2h.getVelocity().getX(),
1210 tp3h.getVelocity().getX(), tp4h.getVelocity().getX());
1211 T numYDot2 = derivative(h,
1212 tm4h.getVelocity().getY(), tm3h.getVelocity().getY(),
1213 tm2h.getVelocity().getY(), tm1h.getVelocity().getY(),
1214 tp1h.getVelocity().getY(), tp2h.getVelocity().getY(),
1215 tp3h.getVelocity().getY(), tp4h.getVelocity().getY());
1216 T numZDot2 = derivative(h,
1217 tm4h.getVelocity().getZ(), tm3h.getVelocity().getZ(),
1218 tm2h.getVelocity().getZ(), tm1h.getVelocity().getZ(),
1219 tp1h.getVelocity().getZ(), tp2h.getVelocity().getZ(),
1220 tp3h.getVelocity().getZ(), tp4h.getVelocity().getZ());
1221 T numQ0Dot = derivative(h,
1222 tm4h.getRotation().getQ0(), tm3h.getRotation().getQ0(),
1223 tm2h.getRotation().getQ0(), tm1h.getRotation().getQ0(),
1224 tp1h.getRotation().getQ0(), tp2h.getRotation().getQ0(),
1225 tp3h.getRotation().getQ0(), tp4h.getRotation().getQ0());
1226 T numQ1Dot = derivative(h,
1227 tm4h.getRotation().getQ1(), tm3h.getRotation().getQ1(),
1228 tm2h.getRotation().getQ1(), tm1h.getRotation().getQ1(),
1229 tp1h.getRotation().getQ1(), tp2h.getRotation().getQ1(),
1230 tp3h.getRotation().getQ1(), tp4h.getRotation().getQ1());
1231 T numQ2Dot = derivative(h,
1232 tm4h.getRotation().getQ2(), tm3h.getRotation().getQ2(),
1233 tm2h.getRotation().getQ2(), tm1h.getRotation().getQ2(),
1234 tp1h.getRotation().getQ2(), tp2h.getRotation().getQ2(),
1235 tp3h.getRotation().getQ2(), tp4h.getRotation().getQ2());
1236 T numQ3Dot = derivative(h,
1237 tm4h.getRotation().getQ3(), tm3h.getRotation().getQ3(),
1238 tm2h.getRotation().getQ3(), tm1h.getRotation().getQ3(),
1239 tp1h.getRotation().getQ3(), tp2h.getRotation().getQ3(),
1240 tp3h.getRotation().getQ3(), tp4h.getRotation().getQ3());
1241 T numOxDot = derivative(h,
1242 tm4h.getRotationRate().getX(), tm3h.getRotationRate().getX(),
1243 tm2h.getRotationRate().getX(), tm1h.getRotationRate().getX(),
1244 tp1h.getRotationRate().getX(), tp2h.getRotationRate().getX(),
1245 tp3h.getRotationRate().getX(), tp4h.getRotationRate().getX());
1246 T numOyDot = derivative(h,
1247 tm4h.getRotationRate().getY(), tm3h.getRotationRate().getY(),
1248 tm2h.getRotationRate().getY(), tm1h.getRotationRate().getY(),
1249 tp1h.getRotationRate().getY(), tp2h.getRotationRate().getY(),
1250 tp3h.getRotationRate().getY(), tp4h.getRotationRate().getY());
1251 T numOzDot = derivative(h,
1252 tm4h.getRotationRate().getZ(), tm3h.getRotationRate().getZ(),
1253 tm2h.getRotationRate().getZ(), tm1h.getRotationRate().getZ(),
1254 tp1h.getRotationRate().getZ(), tp2h.getRotationRate().getZ(),
1255 tp3h.getRotationRate().getZ(), tp4h.getRotationRate().getZ());
1256
1257
1258 T theXDot = t0.getVelocity().getX();
1259 T theYDot = t0.getVelocity().getY();
1260 T theZDot = t0.getVelocity().getZ();
1261 T theXDot2 = t0.getAcceleration().getX();
1262 T theYDot2 = t0.getAcceleration().getY();
1263 T theZDot2 = t0.getAcceleration().getZ();
1264 FieldRotation<T> r0 = t0.getRotation();
1265 FieldVector3D<T> w = t0.getRotationRate();
1266 FieldVector3D<T> q = new FieldVector3D<>(r0.getQ1(), r0.getQ2(), r0.getQ3());
1267 FieldVector3D<T> qw = FieldVector3D.crossProduct(q, w);
1268 T theQ0Dot = FieldVector3D.dotProduct(q, w).multiply(-0.5);
1269 T theQ1Dot = r0.getQ0().multiply(w.getX()).add(qw.getX()).multiply(0.5);
1270 T theQ2Dot = r0.getQ0().multiply(w.getY()).add(qw.getY()).multiply(0.5);
1271 T theQ3Dot = r0.getQ0().multiply(w.getZ()).add(qw.getZ()).multiply(0.5);
1272 T theOxDot2 = t0.getRotationAcceleration().getX();
1273 T theOyDot2 = t0.getRotationAcceleration().getY();
1274 T theOzDot2 = t0.getRotationAcceleration().getZ();
1275
1276
1277 Assertions.assertEquals(theXDot.getReal(), numXDot.getReal(), 1.0e-13 * v.getReal());
1278 Assertions.assertEquals(theYDot.getReal(), numYDot.getReal(), 1.0e-13 * v.getReal());
1279 Assertions.assertEquals(theZDot.getReal(), numZDot.getReal(), 1.0e-13 * v.getReal());
1280
1281 Assertions.assertEquals(theXDot2.getReal(), numXDot2.getReal(), 1.0e-13 * a.getReal());
1282 Assertions.assertEquals(theYDot2.getReal(), numYDot2.getReal(), 1.0e-13 * a.getReal());
1283 Assertions.assertEquals(theZDot2.getReal(), numZDot2.getReal(), 1.0e-13 * a.getReal());
1284
1285 Assertions.assertEquals(theQ0Dot.getReal(), numQ0Dot.getReal(), 1.0e-13 * omega.getReal());
1286 Assertions.assertEquals(theQ1Dot.getReal(), numQ1Dot.getReal(), 1.0e-13 * omega.getReal());
1287 Assertions.assertEquals(theQ2Dot.getReal(), numQ2Dot.getReal(), 1.0e-13 * omega.getReal());
1288 Assertions.assertEquals(theQ3Dot.getReal(), numQ3Dot.getReal(), 1.0e-13 * omega.getReal());
1289
1290
1291 Assertions.assertEquals(theOxDot2.getReal(), numOxDot.getReal(), 1.0e-12 * omegaDot.getReal());
1292 Assertions.assertEquals(theOyDot2.getReal(), numOyDot.getReal(), 1.0e-12 * omegaDot.getReal());
1293 Assertions.assertEquals(theOzDot2.getReal(), numOzDot.getReal(), 1.0e-12 * omegaDot.getReal());
1294
1295 }
1296 }
1297 }
1298
1299 @Test
1300 void testToStaticTransform() {
1301
1302 final Field<Complex> field = ComplexField.getInstance();
1303 final PVCoordinates pvCoordinates = new PVCoordinates();
1304 final AngularCoordinates angularCoordinates = new AngularCoordinates();
1305 final Transform transform = new Transform(AbsoluteDate.ARBITRARY_EPOCH, pvCoordinates, angularCoordinates);
1306 final FieldTransform<Complex> fieldTransform = new FieldTransform<>(field, transform);
1307
1308 final FieldStaticTransform<Complex> actualStaticTransform = fieldTransform.toStaticTransform();
1309
1310 final FieldStaticTransform<Complex> expectedStaticTransform = fieldTransform.staticShiftedBy(Complex.ZERO);
1311 Assertions.assertEquals(expectedStaticTransform.getDate(), actualStaticTransform.getDate());
1312 Assertions.assertEquals(expectedStaticTransform.getTranslation(), actualStaticTransform.getTranslation());
1313 Assertions.assertEquals(0., Rotation.distance(expectedStaticTransform.getRotation().toRotation(),
1314 actualStaticTransform.getRotation().toRotation()));
1315 }
1316
1317 @Test
1318 public void testInterpolation() {
1319 doTestInterpolation(Binary64Field.getInstance());
1320 }
1321
1322 private <T extends CalculusFieldElement<T>> void doTestInterpolation(Field<T> field) {
1323 FieldAbsoluteDate<T> t0 = FieldAbsoluteDate.getGalileoEpoch(field);
1324 List<FieldTransform<T>> sample = new ArrayList<FieldTransform<T>>();
1325 for (int i = 0; i < 5; ++i) {
1326 sample.add(evolvingTransform(t0, i * 0.8));
1327 }
1328
1329 for (double dt = 0.1; dt <= 3.1; dt += 0.01) {
1330 FieldTransform<T> reference = evolvingTransform(t0, dt);
1331 FieldTransform<T> interpolated = FieldTransform.interpolate(reference.getFieldDate(), sample);
1332 FieldTransform<T> error = new FieldTransform<>(reference.getFieldDate(), reference, interpolated.getInverse());
1333 Assertions.assertEquals(0.0, error.getCartesian().getPosition().getNorm().getReal(), 4.0e-12);
1334 Assertions.assertEquals(0.0, error.getCartesian().getVelocity().getNorm().getReal(), 3.0e-11);
1335 Assertions.assertEquals(0.0, error.getCartesian().getAcceleration().getNorm().getReal(), 3.0e-10);
1336 Assertions.assertEquals(0.0, error.getAngular().getRotation().getAngle().getReal(), 2.0e-10);
1337 Assertions.assertEquals(0.0, error.getAngular().getRotationRate().getNorm().getReal(), 2.0e-09);
1338 Assertions.assertEquals(0.0, error.getAngular().getRotationAcceleration().getNorm().getReal(), 8.0e-09);
1339
1340 }
1341
1342 }
1343
1344 private <T extends CalculusFieldElement<T>> FieldTransform<T> evolvingTransform(final FieldAbsoluteDate<T> t0, final double dt) {
1345
1346
1347 final Field<T> field = t0.getField();
1348 final double omega = 0.2;
1349 final FieldAbsoluteDate<T> date = t0.shiftedBy(dt);
1350 final double cos = FastMath.cos(omega * dt);
1351 final double sin = FastMath.sin(omega * dt);
1352 return new FieldTransform<>(date,
1353 new FieldTransform<>(date,
1354 createVector(field, -cos, -sin, 0),
1355 createVector(field, omega * sin, -omega * cos, 0),
1356 createVector(field, omega * omega * cos, omega * omega * sin, 0)),
1357 new FieldTransform<>(date,
1358 new FieldRotation<>(FieldVector3D.getPlusK(field),
1359 field.getZero().add(FastMath.PI - omega * dt),
1360 RotationConvention.VECTOR_OPERATOR),
1361 new FieldVector3D<>(omega, FieldVector3D.getPlusK(field))));
1362 }
1363
1364 private <T extends CalculusFieldElement<T>> T derivative(T h,
1365 T ym4h, T ym3h, T ym2h, T ym1h,
1366 T yp1h, T yp2h, T yp3h, T yp4h) {
1367 return yp4h.subtract(ym4h).multiply( -3).
1368 add(yp3h.subtract(ym3h).multiply( 32)).
1369 add(yp2h.subtract(ym2h).multiply(-168)).
1370 add(yp1h.subtract(ym1h).multiply( 672)).
1371 divide(h.multiply(840));
1372 }
1373
1374 private <T extends CalculusFieldElement<T>> FieldTransform<T> randomTransform(Field<T> field, RandomGenerator random) {
1375
1376 FieldTransform<T> combined = FieldTransform.getIdentity(field);
1377 for (int k = 0; k < 20; ++k) {
1378 FieldTransform<T> t = random.nextBoolean() ?
1379 new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field),
1380 randomVector(field, 1.0e3, random),
1381 randomVector(field, 1.0, random),
1382 randomVector(field, 1.0e-3, random)) :
1383 new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field),
1384 randomRotation(field, random),
1385 randomVector(field, 0.01, random),
1386 randomVector(field, 1.0e-4, random));
1387 combined = new FieldTransform<>(FieldAbsoluteDate.getJ2000Epoch(field), combined, t);
1388 }
1389 return combined;
1390 }
1391
1392 private <T extends CalculusFieldElement<T>> FieldVector3D<T> randomVector(Field<T> field, double scale, RandomGenerator random) {
1393 return createVector(field,
1394 random.nextDouble() * scale,
1395 random.nextDouble() * scale,
1396 random.nextDouble() * scale);
1397 }
1398
1399 private <T extends CalculusFieldElement<T>> FieldRotation<T> randomRotation(Field<T> field, RandomGenerator random) {
1400 double q0 = random.nextDouble() * 2 - 1;
1401 double q1 = random.nextDouble() * 2 - 1;
1402 double q2 = random.nextDouble() * 2 - 1;
1403 double q3 = random.nextDouble() * 2 - 1;
1404 return createRotation(field, q0, q1, q2, q3, true);
1405 }
1406
1407 private <T extends CalculusFieldElement<T>> void checkNoTransform(FieldTransform<T> transform,
1408 RandomGenerator random) {
1409 for (int i = 0; i < 100; ++i) {
1410 FieldVector3D<T> a = randomVector(transform.getFieldDate().getField(), 1.0e3, random);
1411 FieldVector3D<T> tA = transform.transformVector(a);
1412 Assertions.assertEquals(0, a.subtract(tA).getNorm().getReal(), 1.0e-10 * a.getNorm().getReal());
1413 FieldVector3D<T> b = randomVector(transform.getFieldDate().getField(), 1.0e3, random);
1414 FieldVector3D<T> tB = transform.transformPosition(b);
1415 Assertions.assertEquals(0, b.subtract(tB).getNorm().getReal(), 1.0e-10 * b.getNorm().getReal());
1416 FieldPVCoordinates<T> pv = new FieldPVCoordinates<>(randomVector(transform.getFieldDate().getField(), 1.0e3, random),
1417 randomVector(transform.getFieldDate().getField(), 1.0, random),
1418 randomVector(transform.getFieldDate().getField(), 1.0e-3, random));
1419 FieldPVCoordinates<T> tPv = transform.transformPVCoordinates(pv);
1420 checkVector(pv.getPosition(), tPv.getPosition(), 1.0e-10);
1421 checkVector(pv.getVelocity(), tPv.getVelocity(), 3.0e-9);
1422 checkVector(pv.getAcceleration(), tPv.getAcceleration(), 3.0e-9);
1423 }
1424 }
1425
1426 private <T extends CalculusFieldElement<T>> void checkVector(FieldVector3D<T> reference, FieldVector3D<T> result,
1427 double relativeTolerance) {
1428 T refNorm = reference.getNorm();
1429 T resNorm = result.getNorm();
1430 double tolerance = relativeTolerance * (1 + FastMath.max(refNorm.getReal(), resNorm.getReal()));
1431 Assertions.assertEquals(0, FieldVector3D.distance(reference, result).getReal(), tolerance,
1432 "ref = " + reference + ", res = " + result + " -> " +
1433 (FieldVector3D.distance(reference, result).divide(1 + FastMath.max(refNorm.getReal(), resNorm.getReal()))));
1434 }
1435
1436 private <T extends CalculusFieldElement<T>> FieldVector3D<T> createVector(Field<T> field,
1437 double x, double y, double z) {
1438 return new FieldVector3D<>(field.getZero().add(x),
1439 field.getZero().add(y),
1440 field.getZero().add(z));
1441 }
1442
1443 private <T extends CalculusFieldElement<T>> FieldRotation<T> createRotation(Field<T> field,
1444 double q0, double q1,
1445 double q2, double q3,
1446 boolean needsNormalization) {
1447 return new FieldRotation<>(field.getZero().add(q0),
1448 field.getZero().add(q1),
1449 field.getZero().add(q2),
1450 field.getZero().add(q3),
1451 needsNormalization);
1452 }
1453
1454 }