1   /* Copyright 2002-2020 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
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                 // the error in shiftedBy taking acceleration into account is cubic
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                 // the error in shiftedBy not taking acceleration into account is quadratic
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         // the rotations are really different from each other
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         // check the two-way conversion result in identity
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         // identity
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         // PI angle rotation (which is singular for non-modified Rodrigues vector)
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             // expected
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