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.hamcrest.MatcherAssert;
20 import org.hipparchus.Field;
21 import org.hipparchus.complex.Complex;
22 import org.hipparchus.complex.ComplexField;
23 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
24 import org.hipparchus.geometry.euclidean.threed.Line;
25 import org.hipparchus.geometry.euclidean.threed.Rotation;
26 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
27 import org.hipparchus.geometry.euclidean.threed.Vector3D;
28 import org.hipparchus.linear.MatrixUtils;
29 import org.hipparchus.linear.RealMatrix;
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.hipparchus.util.SinCos;
36 import org.junit.jupiter.api.Assertions;
37 import org.junit.jupiter.api.Test;
38 import org.orekit.OrekitMatchers;
39 import org.orekit.Utils;
40 import org.orekit.time.AbsoluteDate;
41 import org.orekit.time.FieldAbsoluteDate;
42 import org.orekit.time.TimeOffset;
43 import org.orekit.time.TimeInterpolator;
44 import org.orekit.time.TimeScale;
45 import org.orekit.time.TimeScalesFactory;
46 import org.orekit.utils.AngularCoordinates;
47 import org.orekit.utils.CartesianDerivativesFilter;
48 import org.orekit.utils.Constants;
49 import org.orekit.utils.FieldPVCoordinates;
50 import org.orekit.utils.PVCoordinates;
51 import org.orekit.utils.TimeStampedFieldPVCoordinates;
52 import org.orekit.utils.TimeStampedPVCoordinates;
53 import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;
54
55 import java.util.ArrayList;
56 import java.util.List;
57
58 public class TransformTest {
59
60 @Test
61 void testIdentityTransformVector() {
62
63 final Transform identity = Transform.IDENTITY;
64 final Vector3D vector3D = new Vector3D(1, 2, 3);
65 final FieldVector3D<Complex> fieldVector3D = new FieldVector3D<>(ComplexField.getInstance(), vector3D);
66
67 final FieldVector3D<Complex> transformed = identity.transformVector(fieldVector3D);
68
69 Assertions.assertEquals(vector3D, transformed.toVector3D());
70 Assertions.assertEquals(identity.transformVector(vector3D), transformed.toVector3D());
71 Assertions.assertEquals(identity.transformPosition(vector3D), transformed.toVector3D());
72 Assertions.assertEquals(identity.transformPosition(fieldVector3D), transformed);
73 }
74
75 @Test
76 void testIdentityTransformPVCoordinates() {
77
78 final Transform identity = Transform.IDENTITY;
79 final Vector3D position = new Vector3D(1, 2, 3);
80 final Vector3D velocity = Vector3D.MINUS_K;
81 final PVCoordinates pv = new PVCoordinates(position, velocity);
82
83 final PVCoordinates transformed = identity.transformPVCoordinates(pv);
84
85 Assertions.assertEquals(position, transformed.getPosition());
86 Assertions.assertEquals(velocity, transformed.getVelocity());
87 Assertions.assertEquals(Vector3D.ZERO, transformed.getAcceleration());
88 final TimeStampedPVCoordinates timeStampedPVCoordinates = new TimeStampedPVCoordinates(identity.getDate(), position, velocity);
89 Assertions.assertEquals(transformed.getPosition(), identity.transformPVCoordinates(timeStampedPVCoordinates).getPosition());
90 Assertions.assertEquals(transformed.getVelocity(), identity.transformPVCoordinates(timeStampedPVCoordinates).getVelocity());
91 Assertions.assertEquals(transformed.getPosition(), identity.transformOnlyPV(timeStampedPVCoordinates).getPosition());
92 Assertions.assertEquals(transformed.getVelocity(), identity.transformOnlyPV(timeStampedPVCoordinates).getVelocity());
93 }
94
95 @Test
96 void testIdentityTransformOnlyPV() {
97
98 final Transform identity = Transform.IDENTITY;
99 final Vector3D position = new Vector3D(1, 2, 3);
100 final Vector3D velocity = Vector3D.MINUS_K;
101 final PVCoordinates pv = new PVCoordinates(position, velocity);
102
103 final PVCoordinates transformed = identity.transformOnlyPV(pv);
104
105 Assertions.assertEquals(position, transformed.getPosition());
106 Assertions.assertEquals(velocity, transformed.getVelocity());
107 Assertions.assertEquals(Vector3D.ZERO, transformed.getAcceleration());
108 Assertions.assertEquals(transformed.getPosition(), identity.transformPVCoordinates(pv).getPosition());
109 Assertions.assertEquals(transformed.getVelocity(), identity.transformPVCoordinates(pv).getVelocity());
110 final TimeStampedPVCoordinates timeStampedPVCoordinates = new TimeStampedPVCoordinates(identity.getDate(), position, velocity);
111 Assertions.assertEquals(transformed.getPosition(), identity.transformOnlyPV(timeStampedPVCoordinates).getPosition());
112 Assertions.assertEquals(transformed.getVelocity(), identity.transformOnlyPV(timeStampedPVCoordinates).getVelocity());
113 }
114
115 @Test
116 void testIdentityShiftedBy() {
117
118 final Transform identity = Transform.IDENTITY;
119
120 final Transform shiftedIdentity = identity.shiftedBy(1.);
121
122 Assertions.assertEquals(identity, shiftedIdentity);
123 }
124
125 @Test
126 void testIdentityStaticShiftedBy() {
127
128 final Transform identity = Transform.IDENTITY;
129
130 final StaticTransform shiftedIdentity = identity.staticShiftedBy(1.);
131
132 Assertions.assertEquals(StaticTransform.getIdentity().getClass(), shiftedIdentity.getClass());
133 }
134
135 @Test
136 void testIdentityGetStaticInverse() {
137
138 final Transform identity = Transform.IDENTITY;
139
140 final StaticTransform staticInverse = identity.getStaticInverse();
141
142 Assertions.assertEquals(identity.toStaticTransform().getClass(), staticInverse.getClass());
143 }
144
145 @Test
146 void testIdentityFreeze() {
147
148 final Transform identity = Transform.IDENTITY;
149
150 final Transform frozen = identity.freeze();
151
152 Assertions.assertEquals(identity, frozen);
153 }
154
155 @Test
156 public void testIdentityTranslation() {
157 checkNoTransform(new Transform(AbsoluteDate.J2000_EPOCH, new Vector3D(0, 0, 0)),
158 new Well19937a(0xfd118eac6b5ec136L));
159 }
160
161 @Test
162 public void testIdentityRotation() {
163 checkNoTransform(new Transform(AbsoluteDate.J2000_EPOCH, new Rotation(1, 0, 0, 0, false)),
164 new Well19937a(0xfd118eac6b5ec136L));
165 }
166
167 @Test
168 public void testIdentityLine() {
169 RandomGenerator random = new Well19937a(0x98603025df70db7cL);
170 Vector3D p1 = randomVector(100.0, random);
171 Vector3D p2 = randomVector(100.0, random);
172 Line line = new Line(p1, p2, 1.0e-6);
173 Line transformed = Transform.IDENTITY.transformLine(line);
174 Assertions.assertSame(line, transformed);
175 }
176
177 @Test
178 public void testFieldBackwardGeneration() {
179 Utils.setDataRoot("regular-data");
180 TimeScale utc = TimeScalesFactory.getUTC();
181 Frame tod = FramesFactory.getTOD(false);
182 Field<Binary64> field = Binary64Field.getInstance();
183 FieldTransform<Binary64> t1 =
184 tod.getParent().getTransformTo(tod, new FieldAbsoluteDate<>(field, new AbsoluteDate(2000, 8, 16, 21, 0, 0, utc)));
185 FieldTransform<Binary64> t2 =
186 tod.getParent().getTransformTo(tod, new FieldAbsoluteDate<>(field, new AbsoluteDate(2000, 8, 16, 9, 0, 0, utc)));
187 Assertions.assertEquals(-43200.0, t2.getDate().durationFrom(t1.getDate()), 1.0e-15);
188 }
189
190 @Test
191 public void testSimpleComposition() {
192 Transform transform =
193 new Transform(AbsoluteDate.J2000_EPOCH,
194 new Transform(AbsoluteDate.J2000_EPOCH,
195 new Rotation(Vector3D.PLUS_K, 0.5 * FastMath.PI,
196 RotationConvention.VECTOR_OPERATOR)),
197 new Transform(AbsoluteDate.J2000_EPOCH, Vector3D.PLUS_I));
198 Vector3D u = transform.transformPosition(new Vector3D(1.0, 1.0, 1.0));
199 Vector3D v = new Vector3D(0.0, 1.0, 1.0);
200 Assertions.assertEquals(0, u.subtract(v).getNorm(), 1.0e-15);
201 }
202
203 @Test
204 public void testAcceleration() {
205
206 PVCoordinates initPV = new PVCoordinates(new Vector3D(9, 8, 7), new Vector3D(6, 5, 4), new Vector3D(3, 2, 1));
207 for (TimeOffset dt = TimeOffset.ZERO; dt.compareTo(TimeOffset.SECOND) < 0; dt = dt.add(
208 TimeOffset.MILLISECOND.multiply(10))) {
209 PVCoordinates basePV = initPV.shiftedBy(dt.toDouble());
210 PVCoordinates transformedPV = evolvingTransform(AbsoluteDate.J2000_EPOCH, dt).transformPVCoordinates(basePV);
211
212
213 List<TimeStampedPVCoordinates> sample = new ArrayList<>();
214 TimeOffset h = TimeOffset.MILLISECOND.multiply(10);
215 for (int i = -3; i < 4; ++i) {
216 TimeOffset dthi = i < 0 ? dt.subtract(h.multiply(-i)) : dt.add(h.multiply(i));
217 Transform t = evolvingTransform(AbsoluteDate.J2000_EPOCH, dthi);
218 PVCoordinates pv = t.transformPVCoordinates(initPV.shiftedBy(dthi.toDouble()));
219 sample.add(new TimeStampedPVCoordinates(t.getDate(), pv.getPosition(), pv.getVelocity(), Vector3D.ZERO));
220 }
221
222
223 final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
224 new TimeStampedPVCoordinatesHermiteInterpolator(sample.size(), CartesianDerivativesFilter.USE_PV);
225
226 PVCoordinates rebuiltPV = interpolator.interpolate(AbsoluteDate.J2000_EPOCH.shiftedBy(dt), sample);
227
228 checkVector(rebuiltPV.getPosition(), transformedPV.getPosition(), 4.0e-16);
229 checkVector(rebuiltPV.getVelocity(), transformedPV.getVelocity(), 2.0e-16);
230 checkVector(rebuiltPV.getAcceleration(), transformedPV.getAcceleration(), 9.0e-11);
231
232 }
233
234 }
235
236 @Test
237 public void testAccelerationComposition() {
238 RandomGenerator random = new Well19937a(0x41fdd07d6c9e9f65L);
239
240 Vector3D p1 = randomVector(1.0e3, random);
241 Vector3D v1 = randomVector(1.0, random);
242 Vector3D a1 = randomVector(1.0e-3, random);
243 Rotation r1 = randomRotation(random);
244 Vector3D o1 = randomVector(0.1, random);
245
246 Vector3D p2 = randomVector(1.0e3, random);
247 Vector3D v2 = randomVector(1.0, random);
248 Vector3D a2 = randomVector(1.0e-3, random);
249 Rotation r2 = randomRotation(random);
250 Vector3D o2 = randomVector(0.1, random);
251
252 Transform t1 = new Transform(AbsoluteDate.J2000_EPOCH,
253 new Transform(AbsoluteDate.J2000_EPOCH, p1, v1, a1),
254 new Transform(AbsoluteDate.J2000_EPOCH, r1, o1));
255 Transform t2 = new Transform(AbsoluteDate.J2000_EPOCH,
256 new Transform(AbsoluteDate.J2000_EPOCH, p2, v2, a2),
257 new Transform(AbsoluteDate.J2000_EPOCH, r2, o2));
258 Transform t12 = new Transform(AbsoluteDate.J2000_EPOCH, t1, t2);
259
260 Vector3D q = randomVector(1.0e3, random);
261 Vector3D qDot = randomVector(1.0, random);
262 Vector3D qDotDot = randomVector(1.0e-3, random);
263
264 PVCoordinates pva0 = new PVCoordinates(q, qDot, qDotDot);
265 PVCoordinates pva1 = t1.transformPVCoordinates(pva0);
266 PVCoordinates pva2 = t2.transformPVCoordinates(pva1);
267 PVCoordinates pvac = t12.transformPVCoordinates(pva0);
268
269 checkVector(pva2.getPosition(), pvac.getPosition(), 1.0e-15);
270 checkVector(pva2.getVelocity(), pvac.getVelocity(), 1.0e-15);
271 checkVector(pva2.getAcceleration(), pvac.getAcceleration(), 1.0e-15);
272
273
274
275
276 Assertions.assertEquals(0.0, t1.getAngular().getRotationAcceleration().getNorm(), 1.0e-15);
277 Assertions.assertEquals(0.0, t2.getAngular().getRotationAcceleration().getNorm(), 1.0e-15);
278 Assertions.assertTrue(t12.getAngular().getRotationAcceleration().getNorm() > 0.01);
279
280 Assertions.assertEquals(0.0, t12.freeze().getCartesian().getVelocity().getNorm(), 1.0e-15);
281 Assertions.assertEquals(0.0, t12.freeze().getCartesian().getAcceleration().getNorm(), 1.0e-15);
282 Assertions.assertEquals(0.0, t12.freeze().getAngular().getRotationRate().getNorm(), 1.0e-15);
283 Assertions.assertEquals(0.0, t12.freeze().getAngular().getRotationAcceleration().getNorm(), 1.0e-15);
284
285 }
286
287 @Test
288 public void testRandomComposition() {
289
290 RandomGenerator random = new Well19937a(0x171c79e323a1123L);
291 for (int i = 0; i < 20; ++i) {
292
293
294 int n = random.nextInt(20);
295 Transform[] transforms = new Transform[n];
296 Transform combined = Transform.IDENTITY;
297 for (int k = 0; k < n; ++k) {
298 transforms[k] = random.nextBoolean()
299 ? new Transform(AbsoluteDate.J2000_EPOCH, randomVector(1.0e3, random), randomVector(1.0, random), randomVector(1.0e-3, random))
300 : new Transform(AbsoluteDate.J2000_EPOCH, randomRotation(random), randomVector(0.01, random), randomVector(1.0e-4, random));
301 combined = new Transform(AbsoluteDate.J2000_EPOCH, combined, transforms[k]);
302 }
303
304
305 for (int j = 0; j < 10; ++j) {
306 Vector3D a = randomVector(1.0, random);
307 FieldVector3D<Binary64> aF = new FieldVector3D<>(Binary64Field.getInstance(), a);
308 Vector3D b = randomVector(1.0e3, random);
309 PVCoordinates c = new PVCoordinates(randomVector(1.0e3, random), randomVector(1.0, random), randomVector(1.0e-3, random));
310 Vector3D aRef = a;
311 FieldVector3D<Binary64> aFRef = aF;
312 Vector3D bRef = b;
313 PVCoordinates cRef = c;
314 for (int k = 0; k < n; ++k) {
315 aRef = transforms[k].transformVector(aRef);
316 aFRef = transforms[k].transformVector(aFRef);
317 bRef = transforms[k].transformPosition(bRef);
318 cRef = transforms[k].transformPVCoordinates(cRef);
319 }
320
321 Vector3D aCombined = combined.transformVector(a);
322 FieldVector3D<Binary64> aFCombined = combined.transformVector(aF);
323 Vector3D bCombined = combined.transformPosition(b);
324 PVCoordinates cCombined = combined.transformPVCoordinates(c);
325 checkVector(aRef, aCombined, 3.0e-15);
326 checkVector(aFRef.toVector3D(), aFCombined.toVector3D(), 3.0e-15);
327 checkVector(bRef, bCombined, 5.0e-15);
328 checkVector(cRef.getPosition(), cCombined.getPosition(), 1.0e-14);
329 checkVector(cRef.getVelocity(), cCombined.getVelocity(), 1.0e-14);
330 checkVector(cRef.getAcceleration(), cCombined.getAcceleration(), 1.0e-14);
331
332 }
333 }
334
335 }
336
337 @Test
338 public void testReverse() {
339 RandomGenerator random = new Well19937a(0x9f82ba2b2c98dac5L);
340 for (int i = 0; i < 20; ++i) {
341 Transform combined = randomTransform(random);
342
343 checkNoTransform(new Transform(AbsoluteDate.J2000_EPOCH, combined, combined.getInverse()), random);
344
345 }
346
347 }
348
349 @Test
350 public void testIdentityJacobianP() {
351 doTestIdentityJacobian(3, CartesianDerivativesFilter.USE_P);
352 }
353
354 @Test
355 public void testIdentityJacobianPV() {
356 doTestIdentityJacobian(6, CartesianDerivativesFilter.USE_PV);
357 }
358
359 @Test
360 public void testIdentityJacobianPVA() {
361 doTestIdentityJacobian(9, CartesianDerivativesFilter.USE_PVA);
362 }
363
364 private void doTestIdentityJacobian(int n, CartesianDerivativesFilter filter) {
365 double[][] jacobian = new double[n][n];
366 Transform.IDENTITY.getJacobian(filter, jacobian);
367 for (int i = 0; i < n; ++i) {
368 for (int j = 0; j < n; ++j) {
369 Assertions.assertEquals(i == j ? 1.0 : 0.0, jacobian[i][j], 1.0e-15);
370 }
371 }
372 }
373
374 @Test
375 public void testDecomposeAndRebuild() {
376 RandomGenerator random = new Well19937a(0xb8ee9da1b05198c9L);
377 for (int i = 0; i < 20; ++i) {
378 Transform combined = randomTransform(random);
379 Transform rebuilt = new Transform(combined.getDate(),
380 new Transform(combined.getDate(), combined.getTranslation(),
381 combined.getVelocity(), combined.getAcceleration()),
382 new Transform(combined.getDate(), combined.getRotation(),
383 combined.getRotationRate(), combined.getRotationAcceleration()));
384
385 checkNoTransform(new Transform(AbsoluteDate.J2000_EPOCH, combined, rebuilt.getInverse()), random);
386
387 }
388
389 }
390
391 @Test
392 public void testTranslation() {
393 RandomGenerator rnd = new Well19937a(0x7e9d737ba4147787L);
394 for (int i = 0; i < 10; ++i) {
395 Vector3D delta = randomVector(1.0e3, rnd);
396 Transform transform = new Transform(AbsoluteDate.J2000_EPOCH, delta);
397 for (int j = 0; j < 10; ++j) {
398 Vector3D a = new Vector3D(rnd.nextDouble(), rnd.nextDouble(), rnd.nextDouble());
399 Vector3D b = transform.transformVector(a);
400 Assertions.assertEquals(0, b.subtract(a).getNorm(), 1.0e-15);
401 Vector3D c = transform.transformPosition(a);
402 Assertions.assertEquals(0,
403 c.subtract(a).subtract(delta).getNorm(),
404 1.0e-14);
405 }
406 }
407 }
408
409 @Test
410 public void testRoughTransPV() {
411
412 PVCoordinates pointP1 = new PVCoordinates(Vector3D.PLUS_I, Vector3D.PLUS_I, Vector3D.PLUS_I);
413
414
415 PVCoordinates pointP2 = new PVCoordinates(new Vector3D(0, 0, 0), new Vector3D(0, 0, 0));
416 Transform R1toR2 = new Transform(AbsoluteDate.J2000_EPOCH, Vector3D.MINUS_I, Vector3D.MINUS_I, Vector3D.MINUS_I);
417 PVCoordinates result1 = R1toR2.transformPVCoordinates(pointP1);
418 checkVector(pointP2.getPosition(), result1.getPosition(), 1.0e-15);
419 checkVector(pointP2.getVelocity(), result1.getVelocity(), 1.0e-15);
420 checkVector(pointP2.getAcceleration(), result1.getAcceleration(), 1.0e-15);
421
422
423 Transform R2toR1 = R1toR2.getInverse();
424 PVCoordinates invResult1 = R2toR1.transformPVCoordinates(pointP2);
425 checkVector(pointP1.getPosition(), invResult1.getPosition(), 1.0e-15);
426 checkVector(pointP1.getVelocity(), invResult1.getVelocity(), 1.0e-15);
427 checkVector(pointP1.getAcceleration(), invResult1.getAcceleration(), 1.0e-15);
428
429
430 PVCoordinates pointP3 = new PVCoordinates(Vector3D.PLUS_J, new Vector3D(-2, 1, 0), new Vector3D(-4, -3, -1));
431 Rotation R = new Rotation(Vector3D.PLUS_K, FastMath.PI / 2, RotationConvention.VECTOR_OPERATOR);
432 Transform R1toR3 = new Transform(AbsoluteDate.J2000_EPOCH, R, new Vector3D(0, 0, -2), new Vector3D(1, 0, 0));
433 PVCoordinates result2 = R1toR3.transformPVCoordinates(pointP1);
434 checkVector(pointP3.getPosition(), result2.getPosition(), 1.0e-15);
435 checkVector(pointP3.getVelocity(), result2.getVelocity(), 1.0e-15);
436 checkVector(pointP3.getAcceleration(), result2.getAcceleration(), 1.0e-15);
437
438
439 Transform R3toR1 = R1toR3.getInverse();
440 PVCoordinates invResult2 = R3toR1.transformPVCoordinates(pointP3);
441 checkVector(pointP1.getPosition(), invResult2.getPosition(), 1.0e-15);
442 checkVector(pointP1.getVelocity(), invResult2.getVelocity(), 1.0e-15);
443 checkVector(pointP1.getAcceleration(), invResult2.getAcceleration(), 1.0e-15);
444
445
446 Transform R1toR4 = new Transform(AbsoluteDate.J2000_EPOCH, new Vector3D(-2, 0, 0), new Vector3D(-2, 0, 0), new Vector3D(-2, 0, 0));
447 PVCoordinates pointP4 = new PVCoordinates(new Vector3D(-1, 0, 0), new Vector3D(-1, 0, 0), new Vector3D(-1, 0, 0));
448 Transform R2toR4 = new Transform(AbsoluteDate.J2000_EPOCH, R2toR1, R1toR4);
449 PVCoordinates compResult = R2toR4.transformPVCoordinates(pointP2);
450 checkVector(pointP4.getPosition(), compResult.getPosition(), 1.0e-15);
451 checkVector(pointP4.getVelocity(), compResult.getVelocity(), 1.0e-15);
452 checkVector(pointP4.getAcceleration(), compResult.getAcceleration(), 1.0e-15);
453
454
455 PVCoordinates pointP5 = new PVCoordinates(new Vector3D(-1, 0, 0), new Vector3D(-1, 0, 3), new Vector3D(8, 0, 6));
456 Rotation R2 = new Rotation( new Vector3D(0, 0, 1), FastMath.PI, RotationConvention.VECTOR_OPERATOR);
457 Transform R1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, R2, new Vector3D(0, -3, 0));
458 Transform R3toR5 = new Transform (AbsoluteDate.J2000_EPOCH, R3toR1, R1toR5);
459 PVCoordinates combResult = R3toR5.transformPVCoordinates(pointP3);
460 checkVector(pointP5.getPosition(), combResult.getPosition(), 1.0e-15);
461 checkVector(pointP5.getVelocity(), combResult.getVelocity(), 1.0e-15);
462 checkVector(pointP5.getAcceleration(), combResult.getAcceleration(), 1.0e-15);
463
464
465 Transform R2toR3 = new Transform (AbsoluteDate.J2000_EPOCH, R2toR1, R1toR3);
466 PVCoordinates result = R2toR3.transformPVCoordinates(pointP2);
467 checkVector(pointP3.getPosition(), result.getPosition(), 1.0e-15);
468 checkVector(pointP3.getVelocity(), result.getVelocity(), 1.0e-15);
469 checkVector(pointP3.getAcceleration(), result.getAcceleration(), 1.0e-15);
470
471 Transform R3toR2 = new Transform (AbsoluteDate.J2000_EPOCH, R3toR1, R1toR2);
472 result = R3toR2.transformPVCoordinates(pointP3);
473 checkVector(pointP2.getPosition(), result.getPosition(), 1.0e-15);
474 checkVector(pointP2.getVelocity(), result.getVelocity(), 1.0e-15);
475 checkVector(pointP2.getAcceleration(), result.getAcceleration(), 1.0e-15);
476
477 Transform newR1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, R1toR2, R2toR3);
478 newR1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, newR1toR5, R3toR5);
479 result = newR1toR5.transformPVCoordinates(pointP1);
480 checkVector(pointP5.getPosition(), result.getPosition(), 1.0e-15);
481 checkVector(pointP5.getVelocity(), result.getVelocity(), 1.0e-15);
482 checkVector(pointP5.getAcceleration(), result.getAcceleration(), 1.0e-15);
483
484
485 newR1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, R1toR2, R2toR3);
486 Transform R3toR4 = new Transform(AbsoluteDate.J2000_EPOCH, R3toR1, R1toR4);
487 newR1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, newR1toR5, R3toR4);
488 Transform R4toR5 = new Transform(AbsoluteDate.J2000_EPOCH, R1toR4.getInverse(), R1toR5);
489 newR1toR5 = new Transform(AbsoluteDate.J2000_EPOCH, newR1toR5, R4toR5);
490 result = newR1toR5.transformPVCoordinates(pointP1);
491 checkVector(pointP5.getPosition(), result.getPosition(), 1.0e-15);
492 checkVector(pointP5.getVelocity(), result.getVelocity(), 1.0e-15);
493 checkVector(pointP5.getAcceleration(), result.getAcceleration(), 1.0e-15);
494
495 }
496
497 @Test
498 public void testRotPV() {
499
500 RandomGenerator rnd = new Well19937a(0x73d5554d99427af0L);
501
502
503
504 for (int i = 0; i < 10; ++i) {
505
506
507
508 Rotation instantRot = randomRotation(rnd);
509 Vector3D normAxis = instantRot.getAxis(RotationConvention.VECTOR_OPERATOR);
510 double w = FastMath.abs(instantRot.getAngle()) / Constants.JULIAN_DAY;
511
512
513 Rotation rot = randomRotation(rnd);
514
515
516 Transform tr = new Transform(AbsoluteDate.J2000_EPOCH, rot, new Vector3D(w, normAxis));
517
518
519 Vector3D pos = randomVector(1.0e3, rnd);
520 Vector3D vel = randomVector(1.0, rnd);
521 Vector3D acc = randomVector(1.0e-3, rnd);
522
523 PVCoordinates pvOne = new PVCoordinates(pos, vel, acc);
524
525
526
527 PVCoordinates pvTwo = tr.transformPVCoordinates(pvOne);
528
529
530
531 Vector3D resultvel = tr.getInverse().transformPVCoordinates(pvTwo).getVelocity();
532
533 checkVector(resultvel, vel, 1.0e-15);
534
535 }
536
537 }
538
539 @Test
540 public void testTransPV() {
541
542 RandomGenerator rnd = new Well19937a(0x73d5554d99427af0L);
543
544
545
546 for (int i = 0; i < 10; ++i) {
547
548
549 Vector3D pos = randomVector(1.0e3, rnd);
550 Vector3D vel = randomVector(1.0, rnd);
551 Vector3D acc = randomVector(1.0e-3, rnd);
552 PVCoordinates pvOne = new PVCoordinates(pos, vel, acc);
553
554
555 Vector3D transPos = randomVector(1.0e3, rnd);
556 Vector3D transVel = randomVector(1.0, rnd);
557 Vector3D transAcc = randomVector(1.0e-3, rnd);
558 Transform tr = new Transform(AbsoluteDate.J2000_EPOCH, transPos, transVel, transAcc);
559
560 double dt = 1;
561
562
563 Vector3D good = tr.transformPosition(pos.add(new Vector3D(dt, vel))).add(new Vector3D(dt, transVel));
564
565
566 PVCoordinates pvTwo = tr.transformPVCoordinates(pvOne);
567 Vector3D result = pvTwo.getPosition().add(new Vector3D(dt, pvTwo.getVelocity()));
568 checkVector(good, result, 1.0e-15);
569
570 FieldPVCoordinates<Binary64> fieldPVOne =
571 new FieldPVCoordinates<>(new FieldVector3D<>(Binary64Field.getInstance(), pvOne.getPosition()),
572 new FieldVector3D<>(Binary64Field.getInstance(), pvOne.getVelocity()),
573 new FieldVector3D<>(Binary64Field.getInstance(), pvOne.getAcceleration()));
574 FieldPVCoordinates<Binary64> fieldPVTwo = tr.transformPVCoordinates(fieldPVOne);
575 FieldVector3D<Binary64> fieldResult =
576 fieldPVTwo.getPosition().add(new FieldVector3D<>(dt, fieldPVTwo.getVelocity()));
577 checkVector(good, fieldResult.toVector3D(), 1.0e-15);
578
579 TimeStampedFieldPVCoordinates<Binary64> fieldTPVOne =
580 new TimeStampedFieldPVCoordinates<>(tr.getDate(),
581 new FieldVector3D<>(Binary64Field.getInstance(), pvOne.getPosition()),
582 new FieldVector3D<>(Binary64Field.getInstance(), pvOne.getVelocity()),
583 new FieldVector3D<>(Binary64Field.getInstance(), pvOne.getAcceleration()));
584 TimeStampedFieldPVCoordinates<Binary64> fieldTPVTwo = tr.transformPVCoordinates(fieldTPVOne);
585 FieldVector3D<Binary64> fieldTResult =
586 fieldTPVTwo.getPosition().add(new FieldVector3D<>(dt, fieldTPVTwo.getVelocity()));
587 checkVector(good, fieldTResult.toVector3D(), 1.0e-15);
588
589
590 Vector3D resultvel = tr.getInverse().
591 transformPVCoordinates(pvTwo).getVelocity();
592 checkVector(resultvel, vel, 1.0e-15);
593
594 }
595
596 }
597
598 @Test
599 public void testRotation() {
600 RandomGenerator rnd = new Well19937a(0x73d5554d99427af0L);
601 for (int i = 0; i < 10; ++i) {
602
603 Rotation r = randomRotation(rnd);
604 Vector3D axis = r.getAxis(RotationConvention.VECTOR_OPERATOR);
605 double angle = r.getAngle();
606
607 Transform transform = new Transform(AbsoluteDate.J2000_EPOCH, r);
608 for (int j = 0; j < 10; ++j) {
609 Vector3D a = new Vector3D(rnd.nextDouble(), rnd.nextDouble(), rnd.nextDouble());
610 Vector3D b = transform.transformVector(a);
611 Assertions.assertEquals(Vector3D.angle(axis, a), Vector3D.angle(axis, b), 1.0e-14);
612 Vector3D aOrtho = Vector3D.crossProduct(axis, a);
613 Vector3D bOrtho = Vector3D.crossProduct(axis, b);
614 Assertions.assertEquals(angle, Vector3D.angle(aOrtho, bOrtho), 1.0e-14);
615 Vector3D c = transform.transformPosition(a);
616 Assertions.assertEquals(0, c.subtract(b).getNorm(), 1.0e-14);
617 }
618
619 }
620 }
621
622 @Test
623 public void testJacobianP() {
624
625
626 PVCoordinates[] directions = new PVCoordinates[] {
627 new PVCoordinates(Vector3D.PLUS_I, Vector3D.ZERO, Vector3D.ZERO),
628 new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO),
629 new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO),
630 };
631 double h = 0.01;
632
633 RandomGenerator random = new Well19937a(0x47fd0d6809f4b173L);
634 for (int i = 0; i < 20; ++i) {
635
636
637 Transform combined = randomTransform(random);
638
639
640 double[][] jacobian = new double[9][9];
641 for (int l = 0; l < jacobian.length; ++l) {
642 for (int c = 0; c < jacobian[l].length; ++c) {
643 jacobian[l][c] = l + 0.1 * c;
644 }
645 }
646 combined.getJacobian(CartesianDerivativesFilter.USE_P, jacobian);
647
648 for (int j = 0; j < 100; ++j) {
649
650 PVCoordinates pv0 = new PVCoordinates(randomVector(1e3, random), randomVector(1.0, random), randomVector(1.0e-3, random));
651 double epsilonP = 2.0e-12 * pv0.getPosition().getNorm();
652
653 for (int c = 0; c < directions.length; ++c) {
654
655
656 PVCoordinates pvm4h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -4 * h, directions[c]));
657 PVCoordinates pvm3h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -3 * h, directions[c]));
658 PVCoordinates pvm2h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -2 * h, directions[c]));
659 PVCoordinates pvm1h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -1 * h, directions[c]));
660 PVCoordinates pvp1h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +1 * h, directions[c]));
661 PVCoordinates pvp2h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +2 * h, directions[c]));
662 PVCoordinates pvp3h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +3 * h, directions[c]));
663 PVCoordinates pvp4h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +4 * h, directions[c]));
664 PVCoordinates d4 = new PVCoordinates(pvm4h, pvp4h);
665 PVCoordinates d3 = new PVCoordinates(pvm3h, pvp3h);
666 PVCoordinates d2 = new PVCoordinates(pvm2h, pvp2h);
667 PVCoordinates d1 = new PVCoordinates(pvm1h, pvp1h);
668 double d = 1.0 / (840 * h);
669 PVCoordinates estimatedColumn = new PVCoordinates(-3 * d, d4, 32 * d, d3, -168 * d, d2, 672 * d, d1);
670
671
672 Assertions.assertEquals(estimatedColumn.getPosition().getX(), jacobian[0][c], epsilonP);
673 Assertions.assertEquals(estimatedColumn.getPosition().getY(), jacobian[1][c], epsilonP);
674 Assertions.assertEquals(estimatedColumn.getPosition().getZ(), jacobian[2][c], epsilonP);
675
676
677 for (int l = 3; l < jacobian.length; ++l) {
678 Assertions.assertEquals(l + 0.1 * c, jacobian[l][c], 1.0e-15);
679 }
680
681 }
682
683
684 for (int c = directions.length; c < jacobian[0].length; ++c) {
685 for (int l = 0; l < jacobian.length; ++l) {
686 Assertions.assertEquals(l + 0.1 * c, jacobian[l][c], 1.0e-15);
687 }
688 }
689
690 }
691 }
692
693 }
694
695 @Test
696 public void testJacobianPV() {
697
698
699 PVCoordinates[] directions = new PVCoordinates[] {
700 new PVCoordinates(Vector3D.PLUS_I, Vector3D.ZERO, Vector3D.ZERO),
701 new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO),
702 new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO),
703 new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_I, Vector3D.ZERO),
704 new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_J, Vector3D.ZERO),
705 new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_K, Vector3D.ZERO)
706 };
707 double h = 0.01;
708
709 RandomGenerator random = new Well19937a(0xce2bfddfbb9796beL);
710 for (int i = 0; i < 20; ++i) {
711
712
713 Transform combined = randomTransform(random);
714
715
716 double[][] jacobian = new double[9][9];
717 for (int l = 0; l < jacobian.length; ++l) {
718 for (int c = 0; c < jacobian[l].length; ++c) {
719 jacobian[l][c] = l + 0.1 * c;
720 }
721 }
722 combined.getJacobian(CartesianDerivativesFilter.USE_PV, jacobian);
723
724 for (int j = 0; j < 100; ++j) {
725
726 PVCoordinates pv0 = new PVCoordinates(randomVector(1e3, random), randomVector(1.0, random), randomVector(1.0e-3, random));
727 double epsilonP = 2.0e-12 * pv0.getPosition().getNorm();
728 double epsilonV = 6.0e-11 * pv0.getVelocity().getNorm();
729
730 for (int c = 0; c < directions.length; ++c) {
731
732
733 PVCoordinates pvm4h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -4 * h, directions[c]));
734 PVCoordinates pvm3h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -3 * h, directions[c]));
735 PVCoordinates pvm2h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -2 * h, directions[c]));
736 PVCoordinates pvm1h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -1 * h, directions[c]));
737 PVCoordinates pvp1h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +1 * h, directions[c]));
738 PVCoordinates pvp2h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +2 * h, directions[c]));
739 PVCoordinates pvp3h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +3 * h, directions[c]));
740 PVCoordinates pvp4h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +4 * h, directions[c]));
741 PVCoordinates d4 = new PVCoordinates(pvm4h, pvp4h);
742 PVCoordinates d3 = new PVCoordinates(pvm3h, pvp3h);
743 PVCoordinates d2 = new PVCoordinates(pvm2h, pvp2h);
744 PVCoordinates d1 = new PVCoordinates(pvm1h, pvp1h);
745 double d = 1.0 / (840 * h);
746 PVCoordinates estimatedColumn = new PVCoordinates(-3 * d, d4, 32 * d, d3, -168 * d, d2, 672 * d, d1);
747
748
749 Assertions.assertEquals(estimatedColumn.getPosition().getX(), jacobian[0][c], epsilonP);
750 Assertions.assertEquals(estimatedColumn.getPosition().getY(), jacobian[1][c], epsilonP);
751 Assertions.assertEquals(estimatedColumn.getPosition().getZ(), jacobian[2][c], epsilonP);
752 Assertions.assertEquals(estimatedColumn.getVelocity().getX(), jacobian[3][c], epsilonV);
753 Assertions.assertEquals(estimatedColumn.getVelocity().getY(), jacobian[4][c], epsilonV);
754 Assertions.assertEquals(estimatedColumn.getVelocity().getZ(), jacobian[5][c], epsilonV);
755
756
757 for (int l = 6; l < jacobian.length; ++l) {
758 Assertions.assertEquals(l + 0.1 * c, jacobian[l][c], 1.0e-15);
759 }
760
761 }
762
763
764 for (int c = directions.length; c < jacobian[0].length; ++c) {
765 for (int l = 0; l < jacobian.length; ++l) {
766 Assertions.assertEquals(l + 0.1 * c, jacobian[l][c], 1.0e-15);
767 }
768 }
769
770 }
771 }
772
773 }
774
775 @Test
776 public void testJacobianPVA() {
777
778
779 PVCoordinates[] directions = new PVCoordinates[] {
780 new PVCoordinates(Vector3D.PLUS_I, Vector3D.ZERO, Vector3D.ZERO),
781 new PVCoordinates(Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.ZERO),
782 new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO),
783 new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_I, Vector3D.ZERO),
784 new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_J, Vector3D.ZERO),
785 new PVCoordinates(Vector3D.ZERO, Vector3D.PLUS_K, Vector3D.ZERO),
786 new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.PLUS_I),
787 new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.PLUS_J),
788 new PVCoordinates(Vector3D.ZERO, Vector3D.ZERO, Vector3D.PLUS_K)
789 };
790 double h = 0.01;
791
792 RandomGenerator random = new Well19937a(0xd223e88b6232198fL);
793 for (int i = 0; i < 20; ++i) {
794
795
796 Transform combined = randomTransform(random);
797
798
799 double[][] jacobian = new double[9][9];
800 for (int l = 0; l < jacobian.length; ++l) {
801 for (int c = 0; c < jacobian[l].length; ++c) {
802 jacobian[l][c] = l + 0.1 * c;
803 }
804 }
805 combined.getJacobian(CartesianDerivativesFilter.USE_PVA, jacobian);
806
807 for (int j = 0; j < 100; ++j) {
808
809 PVCoordinates pv0 = new PVCoordinates(randomVector(1e3, random), randomVector(1.0, random), randomVector(1.0e-3, random));
810 double epsilonP = 2.0e-12 * pv0.getPosition().getNorm();
811 double epsilonV = 6.0e-11 * pv0.getVelocity().getNorm();
812 double epsilonA = 2.0e-9 * pv0.getAcceleration().getNorm();
813
814 for (int c = 0; c < directions.length; ++c) {
815
816
817 PVCoordinates pvm4h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -4 * h, directions[c]));
818 PVCoordinates pvm3h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -3 * h, directions[c]));
819 PVCoordinates pvm2h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -2 * h, directions[c]));
820 PVCoordinates pvm1h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, -1 * h, directions[c]));
821 PVCoordinates pvp1h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +1 * h, directions[c]));
822 PVCoordinates pvp2h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +2 * h, directions[c]));
823 PVCoordinates pvp3h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +3 * h, directions[c]));
824 PVCoordinates pvp4h = combined.transformPVCoordinates(new PVCoordinates(1.0, pv0, +4 * h, directions[c]));
825 PVCoordinates d4 = new PVCoordinates(pvm4h, pvp4h);
826 PVCoordinates d3 = new PVCoordinates(pvm3h, pvp3h);
827 PVCoordinates d2 = new PVCoordinates(pvm2h, pvp2h);
828 PVCoordinates d1 = new PVCoordinates(pvm1h, pvp1h);
829 double d = 1.0 / (840 * h);
830 PVCoordinates estimatedColumn = new PVCoordinates(-3 * d, d4, 32 * d, d3, -168 * d, d2, 672 * d, d1);
831
832
833 Assertions.assertEquals(estimatedColumn.getPosition().getX(), jacobian[0][c], epsilonP);
834 Assertions.assertEquals(estimatedColumn.getPosition().getY(), jacobian[1][c], epsilonP);
835 Assertions.assertEquals(estimatedColumn.getPosition().getZ(), jacobian[2][c], epsilonP);
836 Assertions.assertEquals(estimatedColumn.getVelocity().getX(), jacobian[3][c], epsilonV);
837 Assertions.assertEquals(estimatedColumn.getVelocity().getY(), jacobian[4][c], epsilonV);
838 Assertions.assertEquals(estimatedColumn.getVelocity().getZ(), jacobian[5][c], epsilonV);
839 Assertions.assertEquals(estimatedColumn.getAcceleration().getX(), jacobian[6][c], epsilonA);
840 Assertions.assertEquals(estimatedColumn.getAcceleration().getY(), jacobian[7][c], epsilonA);
841 Assertions.assertEquals(estimatedColumn.getAcceleration().getZ(), jacobian[8][c], epsilonA);
842
843 }
844
845 }
846 }
847
848 }
849
850 @Test
851 public void testLine() {
852 RandomGenerator random = new Well19937a(0x4a5ff67426c5731fL);
853 for (int i = 0; i < 100; ++i) {
854 Transform transform = randomTransform(random);
855 for (int j = 0; j < 20; ++j) {
856 Vector3D p0 = randomVector(1.0e3, random);
857 Vector3D p1 = randomVector(1.0e3, random);
858 Line l = new Line(p0, p1, 1.0e-10);
859 Line transformed = transform.transformLine(l);
860 for (int k = 0; k < 10; ++k) {
861 Vector3D p = l.pointAt(random.nextDouble() * 1.0e6);
862 Assertions.assertEquals(0.0, transformed.distance(transform.transformPosition(p)), 1.0e-9);
863 }
864 }
865 }
866 }
867
868 @Test
869 public void testLinear() {
870
871 RandomGenerator random = new Well19937a(0x14f6411217b148d8L);
872 for (int n = 0; n < 100; ++n) {
873 Transform t = randomTransform(random);
874
875
876 RealMatrix linearA = MatrixUtils.createRealMatrix(3, 4);
877 linearA.setSubMatrix(t.getRotation().getMatrix(), 0, 0);
878 Vector3D rt = t.getRotation().applyTo(t.getTranslation());
879 linearA.setEntry(0, 3, rt.getX());
880 linearA.setEntry(1, 3, rt.getY());
881 linearA.setEntry(2, 3, rt.getZ());
882
883
884 RealMatrix linearB = MatrixUtils.createRealMatrix(3, 4);
885 Vector3D p0 = t.transformPosition(Vector3D.ZERO);
886 Vector3D pI = t.transformPosition(Vector3D.PLUS_I).subtract(p0);
887 Vector3D pJ = t.transformPosition(Vector3D.PLUS_J).subtract(p0);
888 Vector3D pK = t.transformPosition(Vector3D.PLUS_K).subtract(p0);
889 linearB.setColumn(0, new double[] { pI.getX(), pI.getY(), pI.getZ() });
890 linearB.setColumn(1, new double[] { pJ.getX(), pJ.getY(), pJ.getZ() });
891 linearB.setColumn(2, new double[] { pK.getX(), pK.getY(), pK.getZ() });
892 linearB.setColumn(3, new double[] { p0.getX(), p0.getY(), p0.getZ() });
893
894
895 Assertions.assertEquals(0.0, linearB.subtract(linearA).getNorm1(),
896 1.0e-15 * linearA.getNorm1());
897
898 for (int i = 0; i < 100; ++i) {
899 Vector3D p = randomVector(1.0e3, random);
900 Vector3D q = t.transformPosition(p);
901
902 double[] qA = linearA.operate(new double[] { p.getX(), p.getY(), p.getZ(), 1.0 });
903 Assertions.assertEquals(q.getX(), qA[0], 1.0e-13 * p.getNorm());
904 Assertions.assertEquals(q.getY(), qA[1], 1.0e-13 * p.getNorm());
905 Assertions.assertEquals(q.getZ(), qA[2], 1.0e-13 * p.getNorm());
906
907 double[] qB = linearB.operate(new double[] { p.getX(), p.getY(), p.getZ(), 1.0 });
908 Assertions.assertEquals(q.getX(), qB[0], 1.0e-10 * p.getNorm());
909 Assertions.assertEquals(q.getY(), qB[1], 1.0e-10 * p.getNorm());
910 Assertions.assertEquals(q.getZ(), qB[2], 1.0e-10 * p.getNorm());
911
912 }
913
914 }
915
916 }
917
918 @Test
919 public void testShift() {
920
921
922
923
924
925
926 AbsoluteDate date = AbsoluteDate.GALILEO_EPOCH;
927 double alpha0 = 0.5 * FastMath.PI;
928 double omega = 0.5 * FastMath.PI;
929 Transform t = new Transform(date,
930 new Transform(date, Vector3D.MINUS_I, Vector3D.MINUS_J, Vector3D.ZERO),
931 new Transform(date,
932 new Rotation(Vector3D.PLUS_K, alpha0,
933 RotationConvention.VECTOR_OPERATOR),
934 new Vector3D(omega, Vector3D.MINUS_K)));
935
936 for (double dt = -10.0; dt < 10.0; dt += 0.125) {
937
938 Transform shifted = t.shiftedBy(dt);
939
940
941 PVCoordinates expectedFixedPoint =
942 shifted.transformPVCoordinates(new PVCoordinates(new Vector3D(1, dt, 0), Vector3D.PLUS_J, Vector3D.ZERO));
943 checkVector(expectedFixedPoint.getPosition(), Vector3D.ZERO, 1.0e-14);
944 checkVector(expectedFixedPoint.getVelocity(), Vector3D.ZERO, 1.0e-14);
945 checkVector(expectedFixedPoint.getAcceleration(), Vector3D.ZERO, 1.0e-14);
946
947
948 PVCoordinates expectedApparentMotion = shifted.transformPVCoordinates(PVCoordinates.ZERO);
949 double c = FastMath.cos(alpha0 + omega * dt);
950 double s = FastMath.sin(alpha0 + omega * dt);
951 Vector3D referencePosition = new Vector3D(-c + dt * s,
952 -s - dt * c,
953 0);
954 Vector3D referenceVelocity = new Vector3D( (1 + omega) * s + dt * omega * c,
955 -(1 + omega) * c + dt * omega * s,
956 0);
957 Vector3D referenceAcceleration = new Vector3D(omega * (2 + omega) * c - dt * omega * omega * s,
958 omega * (2 + omega) * s + dt * omega * omega * c,
959 0);
960 checkVector(expectedApparentMotion.getPosition(), referencePosition, 1.0e-14);
961 checkVector(expectedApparentMotion.getVelocity(), referenceVelocity, 1.0e-14);
962 checkVector(expectedApparentMotion.getAcceleration(), referenceAcceleration, 1.0e-14);
963
964 MatcherAssert.assertThat("At dt=" + dt,
965 t.staticShiftedBy(dt).transformPosition(Vector3D.ZERO),
966 OrekitMatchers.vectorCloseTo(expectedApparentMotion.getPosition(), 1e-16));
967 MatcherAssert.assertThat("At dt=" + dt,
968 t.staticShiftedBy(dt).transformPosition(Vector3D.ZERO),
969 OrekitMatchers.vectorCloseTo(
970 referencePosition,
971 1e-14 * (1 + referencePosition.getNorm())));
972 }
973
974 }
975
976 @Test
977 public void testShiftDerivatives() {
978
979 RandomGenerator random = new Well19937a(0x5acda4f605aadce7L);
980 for (int i = 0; i < 10; ++i) {
981 Transform t = randomTransform(random);
982
983 for (double dt = -10.0; dt < 10.0; dt += 0.125) {
984
985 Transform t0 = t.shiftedBy(dt);
986 double v = t0.getVelocity().getNorm();
987 double a = t0.getAcceleration().getNorm();
988 double omega = t0.getRotationRate().getNorm();
989 double omegaDot = t0.getRotationAcceleration().getNorm();
990
991
992 double h = 0.01 / omega;
993 Transform tm4h = t.shiftedBy(dt - 4 * h);
994 Transform tm3h = t.shiftedBy(dt - 3 * h);
995 Transform tm2h = t.shiftedBy(dt - 2 * h);
996 Transform tm1h = t.shiftedBy(dt - 1 * h);
997 Transform tp1h = t.shiftedBy(dt + 1 * h);
998 Transform tp2h = t.shiftedBy(dt + 2 * h);
999 Transform tp3h = t.shiftedBy(dt + 3 * h);
1000 Transform tp4h = t.shiftedBy(dt + 4 * h);
1001 double numXDot = derivative(h,
1002 tm4h.getTranslation().getX(), tm3h.getTranslation().getX(),
1003 tm2h.getTranslation().getX(), tm1h.getTranslation().getX(),
1004 tp1h.getTranslation().getX(), tp2h.getTranslation().getX(),
1005 tp3h.getTranslation().getX(), tp4h.getTranslation().getX());
1006 double numYDot = derivative(h,
1007 tm4h.getTranslation().getY(), tm3h.getTranslation().getY(),
1008 tm2h.getTranslation().getY(), tm1h.getTranslation().getY(),
1009 tp1h.getTranslation().getY(), tp2h.getTranslation().getY(),
1010 tp3h.getTranslation().getY(), tp4h.getTranslation().getY());
1011 double numZDot = derivative(h,
1012 tm4h.getTranslation().getZ(), tm3h.getTranslation().getZ(),
1013 tm2h.getTranslation().getZ(), tm1h.getTranslation().getZ(),
1014 tp1h.getTranslation().getZ(), tp2h.getTranslation().getZ(),
1015 tp3h.getTranslation().getZ(), tp4h.getTranslation().getZ());
1016 double numXDot2 = derivative(h,
1017 tm4h.getVelocity().getX(), tm3h.getVelocity().getX(),
1018 tm2h.getVelocity().getX(), tm1h.getVelocity().getX(),
1019 tp1h.getVelocity().getX(), tp2h.getVelocity().getX(),
1020 tp3h.getVelocity().getX(), tp4h.getVelocity().getX());
1021 double numYDot2 = derivative(h,
1022 tm4h.getVelocity().getY(), tm3h.getVelocity().getY(),
1023 tm2h.getVelocity().getY(), tm1h.getVelocity().getY(),
1024 tp1h.getVelocity().getY(), tp2h.getVelocity().getY(),
1025 tp3h.getVelocity().getY(), tp4h.getVelocity().getY());
1026 double numZDot2 = derivative(h,
1027 tm4h.getVelocity().getZ(), tm3h.getVelocity().getZ(),
1028 tm2h.getVelocity().getZ(), tm1h.getVelocity().getZ(),
1029 tp1h.getVelocity().getZ(), tp2h.getVelocity().getZ(),
1030 tp3h.getVelocity().getZ(), tp4h.getVelocity().getZ());
1031 double numQ0Dot = derivative(h,
1032 tm4h.getRotation().getQ0(), tm3h.getRotation().getQ0(),
1033 tm2h.getRotation().getQ0(), tm1h.getRotation().getQ0(),
1034 tp1h.getRotation().getQ0(), tp2h.getRotation().getQ0(),
1035 tp3h.getRotation().getQ0(), tp4h.getRotation().getQ0());
1036 double numQ1Dot = derivative(h,
1037 tm4h.getRotation().getQ1(), tm3h.getRotation().getQ1(),
1038 tm2h.getRotation().getQ1(), tm1h.getRotation().getQ1(),
1039 tp1h.getRotation().getQ1(), tp2h.getRotation().getQ1(),
1040 tp3h.getRotation().getQ1(), tp4h.getRotation().getQ1());
1041 double numQ2Dot = derivative(h,
1042 tm4h.getRotation().getQ2(), tm3h.getRotation().getQ2(),
1043 tm2h.getRotation().getQ2(), tm1h.getRotation().getQ2(),
1044 tp1h.getRotation().getQ2(), tp2h.getRotation().getQ2(),
1045 tp3h.getRotation().getQ2(), tp4h.getRotation().getQ2());
1046 double numQ3Dot = derivative(h,
1047 tm4h.getRotation().getQ3(), tm3h.getRotation().getQ3(),
1048 tm2h.getRotation().getQ3(), tm1h.getRotation().getQ3(),
1049 tp1h.getRotation().getQ3(), tp2h.getRotation().getQ3(),
1050 tp3h.getRotation().getQ3(), tp4h.getRotation().getQ3());
1051 double numOxDot = derivative(h,
1052 tm4h.getRotationRate().getX(), tm3h.getRotationRate().getX(),
1053 tm2h.getRotationRate().getX(), tm1h.getRotationRate().getX(),
1054 tp1h.getRotationRate().getX(), tp2h.getRotationRate().getX(),
1055 tp3h.getRotationRate().getX(), tp4h.getRotationRate().getX());
1056 double numOyDot = derivative(h,
1057 tm4h.getRotationRate().getY(), tm3h.getRotationRate().getY(),
1058 tm2h.getRotationRate().getY(), tm1h.getRotationRate().getY(),
1059 tp1h.getRotationRate().getY(), tp2h.getRotationRate().getY(),
1060 tp3h.getRotationRate().getY(), tp4h.getRotationRate().getY());
1061 double numOzDot = derivative(h,
1062 tm4h.getRotationRate().getZ(), tm3h.getRotationRate().getZ(),
1063 tm2h.getRotationRate().getZ(), tm1h.getRotationRate().getZ(),
1064 tp1h.getRotationRate().getZ(), tp2h.getRotationRate().getZ(),
1065 tp3h.getRotationRate().getZ(), tp4h.getRotationRate().getZ());
1066
1067
1068 double theXDot = t0.getVelocity().getX();
1069 double theYDot = t0.getVelocity().getY();
1070 double theZDot = t0.getVelocity().getZ();
1071 double theXDot2 = t0.getAcceleration().getX();
1072 double theYDot2 = t0.getAcceleration().getY();
1073 double theZDot2 = t0.getAcceleration().getZ();
1074 Rotation r0 = t0.getRotation();
1075 Vector3D w = t0.getRotationRate();
1076 Vector3D q = new Vector3D(r0.getQ1(), r0.getQ2(), r0.getQ3());
1077 Vector3D qw = Vector3D.crossProduct(q, w);
1078 double theQ0Dot = -0.5 * Vector3D.dotProduct(q, w);
1079 double theQ1Dot = 0.5 * (r0.getQ0() * w.getX() + qw.getX());
1080 double theQ2Dot = 0.5 * (r0.getQ0() * w.getY() + qw.getY());
1081 double theQ3Dot = 0.5 * (r0.getQ0() * w.getZ() + qw.getZ());
1082 double theOxDot2 = t0.getRotationAcceleration().getX();
1083 double theOyDot2 = t0.getRotationAcceleration().getY();
1084 double theOzDot2 = t0.getRotationAcceleration().getZ();
1085
1086
1087 Assertions.assertEquals(theXDot, numXDot, 1.0e-13 * v);
1088 Assertions.assertEquals(theYDot, numYDot, 1.0e-13 * v);
1089 Assertions.assertEquals(theZDot, numZDot, 1.0e-13 * v);
1090
1091 Assertions.assertEquals(theXDot2, numXDot2, 1.0e-13 * a);
1092 Assertions.assertEquals(theYDot2, numYDot2, 1.0e-13 * a);
1093 Assertions.assertEquals(theZDot2, numZDot2, 1.0e-13 * a);
1094
1095 Assertions.assertEquals(theQ0Dot, numQ0Dot, 1.0e-13 * omega);
1096 Assertions.assertEquals(theQ1Dot, numQ1Dot, 1.0e-13 * omega);
1097 Assertions.assertEquals(theQ2Dot, numQ2Dot, 1.0e-13 * omega);
1098 Assertions.assertEquals(theQ3Dot, numQ3Dot, 1.0e-13 * omega);
1099
1100
1101 Assertions.assertEquals(theOxDot2, numOxDot, 1.0e-12 * omegaDot);
1102 Assertions.assertEquals(theOyDot2, numOyDot, 1.0e-12 * omegaDot);
1103 Assertions.assertEquals(theOzDot2, numOzDot, 1.0e-12 * omegaDot);
1104
1105 }
1106 }
1107 }
1108
1109 @Test
1110 void testToStaticTransform() {
1111
1112 final PVCoordinates pvCoordinates = new PVCoordinates();
1113 final AngularCoordinates angularCoordinates = new AngularCoordinates();
1114 final Transform transform = new Transform(AbsoluteDate.ARBITRARY_EPOCH, pvCoordinates, angularCoordinates);
1115
1116 final StaticTransform actualStaticTransform = transform.toStaticTransform();
1117
1118 final StaticTransform expectedStaticTransform = transform.staticShiftedBy(0.);
1119 Assertions.assertEquals(expectedStaticTransform.getDate(), actualStaticTransform.getDate());
1120 Assertions.assertEquals(expectedStaticTransform.getTranslation(), actualStaticTransform.getTranslation());
1121 Assertions.assertEquals(0., Rotation.distance(expectedStaticTransform.getRotation(),
1122 actualStaticTransform.getRotation()));
1123 }
1124
1125 @Test
1126 public void testInterpolation() {
1127
1128 AbsoluteDate t0 = AbsoluteDate.GALILEO_EPOCH;
1129 List<Transform> sample = new ArrayList<>();
1130 for (int i = 0; i < 5; ++i) {
1131 sample.add(evolvingTransform(t0, TimeOffset.MILLISECOND.multiply(800 * i)));
1132 }
1133
1134 for (TimeOffset dt = TimeOffset.MILLISECOND.multiply(100);
1135 dt.compareTo(TimeOffset.MILLISECOND.multiply(3100)) < 0;
1136 dt = dt.add(TimeOffset.MILLISECOND.multiply(100))) {
1137 Transform reference = evolvingTransform(t0, dt);
1138 Transform interpolated = sample.get(0).interpolate(reference.getDate(), sample.stream());
1139 Transform error = new Transform(reference.getDate(), reference, interpolated.getInverse());
1140 Assertions.assertEquals(0.0, error.getCartesian().getPosition().getNorm(), 2.0e-15);
1141 Assertions.assertEquals(0.0, error.getCartesian().getVelocity().getNorm(), 6.0e-15);
1142 Assertions.assertEquals(0.0, error.getCartesian().getAcceleration().getNorm(), 4.0e-14);
1143 Assertions.assertEquals(0.0, error.getAngular().getRotation().getAngle(), 2.0e-15);
1144 Assertions.assertEquals(0.0, error.getAngular().getRotationRate().getNorm(), 6.0e-15);
1145 Assertions.assertEquals(0.0, error.getAngular().getRotationAcceleration().getNorm(), 4.0e-14);
1146
1147 }
1148
1149 }
1150
1151 private Transform evolvingTransform(final AbsoluteDate t0, final TimeOffset dt) {
1152
1153
1154 final double omega = 0.2;
1155 final AbsoluteDate date = t0.shiftedBy(dt);
1156 final SinCos sc = FastMath.sinCos(omega * dt.toDouble());
1157 return new Transform(date,
1158 new Transform(date,
1159 new Vector3D(-sc.cos(), -sc.sin(), 0),
1160 new Vector3D(omega * sc.sin(), -omega * sc.cos(), 0),
1161 new Vector3D(omega * omega * sc.cos(), omega * omega * sc.sin(), 0)),
1162 new Transform(date,
1163 new Rotation(Vector3D.PLUS_K, FastMath.PI - omega * dt.toDouble(),
1164 RotationConvention.VECTOR_OPERATOR),
1165 new Vector3D(omega, Vector3D.PLUS_K)));
1166 }
1167
1168 private double derivative(double h,
1169 double ym4h, double ym3h, double ym2h, double ym1h,
1170 double yp1h, double yp2h, double yp3h, double yp4h) {
1171 return (-3 * (yp4h - ym4h) + 32 * (yp3h - ym3h) - 168 * (yp2h - ym2h) + 672 * (yp1h - ym1h)) /
1172 (840 * h);
1173 }
1174
1175 private Transform randomTransform(RandomGenerator random) {
1176
1177 Transform combined = Transform.IDENTITY;
1178 for (int k = 0; k < 20; ++k) {
1179 Transform t = random.nextBoolean() ?
1180 new Transform(AbsoluteDate.J2000_EPOCH, randomVector(1.0e3, random), randomVector(1.0, random), randomVector(1.0e-3, random)) :
1181 new Transform(AbsoluteDate.J2000_EPOCH, randomRotation(random), randomVector(0.01, random), randomVector(1.0e-4, random));
1182 combined = new Transform(AbsoluteDate.J2000_EPOCH, combined, t);
1183 }
1184 return combined;
1185 }
1186
1187 private Vector3D randomVector(double scale, RandomGenerator random) {
1188 return new Vector3D(random.nextDouble() * scale,
1189 random.nextDouble() * scale,
1190 random.nextDouble() * scale);
1191 }
1192
1193 private Rotation randomRotation(RandomGenerator random) {
1194 double q0 = random.nextDouble() * 2 - 1;
1195 double q1 = random.nextDouble() * 2 - 1;
1196 double q2 = random.nextDouble() * 2 - 1;
1197 double q3 = random.nextDouble() * 2 - 1;
1198 double q = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
1199 return new Rotation(q0 / q, q1 / q, q2 / q, q3 / q, false);
1200 }
1201
1202 private void checkNoTransform(Transform transform, RandomGenerator random) {
1203 for (int i = 0; i < 100; ++i) {
1204 Vector3D a = randomVector(1.0e3, random);
1205 Vector3D tA = transform.transformVector(a);
1206 Assertions.assertEquals(0, a.subtract(tA).getNorm(), 1.0e-10 * a.getNorm());
1207 Vector3D b = randomVector(1.0e3, random);
1208 Vector3D tB = transform.transformPosition(b);
1209 Assertions.assertEquals(0, b.subtract(tB).getNorm(), 1.0e-10 * b.getNorm());
1210 PVCoordinates pv = new PVCoordinates(randomVector(1.0e3, random), randomVector(1.0, random), randomVector(1.0e-3, random));
1211 PVCoordinates tPv = transform.transformPVCoordinates(pv);
1212 checkVector(pv.getPosition(), tPv.getPosition(), 1.0e-10);
1213 checkVector(pv.getVelocity(), tPv.getVelocity(), 3.0e-9);
1214 checkVector(pv.getAcceleration(), tPv.getAcceleration(), 3.0e-9);
1215 }
1216 }
1217
1218 private void checkVector(Vector3D reference, Vector3D result, double relativeTolerance) {
1219 double refNorm = reference.getNorm();
1220 double resNorm = result.getNorm();
1221 double tolerance = relativeTolerance * (1 + FastMath.max(refNorm, resNorm));
1222 Assertions.assertEquals(0, Vector3D.distance(reference, result), tolerance, "ref = " + reference + ", res = " + result + " -> " +
1223 (Vector3D.distance(reference, result) / (1 + FastMath.max(refNorm, resNorm))));
1224 }
1225
1226 }