1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23 package org.hipparchus.geometry.euclidean.threed;
24
25 import org.hipparchus.exception.MathIllegalArgumentException;
26 import org.hipparchus.exception.MathIllegalStateException;
27 import org.hipparchus.exception.MathRuntimeException;
28 import org.hipparchus.geometry.LocalizedGeometryFormats;
29 import org.hipparchus.util.FastMath;
30 import org.hipparchus.util.MathUtils;
31 import org.junit.Assert;
32 import org.junit.Test;
33
34 import java.util.Arrays;
35
36 public class RotationTest {
37
38 @Test
39 public void testIssue304Cardan() {
40 for (final RotationConvention convention : RotationConvention.values()) {
41 for (final RotationOrder order : Arrays.asList(RotationOrder.XYZ,
42 RotationOrder.XZY,
43 RotationOrder.YXZ,
44 RotationOrder.YZX,
45 RotationOrder.ZXY,
46 RotationOrder.ZYX)) {
47
48
49 Rotation singularPlus = new Rotation(order, convention, 0.0, MathUtils.SEMI_PI, 0.125);
50 Assert.assertEquals(0.0, singularPlus.getAngles(order, convention)[0], 1.0e-16);
51 Assert.assertEquals(MathUtils.SEMI_PI, singularPlus.getAngles(order, convention)[1], 1.0e-16);
52 Assert.assertEquals(0.125, singularPlus.getAngles(order, convention)[2], 1.0e-16);
53
54
55 Rotation singularMinus = new Rotation(order, convention, 0.0, -MathUtils.SEMI_PI, 0.125);
56 Assert.assertEquals(0.0, singularMinus.getAngles(order, convention)[0], 1.0e-16);
57 Assert.assertEquals(-MathUtils.SEMI_PI, singularMinus.getAngles(order, convention)[1], 1.0e-16);
58 Assert.assertEquals(0.125, singularMinus.getAngles(order, convention)[2], 1.0e-16);
59
60 }
61 }
62 }
63
64 @Test
65 public void testIssue304Euler() {
66 for (final RotationConvention convention : RotationConvention.values()) {
67 for (final RotationOrder order : Arrays.asList(RotationOrder.XYX,
68 RotationOrder.XZX,
69 RotationOrder.YXY,
70 RotationOrder.YZY,
71 RotationOrder.ZXZ,
72 RotationOrder.ZYZ)) {
73
74
75 Rotation singularZero = new Rotation(order, convention, 0.125, 0.0, 0.0);
76 Assert.assertEquals(0.125, singularZero.getAngles(order, convention)[0], 1.0e-16);
77 Assert.assertEquals(0.0, singularZero.getAngles(order, convention)[1], 1.0e-16);
78 Assert.assertEquals(0.0, singularZero.getAngles(order, convention)[2], 1.0e-16);
79
80
81 Rotation singularPi = new Rotation(order, convention, 0.125, FastMath.PI, 0.0);
82 Assert.assertEquals(0.125, singularPi.getAngles(order, convention)[0], 1.0e-16);
83 Assert.assertEquals(FastMath.PI, singularPi.getAngles(order, convention)[1], 1.0e-16);
84 Assert.assertEquals(0.0, singularPi.getAngles(order, convention)[2], 1.0e-16);
85
86 }
87 }
88 }
89
90 @Test
91 public void testIdentity() {
92
93 Rotation r = Rotation.IDENTITY;
94 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
95 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
96 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
97 checkAngle(r.getAngle(), 0);
98
99 r = new Rotation(-1, 0, 0, 0, false);
100 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
101 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
102 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
103 checkAngle(r.getAngle(), 0);
104
105 r = new Rotation(42, 0, 0, 0, true);
106 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
107 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
108 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
109 checkAngle(r.getAngle(), 0);
110
111 }
112
113 @Test
114 public void testAxisAngleVectorOperator() throws MathIllegalArgumentException {
115
116 Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * FastMath.PI / 3, RotationConvention.VECTOR_OPERATOR);
117 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J);
118 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_K);
119 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I);
120 double s = 1 / FastMath.sqrt(3);
121 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D( s, s, s));
122 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(-s, -s, -s));
123 checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
124
125 try {
126 new Rotation(new Vector3D(0, 0, 0), 2 * FastMath.PI / 3, RotationConvention.VECTOR_OPERATOR);
127 Assert.fail("an exception should have been thrown");
128 } catch (MathIllegalArgumentException e) {
129 Assert.assertEquals(LocalizedGeometryFormats.ZERO_NORM_FOR_ROTATION_AXIS, e.getSpecifier());
130 }
131
132 r = new Rotation(Vector3D.PLUS_K, 1.5 * FastMath.PI, RotationConvention.VECTOR_OPERATOR);
133 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(0, 0, -1));
134 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(0, 0, +1));
135 checkAngle(r.getAngle(), 0.5 * FastMath.PI);
136
137 r = new Rotation(Vector3D.PLUS_J, FastMath.PI, RotationConvention.VECTOR_OPERATOR);
138 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_J);
139 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_J);
140 checkAngle(r.getAngle(), FastMath.PI);
141
142 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_I);
143 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_I);
144
145 }
146
147 @Test
148 public void testAxisAngleFrameTransform() throws MathIllegalArgumentException {
149
150 Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * FastMath.PI / 3, RotationConvention.FRAME_TRANSFORM);
151 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
152 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_I);
153 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_J);
154 double s = 1 / FastMath.sqrt(3);
155 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D( s, s, s));
156 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(-s, -s, -s));
157 checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
158
159 try {
160 new Rotation(new Vector3D(0, 0, 0), 2 * FastMath.PI / 3, RotationConvention.FRAME_TRANSFORM);
161 Assert.fail("an exception should have been thrown");
162 } catch (MathIllegalArgumentException e) {
163 Assert.assertEquals(LocalizedGeometryFormats.ZERO_NORM_FOR_ROTATION_AXIS, e.getSpecifier());
164 }
165
166 r = new Rotation(Vector3D.PLUS_K, 1.5 * FastMath.PI, RotationConvention.FRAME_TRANSFORM);
167 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(0, 0, -1));
168 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(0, 0, +1));
169 checkAngle(r.getAngle(), 0.5 * FastMath.PI);
170
171 r = new Rotation(Vector3D.PLUS_J, FastMath.PI, RotationConvention.FRAME_TRANSFORM);
172 checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.PLUS_J);
173 checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.MINUS_J);
174 checkAngle(r.getAngle(), FastMath.PI);
175
176 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_I);
177 checkVector(Rotation.IDENTITY.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_I);
178
179 }
180
181 @Test
182 public void testWrongMatrix() {
183 checkWrongMatrix(new double[2][2]);
184 checkWrongMatrix(new double[][] { new double[2], new double[3], new double[3]});
185 checkWrongMatrix(new double[][] { new double[3], new double[2], new double[3]});
186 checkWrongMatrix(new double[][] { new double[3], new double[3], new double[2]});
187 }
188
189 private void checkWrongMatrix(final double[][] m) {
190 try {
191 new Rotation(m, 0.001);
192 Assert.fail("an exception should have been thrown");
193 } catch (MathIllegalArgumentException miae) {
194 Assert.assertEquals(LocalizedGeometryFormats.ROTATION_MATRIX_DIMENSIONS, miae.getSpecifier());
195 }
196 }
197
198 @Test
199 public void testRevertVectorOperator() {
200 Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
201 Rotation reverted = r.revert();
202 checkRotation(r.compose(reverted, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0);
203 checkRotation(reverted.compose(r, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0);
204 Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
205 Assert.assertEquals(-1,
206 Vector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
207 reverted.getAxis(RotationConvention.VECTOR_OPERATOR)),
208 1.0e-12);
209 }
210
211 @Test
212 public void testRevertFrameTransform() {
213 Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
214 Rotation reverted = r.revert();
215 checkRotation(r.compose(reverted, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0);
216 checkRotation(reverted.compose(r, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0);
217 Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
218 Assert.assertEquals(-1,
219 Vector3D.dotProduct(r.getAxis(RotationConvention.FRAME_TRANSFORM),
220 reverted.getAxis(RotationConvention.FRAME_TRANSFORM)),
221 1.0e-12);
222 }
223
224 @Test
225 public void testVectorOnePair() throws MathRuntimeException {
226
227 Vector3D u = new Vector3D(3, 2, 1);
228 Vector3D v = new Vector3D(-4, 2, 2);
229 Rotation r = new Rotation(u, v);
230 checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));
231
232 checkAngle(new Rotation(u, u.negate()).getAngle(), FastMath.PI);
233
234 try {
235 new Rotation(u, Vector3D.ZERO);
236 Assert.fail("an exception should have been thrown");
237 } catch (MathRuntimeException e) {
238
239 }
240
241 }
242
243 @Test
244 public void testVectorTwoPairs() throws MathRuntimeException {
245
246 Vector3D u1 = new Vector3D(3, 0, 0);
247 Vector3D u2 = new Vector3D(0, 5, 0);
248 Vector3D v1 = new Vector3D(0, 0, 2);
249 Vector3D v2 = new Vector3D(-2, 0, 2);
250 Rotation r = new Rotation(u1, u2, v1, v2);
251 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
252 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.MINUS_I);
253
254 r = new Rotation(u1, u2, u1.negate(), u2.negate());
255 Vector3D axis = r.getAxis(RotationConvention.VECTOR_OPERATOR);
256 if (Vector3D.dotProduct(axis, Vector3D.PLUS_K) > 0) {
257 checkVector(axis, Vector3D.PLUS_K);
258 } else {
259 checkVector(axis, Vector3D.MINUS_K);
260 }
261 checkAngle(r.getAngle(), FastMath.PI);
262
263 double sqrt = FastMath.sqrt(2) / 2;
264 r = new Rotation(Vector3D.PLUS_I, Vector3D.PLUS_J,
265 new Vector3D(0.5, 0.5, sqrt),
266 new Vector3D(0.5, 0.5, -sqrt));
267 checkRotation(r, sqrt, 0.5, 0.5, 0);
268
269 r = new Rotation(u1, u2, u1, Vector3D.crossProduct(u1, u2));
270 checkRotation(r, sqrt, -sqrt, 0, 0);
271
272 checkRotation(new Rotation(u1, u2, u1, u2), 1, 0, 0, 0);
273
274 try {
275 new Rotation(u1, u2, Vector3D.ZERO, v2);
276 Assert.fail("an exception should have been thrown");
277 } catch (MathRuntimeException e) {
278
279 }
280
281 }
282
283 @Test
284 public void testMatrix()
285 throws MathIllegalArgumentException {
286
287 try {
288 new Rotation(new double[][] {
289 { 0.0, 1.0, 0.0 },
290 { 1.0, 0.0, 0.0 }
291 }, 1.0e-7);
292 Assert.fail("Expecting MathIllegalArgumentException");
293 } catch (MathIllegalArgumentException nrme) {
294
295 }
296
297 try {
298 new Rotation(new double[][] {
299 { 0.445888, 0.797184, -0.407040 },
300 { 0.821760, -0.184320, 0.539200 },
301 { -0.354816, 0.574912, 0.737280 }
302 }, 1.0e-7);
303 Assert.fail("Expecting MathIllegalArgumentException");
304 } catch (MathIllegalArgumentException nrme) {
305
306 }
307
308 try {
309 new Rotation(new double[][] {
310 { 0.4, 0.8, -0.4 },
311 { -0.4, 0.6, 0.7 },
312 { 0.8, -0.2, 0.5 }
313 }, 1.0e-15);
314 Assert.fail("Expecting MathIllegalArgumentException");
315 } catch (MathIllegalArgumentException nrme) {
316
317 }
318
319 checkRotation(new Rotation(new double[][] {
320 { 0.445888, 0.797184, -0.407040 },
321 { -0.354816, 0.574912, 0.737280 },
322 { 0.821760, -0.184320, 0.539200 }
323 }, 1.0e-10),
324 0.8, 0.288, 0.384, 0.36);
325
326 checkRotation(new Rotation(new double[][] {
327 { 0.539200, 0.737280, 0.407040 },
328 { 0.184320, -0.574912, 0.797184 },
329 { 0.821760, -0.354816, -0.445888 }
330 }, 1.0e-10),
331 0.36, 0.8, 0.288, 0.384);
332
333 checkRotation(new Rotation(new double[][] {
334 { -0.445888, 0.797184, -0.407040 },
335 { 0.354816, 0.574912, 0.737280 },
336 { 0.821760, 0.184320, -0.539200 }
337 }, 1.0e-10),
338 0.384, 0.36, 0.8, 0.288);
339
340 checkRotation(new Rotation(new double[][] {
341 { -0.539200, 0.737280, 0.407040 },
342 { -0.184320, -0.574912, 0.797184 },
343 { 0.821760, 0.354816, 0.445888 }
344 }, 1.0e-10),
345 0.288, 0.384, 0.36, 0.8);
346
347 double[][] m1 = { { 0.0, 1.0, 0.0 },
348 { 0.0, 0.0, 1.0 },
349 { 1.0, 0.0, 0.0 } };
350 Rotation r = new Rotation(m1, 1.0e-7);
351 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
352 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_I);
353 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_J);
354
355 double[][] m2 = { { 0.83203, -0.55012, -0.07139 },
356 { 0.48293, 0.78164, -0.39474 },
357 { 0.27296, 0.29396, 0.91602 } };
358 r = new Rotation(m2, 1.0e-12);
359
360 double[][] m3 = r.getMatrix();
361 double d00 = m2[0][0] - m3[0][0];
362 double d01 = m2[0][1] - m3[0][1];
363 double d02 = m2[0][2] - m3[0][2];
364 double d10 = m2[1][0] - m3[1][0];
365 double d11 = m2[1][1] - m3[1][1];
366 double d12 = m2[1][2] - m3[1][2];
367 double d20 = m2[2][0] - m3[2][0];
368 double d21 = m2[2][1] - m3[2][1];
369 double d22 = m2[2][2] - m3[2][2];
370
371 Assert.assertTrue(FastMath.abs(d00) < 6.0e-6);
372 Assert.assertTrue(FastMath.abs(d01) < 6.0e-6);
373 Assert.assertTrue(FastMath.abs(d02) < 6.0e-6);
374 Assert.assertTrue(FastMath.abs(d10) < 6.0e-6);
375 Assert.assertTrue(FastMath.abs(d11) < 6.0e-6);
376 Assert.assertTrue(FastMath.abs(d12) < 6.0e-6);
377 Assert.assertTrue(FastMath.abs(d20) < 6.0e-6);
378 Assert.assertTrue(FastMath.abs(d21) < 6.0e-6);
379 Assert.assertTrue(FastMath.abs(d22) < 6.0e-6);
380
381 Assert.assertTrue(FastMath.abs(d00) > 4.0e-7);
382 Assert.assertTrue(FastMath.abs(d01) > 4.0e-7);
383 Assert.assertTrue(FastMath.abs(d02) > 4.0e-7);
384 Assert.assertTrue(FastMath.abs(d10) > 4.0e-7);
385 Assert.assertTrue(FastMath.abs(d11) > 4.0e-7);
386 Assert.assertTrue(FastMath.abs(d12) > 4.0e-7);
387 Assert.assertTrue(FastMath.abs(d20) > 4.0e-7);
388 Assert.assertTrue(FastMath.abs(d21) > 4.0e-7);
389 Assert.assertTrue(FastMath.abs(d22) > 4.0e-7);
390
391 for (int i = 0; i < 3; ++i) {
392 for (int j = 0; j < 3; ++j) {
393 double m3tm3 = m3[i][0] * m3[j][0]
394 + m3[i][1] * m3[j][1]
395 + m3[i][2] * m3[j][2];
396 if (i == j) {
397 Assert.assertTrue(FastMath.abs(m3tm3 - 1.0) < 1.0e-10);
398 } else {
399 Assert.assertTrue(FastMath.abs(m3tm3) < 1.0e-10);
400 }
401 }
402 }
403
404 checkVector(r.applyTo(Vector3D.PLUS_I),
405 new Vector3D(m3[0][0], m3[1][0], m3[2][0]));
406 checkVector(r.applyTo(Vector3D.PLUS_J),
407 new Vector3D(m3[0][1], m3[1][1], m3[2][1]));
408 checkVector(r.applyTo(Vector3D.PLUS_K),
409 new Vector3D(m3[0][2], m3[1][2], m3[2][2]));
410
411 double[][] m4 = { { 1.0, 0.0, 0.0 },
412 { 0.0, -1.0, 0.0 },
413 { 0.0, 0.0, -1.0 } };
414 r = new Rotation(m4, 1.0e-7);
415 checkAngle(r.getAngle(), FastMath.PI);
416
417 try {
418 double[][] m5 = { { 0.0, 0.0, 1.0 },
419 { 0.0, 1.0, 0.0 },
420 { 1.0, 0.0, 0.0 } };
421 r = new Rotation(m5, 1.0e-7);
422 Assert.fail("got " + r + ", should have caught an exception");
423 } catch (MathIllegalArgumentException e) {
424
425 }
426
427 }
428
429 @Test
430 public void testAngles()
431 throws MathIllegalStateException {
432
433 for (RotationConvention convention : RotationConvention.values()) {
434 RotationOrder[] CardanOrders = {
435 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
436 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
437 };
438
439 for (int i = 0; i < CardanOrders.length; ++i) {
440 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
441 for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
442 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
443 Rotation r = new Rotation(CardanOrders[i], convention, alpha1, alpha2, alpha3);
444 double[] angles = r.getAngles(CardanOrders[i], convention);
445 checkAngle(angles[0], alpha1);
446 checkAngle(angles[1], alpha2);
447 checkAngle(angles[2], alpha3);
448 }
449 }
450 }
451 }
452
453 RotationOrder[] EulerOrders = {
454 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
455 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
456 };
457
458 for (int i = 0; i < EulerOrders.length; ++i) {
459 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
460 for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
461 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
462 Rotation r = new Rotation(EulerOrders[i], convention,
463 alpha1, alpha2, alpha3);
464 double[] angles = r.getAngles(EulerOrders[i], convention);
465 checkAngle(angles[0], alpha1);
466 checkAngle(angles[1], alpha2);
467 checkAngle(angles[2], alpha3);
468 }
469 }
470 }
471 }
472 }
473
474 }
475
476 @Test
477 public void testSingularities() {
478
479 for (RotationConvention convention : RotationConvention.values()) {
480 RotationOrder[] CardanOrders = {
481 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
482 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
483 };
484
485 double[] singularCardanAngle = {
486 -FastMath.PI / 2, -FastMath.PI / 2 + 1.0e-12, -FastMath.PI / 2 + 1.0e-10,
487 FastMath.PI / 2 - 1.0e-10, FastMath.PI / 2 - 1.0e-12, FastMath.PI / 2
488 };
489 for (int i = 0; i < CardanOrders.length; ++i) {
490 for (int j = 0; j < singularCardanAngle.length; ++j) {
491 Rotation r = new Rotation(CardanOrders[i], convention, 0.1, singularCardanAngle[j], 0.3);
492 Assert.assertEquals(singularCardanAngle[j], r.getAngles(CardanOrders[i], convention)[1], 4.5e-16);
493 }
494 }
495
496 RotationOrder[] EulerOrders = {
497 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
498 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
499 };
500
501 double[] singularEulerAngle = { 0, 1.0e-12, 1.0e-10, FastMath.PI - 1.0e-10, FastMath.PI - 1.0e-12, FastMath.PI };
502 for (int i = 0; i < EulerOrders.length; ++i) {
503 for (int j = 0; j < singularEulerAngle.length; ++j) {
504 Rotation r = new Rotation(EulerOrders[i], convention, 0.1, singularEulerAngle[j], 0.3);
505 Assert.assertEquals(singularEulerAngle[j], r.getAngles(EulerOrders[i], convention)[1], 1.0e-24);
506 }
507 }
508 }
509
510
511 }
512
513 @Test
514 public void testQuaternion() throws MathIllegalArgumentException {
515
516 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
517 double n = 23.5;
518 Rotation r2 = new Rotation(n * r1.getQ0(), n * r1.getQ1(),
519 n * r1.getQ2(), n * r1.getQ3(),
520 true);
521 for (double x = -0.9; x < 0.9; x += 0.2) {
522 for (double y = -0.9; y < 0.9; y += 0.2) {
523 for (double z = -0.9; z < 0.9; z += 0.2) {
524 Vector3D u = new Vector3D(x, y, z);
525 checkVector(r2.applyTo(u), r1.applyTo(u));
526 }
527 }
528 }
529
530 r1 = new Rotation( 0.288, 0.384, 0.36, 0.8, false);
531 checkRotation(r1, -r1.getQ0(), -r1.getQ1(), -r1.getQ2(), -r1.getQ3());
532
533 }
534
535 @Test
536 public void testApplyTo() throws MathIllegalArgumentException {
537
538 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
539 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
540 Rotation r3 = r2.applyTo(r1);
541
542 for (double x = -0.9; x < 0.9; x += 0.2) {
543 for (double y = -0.9; y < 0.9; y += 0.2) {
544 for (double z = -0.9; z < 0.9; z += 0.2) {
545 Vector3D u = new Vector3D(x, y, z);
546 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
547 }
548 }
549 }
550
551 }
552
553 @Test
554 public void testComposeVectorOperator() throws MathIllegalArgumentException {
555
556 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
557 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
558 Rotation r3 = r2.compose(r1, RotationConvention.VECTOR_OPERATOR);
559
560 for (double x = -0.9; x < 0.9; x += 0.2) {
561 for (double y = -0.9; y < 0.9; y += 0.2) {
562 for (double z = -0.9; z < 0.9; z += 0.2) {
563 Vector3D u = new Vector3D(x, y, z);
564 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
565 }
566 }
567 }
568
569 }
570
571 @Test
572 public void testComposeFrameTransform() throws MathIllegalArgumentException {
573
574 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM);
575 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM);
576 Rotation r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
577 Rotation r4 = r1.compose(r2, RotationConvention.VECTOR_OPERATOR);
578 Assert.assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15);
579
580 for (double x = -0.9; x < 0.9; x += 0.2) {
581 for (double y = -0.9; y < 0.9; y += 0.2) {
582 for (double z = -0.9; z < 0.9; z += 0.2) {
583 Vector3D u = new Vector3D(x, y, z);
584 checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
585 }
586 }
587 }
588
589 }
590
591 @Test
592 public void testApplyInverseToRotation() throws MathIllegalArgumentException {
593
594 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
595 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
596 Rotation r3 = r2.applyInverseTo(r1);
597
598 for (double x = -0.9; x < 0.9; x += 0.2) {
599 for (double y = -0.9; y < 0.9; y += 0.2) {
600 for (double z = -0.9; z < 0.9; z += 0.2) {
601 Vector3D u = new Vector3D(x, y, z);
602 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
603 }
604 }
605 }
606
607 }
608
609 @Test
610 public void testComposeInverseVectorOperator() throws MathIllegalArgumentException {
611
612 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
613 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
614 Rotation r3 = r2.composeInverse(r1, RotationConvention.VECTOR_OPERATOR);
615
616 for (double x = -0.9; x < 0.9; x += 0.2) {
617 for (double y = -0.9; y < 0.9; y += 0.2) {
618 for (double z = -0.9; z < 0.9; z += 0.2) {
619 Vector3D u = new Vector3D(x, y, z);
620 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
621 }
622 }
623 }
624
625 }
626
627 @Test
628 public void testComposeInverseFrameTransform() throws MathIllegalArgumentException {
629
630 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM);
631 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM);
632 Rotation r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
633 Rotation r4 = r1.revert().composeInverse(r2.revert(), RotationConvention.VECTOR_OPERATOR);
634 Assert.assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15);
635
636 for (double x = -0.9; x < 0.9; x += 0.2) {
637 for (double y = -0.9; y < 0.9; y += 0.2) {
638 for (double z = -0.9; z < 0.9; z += 0.2) {
639 Vector3D u = new Vector3D(x, y, z);
640 checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
641 }
642 }
643 }
644
645 }
646
647 @Test
648 public void testArray() throws MathIllegalArgumentException {
649
650 Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
651
652 for (double x = -0.9; x < 0.9; x += 0.2) {
653 for (double y = -0.9; y < 0.9; y += 0.2) {
654 for (double z = -0.9; z < 0.9; z += 0.2) {
655 Vector3D u = new Vector3D(x, y, z);
656 Vector3D v = r.applyTo(u);
657 double[] inOut = new double[] { x, y, z };
658 r.applyTo(inOut, inOut);
659 Assert.assertEquals(v.getX(), inOut[0], 1.0e-10);
660 Assert.assertEquals(v.getY(), inOut[1], 1.0e-10);
661 Assert.assertEquals(v.getZ(), inOut[2], 1.0e-10);
662 r.applyInverseTo(inOut, inOut);
663 Assert.assertEquals(u.getX(), inOut[0], 1.0e-10);
664 Assert.assertEquals(u.getY(), inOut[1], 1.0e-10);
665 Assert.assertEquals(u.getZ(), inOut[2], 1.0e-10);
666 }
667 }
668 }
669
670 }
671
672 @Test
673 public void testApplyInverseTo() throws MathIllegalArgumentException {
674
675 Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
676 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
677 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
678 Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi),
679 FastMath.sin(lambda) * FastMath.cos(phi),
680 FastMath.sin(phi));
681 r.applyInverseTo(r.applyTo(u));
682 checkVector(u, r.applyInverseTo(r.applyTo(u)));
683 checkVector(u, r.applyTo(r.applyInverseTo(u)));
684 }
685 }
686
687 r = Rotation.IDENTITY;
688 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
689 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
690 Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi),
691 FastMath.sin(lambda) * FastMath.cos(phi),
692 FastMath.sin(phi));
693 checkVector(u, r.applyInverseTo(r.applyTo(u)));
694 checkVector(u, r.applyTo(r.applyInverseTo(u)));
695 }
696 }
697
698 r = new Rotation(Vector3D.PLUS_K, FastMath.PI, RotationConvention.VECTOR_OPERATOR);
699 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
700 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
701 Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi),
702 FastMath.sin(lambda) * FastMath.cos(phi),
703 FastMath.sin(phi));
704 checkVector(u, r.applyInverseTo(r.applyTo(u)));
705 checkVector(u, r.applyTo(r.applyInverseTo(u)));
706 }
707 }
708
709 }
710
711 @Test
712 public void testIssue639() throws MathRuntimeException{
713 Vector3D u1 = new Vector3D(-1321008684645961.0 / 268435456.0,
714 -5774608829631843.0 / 268435456.0,
715 -3822921525525679.0 / 4294967296.0);
716 Vector3D u2 =new Vector3D( -5712344449280879.0 / 2097152.0,
717 -2275058564560979.0 / 1048576.0,
718 4423475992255071.0 / 65536.0);
719 Rotation rot = new Rotation(u1, u2, Vector3D.PLUS_I,Vector3D.PLUS_K);
720 Assert.assertEquals( 0.6228370359608200639829222, rot.getQ0(), 1.0e-15);
721 Assert.assertEquals( 0.0257707621456498790029987, rot.getQ1(), 1.0e-15);
722 Assert.assertEquals(-0.0000000002503012255839931, rot.getQ2(), 1.0e-15);
723 Assert.assertEquals(-0.7819270390861109450724902, rot.getQ3(), 1.0e-15);
724 }
725
726 @Test
727 public void testIssue801() throws MathRuntimeException {
728 Vector3D u1 = new Vector3D(0.9999988431610581, -0.0015210774290851095, 0.0);
729 Vector3D u2 = new Vector3D(0.0, 0.0, 1.0);
730
731 Vector3D v1 = new Vector3D(0.9999999999999999, 0.0, 0.0);
732 Vector3D v2 = new Vector3D(0.0, 0.0, -1.0);
733
734 Rotation quat = new Rotation(u1, u2, v1, v2);
735 double q2 = quat.getQ0() * quat.getQ0() +
736 quat.getQ1() * quat.getQ1() +
737 quat.getQ2() * quat.getQ2() +
738 quat.getQ3() * quat.getQ3();
739 Assert.assertEquals(1.0, q2, 1.0e-14);
740 Assert.assertEquals(0.0, Vector3D.angle(v1, quat.applyTo(u1)), 1.0e-14);
741 Assert.assertEquals(0.0, Vector3D.angle(v2, quat.applyTo(u2)), 1.0e-14);
742
743 }
744
745 @Test
746 public void testGithubPullRequest22A() {
747 final RotationOrder order = RotationOrder.ZYX;
748 final double xRotation = FastMath.toDegrees(30);
749 final double yRotation = FastMath.toDegrees(20);
750 final double zRotation = FastMath.toDegrees(10);
751 final Vector3D startingVector = Vector3D.PLUS_I;
752 Vector3D appliedIndividually = startingVector;
753 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, 0, 0).applyTo(appliedIndividually);
754 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, yRotation, 0).applyTo(appliedIndividually);
755 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, 0, xRotation).applyTo(appliedIndividually);
756
757 final Vector3D bad = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, yRotation, xRotation).applyTo(startingVector);
758
759 Assert.assertEquals(bad.getX(), appliedIndividually.getX(), 1e-12);
760 Assert.assertEquals(bad.getY(), appliedIndividually.getY(), 1e-12);
761 Assert.assertEquals(bad.getZ(), appliedIndividually.getZ(), 1e-12);
762 }
763
764 @Test
765 public void testGithubPullRequest22B() {
766 final RotationOrder order = RotationOrder.ZYX;
767 final double xRotation = FastMath.toDegrees(30);
768 final double yRotation = FastMath.toDegrees(20);
769 final double zRotation = FastMath.toDegrees(10);
770 final Vector3D startingVector = Vector3D.PLUS_I;
771 Vector3D appliedIndividually = startingVector;
772 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, 0, 0).applyTo(appliedIndividually);
773 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, yRotation, 0).applyTo(appliedIndividually);
774 appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, 0, xRotation).applyTo(appliedIndividually);
775
776 final Rotation r1 = new Rotation(order.getA1(), zRotation, RotationConvention.FRAME_TRANSFORM);
777 final Rotation r2 = new Rotation(order.getA2(), yRotation, RotationConvention.FRAME_TRANSFORM);
778 final Rotation r3 = new Rotation(order.getA3(), xRotation, RotationConvention.FRAME_TRANSFORM);
779 final Rotation composite = r1.compose(r2.compose(r3,
780 RotationConvention.FRAME_TRANSFORM),
781 RotationConvention.FRAME_TRANSFORM);
782 final Vector3D good = composite.applyTo(startingVector);
783
784 Assert.assertEquals(good.getX(), appliedIndividually.getX(), 1e-12);
785 Assert.assertEquals(good.getY(), appliedIndividually.getY(), 1e-12);
786 Assert.assertEquals(good.getZ(), appliedIndividually.getZ(), 1e-12);
787 }
788
789 private void checkVector(Vector3D v1, Vector3D v2) {
790 Assert.assertTrue(v1.subtract(v2).getNorm() < 1.0e-10);
791 }
792
793 private void checkAngle(double a1, double a2) {
794 Assert.assertEquals(a1, MathUtils.normalizeAngle(a2, a1), 1.0e-10);
795 }
796
797 private void checkRotation(Rotation r, double q0, double q1, double q2, double q3) {
798 Assert.assertEquals(0, Rotation.distance(r, new Rotation(q0, q1, q2, q3, false)), 1.0e-12);
799 }
800
801 }