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