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