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