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