1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.utils;
18
19
20 import java.lang.reflect.InvocationTargetException;
21 import java.lang.reflect.Method;
22
23 import org.hipparchus.analysis.differentiation.DerivativeStructure;
24 import org.hipparchus.analysis.differentiation.UnivariateDerivative1;
25 import org.hipparchus.exception.MathIllegalArgumentException;
26 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
27 import org.hipparchus.geometry.euclidean.threed.Rotation;
28 import org.hipparchus.geometry.euclidean.threed.RotationConvention;
29 import org.hipparchus.geometry.euclidean.threed.Vector3D;
30 import org.hipparchus.ode.ODEIntegrator;
31 import org.hipparchus.ode.ODEState;
32 import org.hipparchus.ode.ODEStateAndDerivative;
33 import org.hipparchus.ode.OrdinaryDifferentialEquation;
34 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
35 import org.hipparchus.ode.sampling.ODEFixedStepHandler;
36 import org.hipparchus.ode.sampling.StepNormalizer;
37 import org.hipparchus.random.RandomGenerator;
38 import org.hipparchus.random.Well1024a;
39 import org.hipparchus.util.FastMath;
40 import org.hipparchus.util.MathArrays;
41 import org.hipparchus.util.Precision;
42 import org.junit.Assert;
43 import org.junit.Test;
44 import org.orekit.errors.OrekitException;
45 import org.orekit.errors.OrekitMessages;
46
47 public class AngularCoordinatesTest {
48
49 @Test
50 public void testDefaultConstructor() {
51 AngularCoordinates ac = new AngularCoordinates();
52 Assert.assertEquals(0.0, ac.getRotationAcceleration().getNorm(), 1.0e-15);
53 Assert.assertEquals(0.0, ac.getRotationRate().getNorm(), 1.0e-15);
54 Assert.assertEquals(0.0, Rotation.distance(ac.getRotation(), Rotation.IDENTITY), 1.0e-10);
55 }
56
57 @Test
58 public void testDerivativesStructuresNeg() {
59 try {
60 AngularCoordinates.IDENTITY.toDerivativeStructureRotation(-1);
61 Assert.fail("an exception should have been thrown");
62 } catch (OrekitException oe) {
63 Assert.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
64 Assert.assertEquals(-1, ((Integer) (oe.getParts()[0])).intValue());
65 }
66
67 }
68
69 @Test
70 public void testDerivativesStructures3() {
71 try {
72 AngularCoordinates.IDENTITY.toDerivativeStructureRotation(3);
73 Assert.fail("an exception should have been thrown");
74 } catch (OrekitException oe) {
75 Assert.assertEquals(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, oe.getSpecifier());
76 Assert.assertEquals(3, ((Integer) (oe.getParts()[0])).intValue());
77 }
78
79 }
80
81 @Test
82 public void testDerivativesStructures0() {
83 RandomGenerator random = new Well1024a(0x18a0a08fd63f047al);
84
85 Rotation r = randomRotation(random);
86 Vector3D o = randomVector(random, 1.0e-2);
87 Vector3D oDot = randomVector(random, 1.0e-2);
88 AngularCoordinates ac = new AngularCoordinates(r, o, oDot);
89 AngularCoordinates rebuilt = new AngularCoordinates(ac.toDerivativeStructureRotation(0));
90 Assert.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
91 Assert.assertEquals(0.0, rebuilt.getRotationRate().getNorm(), 1.0e-15);
92 Assert.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm(), 1.0e-15);
93 }
94
95 @Test
96 public void testDerivativesStructures1() {
97 RandomGenerator random = new Well1024a(0x8f8fc6d27bbdc46dl);
98
99 Rotation r = randomRotation(random);
100 Vector3D o = randomVector(random, 1.0e-2);
101 Vector3D oDot = randomVector(random, 1.0e-2);
102 AngularCoordinates ac = new AngularCoordinates(r, o, oDot);
103 AngularCoordinates rebuilt = new AngularCoordinates(ac.toDerivativeStructureRotation(1));
104 Assert.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
105 Assert.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
106 Assert.assertEquals(0.0, rebuilt.getRotationAcceleration().getNorm(), 1.0e-15);
107 }
108
109 @Test
110 public void testDerivativesStructures2() {
111 RandomGenerator random = new Well1024a(0x1633878dddac047dl);
112
113 Rotation r = randomRotation(random);
114 Vector3D o = randomVector(random, 1.0e-2);
115 Vector3D oDot = randomVector(random, 1.0e-2);
116 AngularCoordinates ac = new AngularCoordinates(r, o, oDot);
117 AngularCoordinates rebuilt = new AngularCoordinates(ac.toDerivativeStructureRotation(2));
118 Assert.assertEquals(0.0, Rotation.distance(ac.getRotation(), rebuilt.getRotation()), 1.0e-15);
119 Assert.assertEquals(0.0, Vector3D.distance(ac.getRotationRate(), rebuilt.getRotationRate()), 1.0e-15);
120 Assert.assertEquals(0.0, Vector3D.distance(ac.getRotationAcceleration(), rebuilt.getRotationAcceleration()), 1.0e-15);
121 }
122
123 @Test
124 public void testUnivariateDerivative1() {
125 RandomGenerator random = new Well1024a(0x1633878dddac047dl);
126
127 Rotation r = randomRotation(random);
128 Vector3D o = randomVector(random, 1.0e-2);
129 Vector3D oDot = randomVector(random, 1.0e-2);
130 AngularCoordinates ac = new AngularCoordinates(r, o, oDot);
131 FieldRotation<UnivariateDerivative1> rotationUD = ac.toUnivariateDerivative1Rotation();
132 FieldRotation<DerivativeStructure> rotationDS = ac.toDerivativeStructureRotation(1);
133 Assert.assertEquals(rotationDS.getQ0().getReal(), rotationUD.getQ0().getReal(), 1.0e-15);
134 Assert.assertEquals(rotationDS.getQ1().getReal(), rotationUD.getQ1().getReal(), 1.0e-15);
135 Assert.assertEquals(rotationDS.getQ2().getReal(), rotationUD.getQ2().getReal(), 1.0e-15);
136 Assert.assertEquals(rotationDS.getQ3().getReal(), rotationUD.getQ3().getReal(), 1.0e-15);
137 Assert.assertEquals(rotationDS.getQ0().getPartialDerivative(1), rotationUD.getQ0().getFirstDerivative(), 1.0e-15);
138 Assert.assertEquals(rotationDS.getQ1().getPartialDerivative(1), rotationUD.getQ1().getFirstDerivative(), 1.0e-15);
139 Assert.assertEquals(rotationDS.getQ2().getPartialDerivative(1), rotationUD.getQ2().getFirstDerivative(), 1.0e-15);
140 Assert.assertEquals(rotationDS.getQ3().getPartialDerivative(1), rotationUD.getQ3().getFirstDerivative(), 1.0e-15);
141 }
142
143 @Test
144 public void testZeroRate() {
145 AngularCoordinates ac =
146 new AngularCoordinates(new Rotation(0.48, 0.64, 0.36, 0.48, false), Vector3D.ZERO, Vector3D.ZERO);
147 Assert.assertEquals(Vector3D.ZERO, ac.getRotationRate());
148 double dt = 10.0;
149 AngularCoordinates shifted = ac.shiftedBy(dt);
150 Assert.assertEquals(Vector3D.ZERO, shifted.getRotationAcceleration());
151 Assert.assertEquals(Vector3D.ZERO, shifted.getRotationRate());
152 Assert.assertEquals(0.0, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-15);
153 }
154
155 @Test
156 public void testShiftWithoutAcceleration() {
157 double rate = 2 * FastMath.PI / (12 * 60);
158 AngularCoordinates ac =
159 new AngularCoordinates(Rotation.IDENTITY,
160 new Vector3D(rate, Vector3D.PLUS_K),
161 Vector3D.ZERO);
162 Assert.assertEquals(rate, ac.getRotationRate().getNorm(), 1.0e-10);
163 double dt = 10.0;
164 double alpha = rate * dt;
165 AngularCoordinates shifted = ac.shiftedBy(dt);
166 Assert.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10);
167 Assert.assertEquals(alpha, Rotation.distance(ac.getRotation(), shifted.getRotation()), 1.0e-15);
168
169 Vector3D xSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
170 Assert.assertEquals(0.0, xSat.subtract(new Vector3D(FastMath.cos(alpha), FastMath.sin(alpha), 0)).getNorm(), 1.0e-15);
171 Vector3D ySat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
172 Assert.assertEquals(0.0, ySat.subtract(new Vector3D(-FastMath.sin(alpha), FastMath.cos(alpha), 0)).getNorm(), 1.0e-15);
173 Vector3D zSat = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
174 Assert.assertEquals(0.0, zSat.subtract(Vector3D.PLUS_K).getNorm(), 1.0e-15);
175
176 }
177
178 @Test
179 public void testShiftWithAcceleration() {
180 double rate = 2 * FastMath.PI / (12 * 60);
181 double acc = 0.001;
182 double dt = 1.0;
183 int n = 2000;
184 final AngularCoordinates quadratic =
185 new AngularCoordinates(Rotation.IDENTITY,
186 new Vector3D(rate, Vector3D.PLUS_K),
187 new Vector3D(acc, Vector3D.PLUS_J));
188 final AngularCoordinates linear =
189 new AngularCoordinates(quadratic.getRotation(), quadratic.getRotationRate(), Vector3D.ZERO);
190
191 final OrdinaryDifferentialEquation ode = new OrdinaryDifferentialEquation() {
192 public int getDimension() {
193 return 4;
194 }
195 public double[] computeDerivatives(final double t, final double[] q) {
196 final double omegaX = quadratic.getRotationRate().getX() + t * quadratic.getRotationAcceleration().getX();
197 final double omegaY = quadratic.getRotationRate().getY() + t * quadratic.getRotationAcceleration().getY();
198 final double omegaZ = quadratic.getRotationRate().getZ() + t * quadratic.getRotationAcceleration().getZ();
199 return new double[] {
200 0.5 * MathArrays.linearCombination(-q[1], omegaX, -q[2], omegaY, -q[3], omegaZ),
201 0.5 * MathArrays.linearCombination( q[0], omegaX, -q[3], omegaY, q[2], omegaZ),
202 0.5 * MathArrays.linearCombination( q[3], omegaX, q[0], omegaY, -q[1], omegaZ),
203 0.5 * MathArrays.linearCombination(-q[2], omegaX, q[1], omegaY, q[0], omegaZ)
204 };
205 }
206 };
207 ODEIntegrator integrator = new DormandPrince853Integrator(1.0e-6, 1.0, 1.0e-12, 1.0e-12);
208 integrator.addStepHandler(new StepNormalizer(dt / n, new ODEFixedStepHandler() {
209 public void handleStep(ODEStateAndDerivative s, boolean isLast) {
210 final double t = s.getTime();
211 final double[] y = s.getPrimaryState();
212 Rotation reference = new Rotation(y[0], y[1], y[2], y[3], true);
213
214
215 double expectedCubicError = 1.4544e-6 * t * t * t;
216 Assert.assertEquals(expectedCubicError,
217 Rotation.distance(reference, quadratic.shiftedBy(t).getRotation()),
218 0.0001 * expectedCubicError);
219
220
221 double expectedQuadraticError = 5.0e-4 * t * t;
222 Assert.assertEquals(expectedQuadraticError,
223 Rotation.distance(reference, linear.shiftedBy(t).getRotation()),
224 0.00001 * expectedQuadraticError);
225
226 }
227 }));
228
229 double[] y = new double[] {
230 quadratic.getRotation().getQ0(),
231 quadratic.getRotation().getQ1(),
232 quadratic.getRotation().getQ2(),
233 quadratic.getRotation().getQ3()
234 };
235 integrator.integrate(ode, new ODEState(0, y), dt);
236
237 }
238
239 @Test
240 public void testSpin() {
241 double rate = 2 * FastMath.PI / (12 * 60);
242 AngularCoordinates angularCoordinates =
243 new AngularCoordinates(new Rotation(0.48, 0.64, 0.36, 0.48, false),
244 new Vector3D(rate, Vector3D.PLUS_K));
245 Assert.assertEquals(rate, angularCoordinates.getRotationRate().getNorm(), 1.0e-10);
246 double dt = 10.0;
247 AngularCoordinates shifted = angularCoordinates.shiftedBy(dt);
248 Assert.assertEquals(rate, shifted.getRotationRate().getNorm(), 1.0e-10);
249 Assert.assertEquals(rate * dt, Rotation.distance(angularCoordinates.getRotation(), shifted.getRotation()), 1.0e-10);
250
251 Vector3D shiftedX = shifted.getRotation().applyInverseTo(Vector3D.PLUS_I);
252 Vector3D shiftedY = shifted.getRotation().applyInverseTo(Vector3D.PLUS_J);
253 Vector3D shiftedZ = shifted.getRotation().applyInverseTo(Vector3D.PLUS_K);
254 Vector3D originalX = angularCoordinates.getRotation().applyInverseTo(Vector3D.PLUS_I);
255 Vector3D originalY = angularCoordinates.getRotation().applyInverseTo(Vector3D.PLUS_J);
256 Vector3D originalZ = angularCoordinates.getRotation().applyInverseTo(Vector3D.PLUS_K);
257 Assert.assertEquals( FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedX, originalX), 1.0e-10);
258 Assert.assertEquals( FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedX, originalY), 1.0e-10);
259 Assert.assertEquals( 0.0, Vector3D.dotProduct(shiftedX, originalZ), 1.0e-10);
260 Assert.assertEquals(-FastMath.sin(rate * dt), Vector3D.dotProduct(shiftedY, originalX), 1.0e-10);
261 Assert.assertEquals( FastMath.cos(rate * dt), Vector3D.dotProduct(shiftedY, originalY), 1.0e-10);
262 Assert.assertEquals( 0.0, Vector3D.dotProduct(shiftedY, originalZ), 1.0e-10);
263 Assert.assertEquals( 0.0, Vector3D.dotProduct(shiftedZ, originalX), 1.0e-10);
264 Assert.assertEquals( 0.0, Vector3D.dotProduct(shiftedZ, originalY), 1.0e-10);
265 Assert.assertEquals( 1.0, Vector3D.dotProduct(shiftedZ, originalZ), 1.0e-10);
266
267 Vector3D forward = AngularCoordinates.estimateRate(angularCoordinates.getRotation(), shifted.getRotation(), dt);
268 Assert.assertEquals(0.0, forward.subtract(angularCoordinates.getRotationRate()).getNorm(), 1.0e-10);
269
270 Vector3D reversed = AngularCoordinates.estimateRate(shifted.getRotation(), angularCoordinates.getRotation(), dt);
271 Assert.assertEquals(0.0, reversed.add(angularCoordinates.getRotationRate()).getNorm(), 1.0e-10);
272
273 }
274
275 @Test
276 public void testReverseOffset() {
277 RandomGenerator random = new Well1024a(0x4ecca9d57a8f1611l);
278 for (int i = 0; i < 100; ++i) {
279 Rotation r = randomRotation(random);
280 Vector3D o = randomVector(random, 1.0e-3);
281 AngularCoordinates ac = new AngularCoordinates(r, o);
282 AngularCoordinates sum = ac.addOffset(ac.revert());
283 Assert.assertEquals(0.0, sum.getRotation().getAngle(), 1.0e-15);
284 Assert.assertEquals(0.0, sum.getRotationRate().getNorm(), 1.0e-15);
285 Assert.assertEquals(0.0, sum.getRotationAcceleration().getNorm(), 1.0e-15);
286 }
287 }
288
289 @Test
290 public void testNoCommute() {
291 AngularCoordinates ac1 =
292 new AngularCoordinates(new Rotation(0.48, 0.64, 0.36, 0.48, false), Vector3D.ZERO);
293 AngularCoordinates ac2 =
294 new AngularCoordinates(new Rotation(0.36, -0.48, 0.48, 0.64, false), Vector3D.ZERO);
295
296 AngularCoordinates add12 = ac1.addOffset(ac2);
297 AngularCoordinates add21 = ac2.addOffset(ac1);
298
299
300 Assert.assertEquals(2.574, Rotation.distance(add12.getRotation(), add21.getRotation()), 1.0e-3);
301
302 }
303
304 @Test
305 public void testRoundTripNoOp() {
306 RandomGenerator random = new Well1024a(0x1e610cfe89306669l);
307 for (int i = 0; i < 100; ++i) {
308
309 Rotation r1 = randomRotation(random);
310 Vector3D o1 = randomVector(random, 1.0e-2);
311 Vector3D oDot1 = randomVector(random, 1.0e-2);
312 AngularCoordinates ac1 = new AngularCoordinates(r1, o1, oDot1);
313
314 Rotation r2 = randomRotation(random);
315 Vector3D o2 = randomVector(random, 1.0e-2);
316 Vector3D oDot2 = randomVector(random, 1.0e-2);
317 AngularCoordinates ac2 = new AngularCoordinates(r2, o2, oDot2);
318
319 AngularCoordinates roundTripSA = ac1.subtractOffset(ac2).addOffset(ac2);
320 Assert.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripSA.getRotation()), 5.0e-16);
321 Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripSA.getRotationRate()), 2.0e-17);
322 Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripSA.getRotationAcceleration()), 2.0e-17);
323
324 AngularCoordinates roundTripAS = ac1.addOffset(ac2).subtractOffset(ac2);
325 Assert.assertEquals(0.0, Rotation.distance(ac1.getRotation(), roundTripAS.getRotation()), 5.0e-16);
326 Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationRate(), roundTripAS.getRotationRate()), 2.0e-17);
327 Assert.assertEquals(0.0, Vector3D.distance(ac1.getRotationAcceleration(), roundTripAS.getRotationAcceleration()), 2.0e-17);
328
329 }
330 }
331
332 @Test
333 public void testRodriguesSymmetry() {
334
335
336 RandomGenerator random = new Well1024a(0xb1e615aaa8236b52l);
337 for (int i = 0; i < 1000; ++i) {
338 Rotation rotation = randomRotation(random);
339 Vector3D rotationRate = randomVector(random, 0.01);
340 Vector3D rotationAcceleration = randomVector(random, 0.01);
341 AngularCoordinates ac = new AngularCoordinates(rotation, rotationRate, rotationAcceleration);
342 AngularCoordinates rebuilt = AngularCoordinates.createFromModifiedRodrigues(ac.getModifiedRodrigues(1.0));
343 Assert.assertEquals(0.0, Rotation.distance(rotation, rebuilt.getRotation()), 1.0e-14);
344 Assert.assertEquals(0.0, Vector3D.distance(rotationRate, rebuilt.getRotationRate()), 1.0e-15);
345 Assert.assertEquals(0.0, Vector3D.distance(rotationAcceleration, rebuilt.getRotationAcceleration()), 1.0e-15);
346 }
347
348 }
349
350 @Test
351 public void testRodriguesSpecialCases() {
352
353
354 double[][] identity = new AngularCoordinates(Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO).getModifiedRodrigues(1.0);
355 for (double[] row : identity) {
356 for (double element : row) {
357 Assert.assertEquals(0.0, element, Precision.SAFE_MIN);
358 }
359 }
360 AngularCoordinates acId = AngularCoordinates.createFromModifiedRodrigues(identity);
361 Assert.assertEquals(0.0, acId.getRotation().getAngle(), Precision.SAFE_MIN);
362 Assert.assertEquals(0.0, acId.getRotationRate().getNorm(), Precision.SAFE_MIN);
363
364
365 RandomGenerator random = new Well1024a(0x2158523e6accb859l);
366 for (int i = 0; i < 100; ++i) {
367 Vector3D axis = randomVector(random, 1.0);
368 AngularCoordinates original = new AngularCoordinates(new Rotation(axis, FastMath.PI, RotationConvention.VECTOR_OPERATOR),
369 Vector3D.ZERO, Vector3D.ZERO);
370 AngularCoordinates rebuilt = AngularCoordinates.createFromModifiedRodrigues(original.getModifiedRodrigues(1.0));
371 Assert.assertEquals(FastMath.PI, rebuilt.getRotation().getAngle(), 1.0e-15);
372 Assert.assertEquals(0.0, FastMath.sin(Vector3D.angle(axis, rebuilt.getRotation().getAxis(RotationConvention.VECTOR_OPERATOR))), 1.0e-15);
373 Assert.assertEquals(0.0, rebuilt.getRotationRate().getNorm(), 1.0e-16);
374 }
375
376 }
377
378 @Test
379 public void testInverseCrossProducts()
380 {
381 checkInverse(Vector3D.PLUS_K, Vector3D.PLUS_I, Vector3D.PLUS_J);
382 checkInverse(Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO);
383 checkInverse(Vector3D.ZERO, Vector3D.ZERO, Vector3D.PLUS_J);
384 checkInverse(Vector3D.PLUS_K, Vector3D.PLUS_K, Vector3D.PLUS_J);
385 checkInverse(Vector3D.ZERO, Vector3D.PLUS_K, Vector3D.ZERO);
386 checkInverse(Vector3D.PLUS_K, Vector3D.PLUS_I, Vector3D.PLUS_K);
387 checkInverse(Vector3D.PLUS_K, Vector3D.PLUS_I, Vector3D.PLUS_I);
388 checkInverse(Vector3D.PLUS_K, Vector3D.PLUS_I, new Vector3D(1, 0, -1).normalize());
389 checkInverse(Vector3D.ZERO, Vector3D.PLUS_I, Vector3D.ZERO, Vector3D.PLUS_J, Vector3D.ZERO);
390 }
391
392 @Test
393 public void testInverseCrossProductsFailures() {
394 checkInverseFailure(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.PLUS_J, Vector3D.PLUS_I, Vector3D.PLUS_K);
395 checkInverseFailure(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO, Vector3D.PLUS_K);
396 checkInverseFailure(Vector3D.PLUS_I, Vector3D.PLUS_I, Vector3D.ZERO, Vector3D.MINUS_I, Vector3D.PLUS_K);
397 checkInverseFailure(Vector3D.PLUS_I, Vector3D.PLUS_I, Vector3D.ZERO, Vector3D.PLUS_J, Vector3D.PLUS_J);
398 checkInverseFailure(Vector3D.PLUS_I, Vector3D.PLUS_I, Vector3D.PLUS_J, Vector3D.PLUS_J, Vector3D.ZERO);
399 checkInverseFailure(Vector3D.PLUS_I, Vector3D.PLUS_I, Vector3D.PLUS_J, Vector3D.ZERO, Vector3D.PLUS_J);
400 }
401
402 @Test
403 public void testRandomInverseCrossProducts() {
404 RandomGenerator generator = new Well1024a(0x52b29d8f6ac2d64bl);
405 for (int i = 0; i < 10000; ++i) {
406 Vector3D omega = randomVector(generator, 10 * generator.nextDouble() + 1.0);
407 Vector3D v1 = randomVector(generator, 10 * generator.nextDouble() + 1.0);
408 Vector3D v2 = randomVector(generator, 10 * generator.nextDouble() + 1.0);
409 checkInverse(omega, v1, v2);
410 }
411 }
412
413 private void checkInverse(Vector3D omega, Vector3D v1, Vector3D v2) {
414 checkInverse(omega,
415 v1, Vector3D.crossProduct(omega, v1),
416 v2, Vector3D.crossProduct(omega, v2));
417 }
418
419 private void checkInverseFailure(Vector3D omega, Vector3D v1, Vector3D c1, Vector3D v2, Vector3D c2) {
420 try {
421 checkInverse(omega, v1, c1, v2, c2);
422 Assert.fail("an exception should have been thrown");
423 } catch (MathIllegalArgumentException miae) {
424
425 }
426 }
427
428 private void checkInverse(Vector3D omega, Vector3D v1, Vector3D c1, Vector3D v2, Vector3D c2)
429 throws MathIllegalArgumentException {
430 try {
431 Method inverse;
432 inverse = AngularCoordinates.class.getDeclaredMethod("inverseCrossProducts",
433 Vector3D.class, Vector3D.class,
434 Vector3D.class, Vector3D.class,
435 double.class);
436 inverse.setAccessible(true);
437 Vector3D rebuilt = (Vector3D) inverse.invoke(null, v1, c1, v2, c2, 1.0e-9);
438 Assert.assertEquals(0.0, Vector3D.distance(omega, rebuilt), 5.0e-12 * omega.getNorm());
439 } catch (NoSuchMethodException e) {
440 Assert.fail(e.getLocalizedMessage());
441 } catch (SecurityException e) {
442 Assert.fail(e.getLocalizedMessage());
443 } catch (IllegalAccessException e) {
444 Assert.fail(e.getLocalizedMessage());
445 } catch (IllegalArgumentException e) {
446 Assert.fail(e.getLocalizedMessage());
447 } catch (InvocationTargetException e) {
448 throw (MathIllegalArgumentException) e.getCause();
449 }
450 }
451
452 @Test
453 public void testRandomPVCoordinates() {
454 RandomGenerator generator = new Well1024a(0x49eb5b92d1f94b89l);
455 for (int i = 0; i < 100; ++i) {
456 Rotation r = randomRotation(generator);
457 Vector3D omega = randomVector(generator, 10 * generator.nextDouble() + 1.0);
458 Vector3D omegaDot = randomVector(generator, 0.1 * generator.nextDouble() + 0.01);
459 AngularCoordinates ref = new AngularCoordinates(r, omega, omegaDot);
460 AngularCoordinates inv = ref.revert();
461 for (int j = 0; j < 100; ++j) {
462 PVCoordinates v1 = randomPVCoordinates(generator, 1000, 1.0, 0.001);
463 PVCoordinates v2 = randomPVCoordinates(generator, 1000, 1.0, 0.0010);
464 PVCoordinates u1 = inv.applyTo(v1);
465 PVCoordinates u2 = inv.applyTo(v2);
466 AngularCoordinates rebuilt = new AngularCoordinates(u1, u2, v1, v2, 1.0e-9);
467 Assert.assertEquals(0.0,
468 Rotation.distance(r, rebuilt.getRotation()),
469 4.0e-14);
470 Assert.assertEquals(0.0,
471 Vector3D.distance(omega, rebuilt.getRotationRate()),
472 3.0e-12 * omega.getNorm());
473 Assert.assertEquals(0.0,
474 Vector3D.distance(omegaDot, rebuilt.getRotationAcceleration()),
475 2.0e-6 * omegaDot.getNorm());
476 }
477 }
478 }
479
480 @Test
481 public void testCancellingDerivatives() {
482 PVCoordinates u1 = new PVCoordinates(new Vector3D(-0.4466591282528639, -0.009657376949231283, -0.894652087807798),
483 new Vector3D(-8.897296517803556E-4, 2.7825250920407674E-4, 4.411979658413134E-4),
484 new Vector3D( 4.753127475302486E-7, 1.0209400376727623E-8, 9.515403756524403E-7));
485 PVCoordinates u2 = new PVCoordinates(new Vector3D( 0.23723907259910096, 0.9628700806685033, -0.1288364474275361),
486 new Vector3D(-7.98741002062555E-24, 2.4979687659429984E-24, 3.9607863426704016E-24),
487 new Vector3D(-3.150541868418562E-23, 9.856329862034835E-24, 1.5648124883326986E-23));
488 PVCoordinates v1 = new PVCoordinates(Vector3D.PLUS_K, Vector3D.ZERO, Vector3D.ZERO);
489 PVCoordinates v2 = new PVCoordinates(Vector3D.MINUS_J, Vector3D.ZERO, Vector3D.ZERO);
490 AngularCoordinates ac = new AngularCoordinates(u1, u2, v1, v2, 1.0e-9);
491 PVCoordinates v1Computed = ac.applyTo(u1);
492 PVCoordinates v2Computed = ac.applyTo(u2);
493 Assert.assertEquals(0, Vector3D.distance(v1.getPosition(), v1Computed.getPosition()), 1.0e-15);
494 Assert.assertEquals(0, Vector3D.distance(v2.getPosition(), v2Computed.getPosition()), 1.0e-15);
495 Assert.assertEquals(0, Vector3D.distance(v1.getVelocity(), v1Computed.getVelocity()), 1.0e-15);
496 Assert.assertEquals(0, Vector3D.distance(v2.getVelocity(), v2Computed.getVelocity()), 1.0e-15);
497 Assert.assertEquals(0, Vector3D.distance(v1.getAcceleration(), v1Computed.getAcceleration()), 1.0e-15);
498 Assert.assertEquals(0, Vector3D.distance(v2.getAcceleration(), v2Computed.getAcceleration()), 1.0e-15);
499 }
500
501 private Vector3D randomVector(RandomGenerator random, double norm) {
502 double n = random.nextDouble() * norm;
503 double x = random.nextDouble();
504 double y = random.nextDouble();
505 double z = random.nextDouble();
506 return new Vector3D(n, new Vector3D(x, y, z).normalize());
507 }
508
509 private PVCoordinates randomPVCoordinates(RandomGenerator random,
510 double norm0, double norm1, double norm2) {
511 Vector3D p0 = randomVector(random, norm0);
512 Vector3D p1 = randomVector(random, norm1);
513 Vector3D p2 = randomVector(random, norm2);
514 return new PVCoordinates(p0, p1, p2);
515 }
516
517 private Rotation randomRotation(RandomGenerator random) {
518 double q0 = random.nextDouble() * 2 - 1;
519 double q1 = random.nextDouble() * 2 - 1;
520 double q2 = random.nextDouble() * 2 - 1;
521 double q3 = random.nextDouble() * 2 - 1;
522 double q = FastMath.sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
523 return new Rotation(q0 / q, q1 / q, q2 / q, q3 / q, false);
524 }
525
526 }
527