1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18 package org.apache.commons.math.geometry;
19
20 import org.apache.commons.math.geometry.CardanEulerSingularityException;
21 import org.apache.commons.math.geometry.NotARotationMatrixException;
22 import org.apache.commons.math.geometry.Rotation;
23 import org.apache.commons.math.geometry.RotationOrder;
24 import org.apache.commons.math.geometry.Vector3D;
25 import org.apache.commons.math.util.MathUtils;
26
27 import junit.framework.*;
28
29 public class RotationTest
30 extends TestCase {
31
32 public RotationTest(String name) {
33 super(name);
34 }
35
36 public void testIdentity() {
37
38 Rotation r = Rotation.IDENTITY;
39 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
40 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
41 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
42 checkAngle(r.getAngle(), 0);
43
44 r = new Rotation(-1, 0, 0, 0, false);
45 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
46 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
47 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
48 checkAngle(r.getAngle(), 0);
49
50 r = new Rotation(42, 0, 0, 0, true);
51 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
52 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
53 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
54 checkAngle(r.getAngle(), 0);
55
56 }
57
58 public void testAxisAngle() {
59
60 Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * Math.PI / 3);
61 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J);
62 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_K);
63 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I);
64 double s = 1 / Math.sqrt(3);
65 checkVector(r.getAxis(), new Vector3D(s, s, s));
66 checkAngle(r.getAngle(), 2 * Math.PI / 3);
67
68 try {
69 new Rotation(new Vector3D(0, 0, 0), 2 * Math.PI / 3);
70 fail("an exception should have been thrown");
71 } catch (ArithmeticException e) {
72 } catch (Exception e) {
73 fail("unexpected exception");
74 }
75
76 r = new Rotation(Vector3D.PLUS_K, 1.5 * Math.PI);
77 checkVector(r.getAxis(), new Vector3D(0, 0, -1));
78 checkAngle(r.getAngle(), 0.5 * Math.PI);
79
80 r = new Rotation(Vector3D.PLUS_J, Math.PI);
81 checkVector(r.getAxis(), Vector3D.PLUS_J);
82 checkAngle(r.getAngle(), Math.PI);
83
84 checkVector(Rotation.IDENTITY.getAxis(), Vector3D.PLUS_I);
85
86 }
87
88 public void testRevert() {
89 Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
90 Rotation reverted = r.revert();
91 checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
92 checkRotation(reverted.applyTo(r), 1, 0, 0, 0);
93 assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
94 assertEquals(-1, Vector3D.dotProduct(r.getAxis(), reverted.getAxis()), 1.0e-12);
95 }
96
97 public void testVectorOnePair() {
98
99 Vector3D u = new Vector3D(3, 2, 1);
100 Vector3D v = new Vector3D(-4, 2, 2);
101 Rotation r = new Rotation(u, v);
102 checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));
103
104 checkAngle(new Rotation(u, u.negate()).getAngle(), Math.PI);
105
106 try {
107 new Rotation(u, Vector3D.ZERO);
108 fail("an exception should have been thrown");
109 } catch (IllegalArgumentException e) {
110
111 } catch (Exception e) {
112 fail("unexpected exception");
113 }
114
115 }
116
117 public void testVectorTwoPairs() {
118
119 Vector3D u1 = new Vector3D(3, 0, 0);
120 Vector3D u2 = new Vector3D(0, 5, 0);
121 Vector3D v1 = new Vector3D(0, 0, 2);
122 Vector3D v2 = new Vector3D(-2, 0, 2);
123 Rotation r = new Rotation(u1, u2, v1, v2);
124 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
125 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.MINUS_I);
126
127 r = new Rotation(u1, u2, u1.negate(), u2.negate());
128 Vector3D axis = r.getAxis();
129 if (Vector3D.dotProduct(axis, Vector3D.PLUS_K) > 0) {
130 checkVector(axis, Vector3D.PLUS_K);
131 } else {
132 checkVector(axis, Vector3D.MINUS_K);
133 }
134 checkAngle(r.getAngle(), Math.PI);
135
136 double sqrt = Math.sqrt(2) / 2;
137 r = new Rotation(Vector3D.PLUS_I, Vector3D.PLUS_J,
138 new Vector3D(0.5, 0.5, sqrt),
139 new Vector3D(0.5, 0.5, -sqrt));
140 checkRotation(r, sqrt, 0.5, 0.5, 0);
141
142 r = new Rotation(u1, u2, u1, Vector3D.crossProduct(u1, u2));
143 checkRotation(r, sqrt, -sqrt, 0, 0);
144
145 checkRotation(new Rotation(u1, u2, u1, u2), 1, 0, 0, 0);
146
147 try {
148 new Rotation(u1, u2, Vector3D.ZERO, v2);
149 fail("an exception should have been thrown");
150 } catch (IllegalArgumentException e) {
151
152 } catch (Exception e) {
153 fail("unexpected exception");
154 }
155
156 }
157
158 public void testMatrix()
159 throws NotARotationMatrixException {
160
161 try {
162 new Rotation(new double[][] {
163 { 0.0, 1.0, 0.0 },
164 { 1.0, 0.0, 0.0 }
165 }, 1.0e-7);
166 } catch (NotARotationMatrixException nrme) {
167
168 } catch (Exception e) {
169 fail("wrong exception caught: " + e.getMessage());
170 }
171
172 try {
173 new Rotation(new double[][] {
174 { 0.445888, 0.797184, -0.407040 },
175 { 0.821760, -0.184320, 0.539200 },
176 { -0.354816, 0.574912, 0.737280 }
177 }, 1.0e-7);
178 } catch (NotARotationMatrixException nrme) {
179
180 } catch (Exception e) {
181 fail("wrong exception caught: " + e.getMessage());
182 }
183
184 try {
185 new Rotation(new double[][] {
186 { 0.4, 0.8, -0.4 },
187 { -0.4, 0.6, 0.7 },
188 { 0.8, -0.2, 0.5 }
189 }, 1.0e-15);
190 } catch (NotARotationMatrixException nrme) {
191
192 } catch (Exception e) {
193 fail("wrong exception caught: " + e.getMessage());
194 }
195
196 checkRotation(new Rotation(new double[][] {
197 { 0.445888, 0.797184, -0.407040 },
198 { -0.354816, 0.574912, 0.737280 },
199 { 0.821760, -0.184320, 0.539200 }
200 }, 1.0e-10),
201 0.8, 0.288, 0.384, 0.36);
202
203 checkRotation(new Rotation(new double[][] {
204 { 0.539200, 0.737280, 0.407040 },
205 { 0.184320, -0.574912, 0.797184 },
206 { 0.821760, -0.354816, -0.445888 }
207 }, 1.0e-10),
208 0.36, 0.8, 0.288, 0.384);
209
210 checkRotation(new Rotation(new double[][] {
211 { -0.445888, 0.797184, -0.407040 },
212 { 0.354816, 0.574912, 0.737280 },
213 { 0.821760, 0.184320, -0.539200 }
214 }, 1.0e-10),
215 0.384, 0.36, 0.8, 0.288);
216
217 checkRotation(new Rotation(new double[][] {
218 { -0.539200, 0.737280, 0.407040 },
219 { -0.184320, -0.574912, 0.797184 },
220 { 0.821760, 0.354816, 0.445888 }
221 }, 1.0e-10),
222 0.288, 0.384, 0.36, 0.8);
223
224 double[][] m1 = { { 0.0, 1.0, 0.0 },
225 { 0.0, 0.0, 1.0 },
226 { 1.0, 0.0, 0.0 } };
227 Rotation r = new Rotation(m1, 1.0e-7);
228 checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
229 checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_I);
230 checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_J);
231
232 double[][] m2 = { { 0.83203, -0.55012, -0.07139 },
233 { 0.48293, 0.78164, -0.39474 },
234 { 0.27296, 0.29396, 0.91602 } };
235 r = new Rotation(m2, 1.0e-12);
236
237 double[][] m3 = r.getMatrix();
238 double d00 = m2[0][0] - m3[0][0];
239 double d01 = m2[0][1] - m3[0][1];
240 double d02 = m2[0][2] - m3[0][2];
241 double d10 = m2[1][0] - m3[1][0];
242 double d11 = m2[1][1] - m3[1][1];
243 double d12 = m2[1][2] - m3[1][2];
244 double d20 = m2[2][0] - m3[2][0];
245 double d21 = m2[2][1] - m3[2][1];
246 double d22 = m2[2][2] - m3[2][2];
247
248 assertTrue(Math.abs(d00) < 6.0e-6);
249 assertTrue(Math.abs(d01) < 6.0e-6);
250 assertTrue(Math.abs(d02) < 6.0e-6);
251 assertTrue(Math.abs(d10) < 6.0e-6);
252 assertTrue(Math.abs(d11) < 6.0e-6);
253 assertTrue(Math.abs(d12) < 6.0e-6);
254 assertTrue(Math.abs(d20) < 6.0e-6);
255 assertTrue(Math.abs(d21) < 6.0e-6);
256 assertTrue(Math.abs(d22) < 6.0e-6);
257
258 assertTrue(Math.abs(d00) > 4.0e-7);
259 assertTrue(Math.abs(d01) > 4.0e-7);
260 assertTrue(Math.abs(d02) > 4.0e-7);
261 assertTrue(Math.abs(d10) > 4.0e-7);
262 assertTrue(Math.abs(d11) > 4.0e-7);
263 assertTrue(Math.abs(d12) > 4.0e-7);
264 assertTrue(Math.abs(d20) > 4.0e-7);
265 assertTrue(Math.abs(d21) > 4.0e-7);
266 assertTrue(Math.abs(d22) > 4.0e-7);
267
268 for (int i = 0; i < 3; ++i) {
269 for (int j = 0; j < 3; ++j) {
270 double m3tm3 = m3[i][0] * m3[j][0]
271 + m3[i][1] * m3[j][1]
272 + m3[i][2] * m3[j][2];
273 if (i == j) {
274 assertTrue(Math.abs(m3tm3 - 1.0) < 1.0e-10);
275 } else {
276 assertTrue(Math.abs(m3tm3) < 1.0e-10);
277 }
278 }
279 }
280
281 checkVector(r.applyTo(Vector3D.PLUS_I),
282 new Vector3D(m3[0][0], m3[1][0], m3[2][0]));
283 checkVector(r.applyTo(Vector3D.PLUS_J),
284 new Vector3D(m3[0][1], m3[1][1], m3[2][1]));
285 checkVector(r.applyTo(Vector3D.PLUS_K),
286 new Vector3D(m3[0][2], m3[1][2], m3[2][2]));
287
288 double[][] m4 = { { 1.0, 0.0, 0.0 },
289 { 0.0, -1.0, 0.0 },
290 { 0.0, 0.0, -1.0 } };
291 r = new Rotation(m4, 1.0e-7);
292 checkAngle(r.getAngle(), Math.PI);
293
294 try {
295 double[][] m5 = { { 0.0, 0.0, 1.0 },
296 { 0.0, 1.0, 0.0 },
297 { 1.0, 0.0, 0.0 } };
298 r = new Rotation(m5, 1.0e-7);
299 fail("got " + r + ", should have caught an exception");
300 } catch (NotARotationMatrixException e) {
301
302 } catch (Exception e) {
303 fail("wrong exception caught");
304 }
305
306 }
307
308 public void testAngles()
309 throws CardanEulerSingularityException {
310
311 RotationOrder[] CardanOrders = {
312 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
313 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
314 };
315
316 for (int i = 0; i < CardanOrders.length; ++i) {
317 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
318 for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
319 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
320 Rotation r = new Rotation(CardanOrders[i], alpha1, alpha2, alpha3);
321 double[] angles = r.getAngles(CardanOrders[i]);
322 checkAngle(angles[0], alpha1);
323 checkAngle(angles[1], alpha2);
324 checkAngle(angles[2], alpha3);
325 }
326 }
327 }
328 }
329
330 RotationOrder[] EulerOrders = {
331 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
332 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
333 };
334
335 for (int i = 0; i < EulerOrders.length; ++i) {
336 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
337 for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
338 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
339 Rotation r = new Rotation(EulerOrders[i],
340 alpha1, alpha2, alpha3);
341 double[] angles = r.getAngles(EulerOrders[i]);
342 checkAngle(angles[0], alpha1);
343 checkAngle(angles[1], alpha2);
344 checkAngle(angles[2], alpha3);
345 }
346 }
347 }
348 }
349
350 }
351
352 public void testSingularities() {
353
354 RotationOrder[] CardanOrders = {
355 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
356 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
357 };
358
359 double[] singularCardanAngle = { Math.PI / 2, -Math.PI / 2 };
360 for (int i = 0; i < CardanOrders.length; ++i) {
361 for (int j = 0; j < singularCardanAngle.length; ++j) {
362 Rotation r = new Rotation(CardanOrders[i], 0.1, singularCardanAngle[j], 0.3);
363 try {
364 r.getAngles(CardanOrders[i]);
365 fail("an exception should have been caught");
366 } catch (CardanEulerSingularityException cese) {
367
368 } catch (Exception e) {
369 fail("wrong exception caught: " + e.getMessage());
370 }
371 }
372 }
373
374 RotationOrder[] EulerOrders = {
375 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
376 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
377 };
378
379 double[] singularEulerAngle = { 0, Math.PI };
380 for (int i = 0; i < EulerOrders.length; ++i) {
381 for (int j = 0; j < singularEulerAngle.length; ++j) {
382 Rotation r = new Rotation(EulerOrders[i], 0.1, singularEulerAngle[j], 0.3);
383 try {
384 r.getAngles(EulerOrders[i]);
385 fail("an exception should have been caught");
386 } catch (CardanEulerSingularityException cese) {
387
388 } catch (Exception e) {
389 fail("wrong exception caught: " + e.getMessage());
390 }
391 }
392 }
393
394
395 }
396
397 public void testQuaternion() {
398
399 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7);
400 double n = 23.5;
401 Rotation r2 = new Rotation(n * r1.getQ0(), n * r1.getQ1(),
402 n * r1.getQ2(), n * r1.getQ3(),
403 true);
404 for (double x = -0.9; x < 0.9; x += 0.2) {
405 for (double y = -0.9; y < 0.9; y += 0.2) {
406 for (double z = -0.9; z < 0.9; z += 0.2) {
407 Vector3D u = new Vector3D(x, y, z);
408 checkVector(r2.applyTo(u), r1.applyTo(u));
409 }
410 }
411 }
412
413 r1 = new Rotation( 0.288, 0.384, 0.36, 0.8, false);
414 checkRotation(r1, -r1.getQ0(), -r1.getQ1(), -r1.getQ2(), -r1.getQ3());
415
416 }
417
418 public void testCompose() {
419
420 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7);
421 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3);
422 Rotation r3 = r2.applyTo(r1);
423
424 for (double x = -0.9; x < 0.9; x += 0.2) {
425 for (double y = -0.9; y < 0.9; y += 0.2) {
426 for (double z = -0.9; z < 0.9; z += 0.2) {
427 Vector3D u = new Vector3D(x, y, z);
428 checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
429 }
430 }
431 }
432
433 }
434
435 public void testComposeInverse() {
436
437 Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7);
438 Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3);
439 Rotation r3 = r2.applyInverseTo(r1);
440
441 for (double x = -0.9; x < 0.9; x += 0.2) {
442 for (double y = -0.9; y < 0.9; y += 0.2) {
443 for (double z = -0.9; z < 0.9; z += 0.2) {
444 Vector3D u = new Vector3D(x, y, z);
445 checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
446 }
447 }
448 }
449
450 }
451
452 public void testApplyInverseTo() {
453
454 Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7);
455 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
456 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
457 Vector3D u = new Vector3D(Math.cos(lambda) * Math.cos(phi),
458 Math.sin(lambda) * Math.cos(phi),
459 Math.sin(phi));
460 r.applyInverseTo(r.applyTo(u));
461 checkVector(u, r.applyInverseTo(r.applyTo(u)));
462 checkVector(u, r.applyTo(r.applyInverseTo(u)));
463 }
464 }
465
466 r = Rotation.IDENTITY;
467 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
468 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
469 Vector3D u = new Vector3D(Math.cos(lambda) * Math.cos(phi),
470 Math.sin(lambda) * Math.cos(phi),
471 Math.sin(phi));
472 checkVector(u, r.applyInverseTo(r.applyTo(u)));
473 checkVector(u, r.applyTo(r.applyInverseTo(u)));
474 }
475 }
476
477 r = new Rotation(Vector3D.PLUS_K, Math.PI);
478 for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
479 for (double phi = -1.55; phi < 1.55; phi += 0.2) {
480 Vector3D u = new Vector3D(Math.cos(lambda) * Math.cos(phi),
481 Math.sin(lambda) * Math.cos(phi),
482 Math.sin(phi));
483 checkVector(u, r.applyInverseTo(r.applyTo(u)));
484 checkVector(u, r.applyTo(r.applyInverseTo(u)));
485 }
486 }
487
488 }
489
490 private void checkVector(Vector3D v1, Vector3D v2) {
491 assertTrue(v1.subtract(v2).getNorm() < 1.0e-10);
492 }
493
494 private void checkAngle(double a1, double a2) {
495 assertEquals(a1, MathUtils.normalizeAngle(a2, a1), 1.0e-10);
496 }
497
498 private void checkRotation(Rotation r, double q0, double q1, double q2, double q3) {
499 assertEquals(0, Rotation.distance(r, new Rotation(q0, q1, q2, q3, false)), 1.0e-12);
500 }
501
502 public static Test suite() {
503 return new TestSuite(RotationTest.class);
504 }
505
506 }