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