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