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
26 import junit.framework.*;
27
28 public class RotationTest
29 extends TestCase {
30
31 public RotationTest(String name) {
32 super(name);
33 }
34
35 public void testIdentity() {
36
37 Rotation r = new Rotation();
38 checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusI);
39 checkVector(r.applyTo(Vector3D.plusJ), Vector3D.plusJ);
40 checkVector(r.applyTo(Vector3D.plusK), Vector3D.plusK);
41 checkAngle(r.getAngle(), 0);
42
43 r = new Rotation(-1, 0, 0, 0, false);
44 checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusI);
45 checkVector(r.applyTo(Vector3D.plusJ), Vector3D.plusJ);
46 checkVector(r.applyTo(Vector3D.plusK), Vector3D.plusK);
47 checkAngle(r.getAngle(), 0);
48
49 r = new Rotation(42, 0, 0, 0, true);
50 checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusI);
51 checkVector(r.applyTo(Vector3D.plusJ), Vector3D.plusJ);
52 checkVector(r.applyTo(Vector3D.plusK), Vector3D.plusK);
53 checkAngle(r.getAngle(), 0);
54
55 }
56
57 public void testAxisAngle() {
58
59 Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * Math.PI / 3);
60 checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusJ);
61 checkVector(r.applyTo(Vector3D.plusJ), Vector3D.plusK);
62 checkVector(r.applyTo(Vector3D.plusK), Vector3D.plusI);
63 double s = 1 / Math.sqrt(3);
64 checkVector(r.getAxis(), new Vector3D(s, s, s));
65 checkAngle(r.getAngle(), 2 * Math.PI / 3);
66
67 try {
68 new Rotation(new Vector3D(0, 0, 0), 2 * Math.PI / 3);
69 fail("an exception should have been thrown");
70 } catch (ArithmeticException e) {
71 } catch (Exception e) {
72 fail("unexpected exception");
73 }
74
75 r = new Rotation(Vector3D.plusK, 1.5 * Math.PI);
76 checkVector(r.getAxis(), new Vector3D(0, 0, -1));
77 checkAngle(r.getAngle(), 0.5 * Math.PI);
78
79 r = new Rotation(Vector3D.plusJ, Math.PI);
80 checkVector(r.getAxis(), Vector3D.plusJ);
81 checkAngle(r.getAngle(), Math.PI);
82
83 checkVector(new Rotation().getAxis(), Vector3D.plusI);
84
85 }
86
87 public void testRevert() {
88 Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
89 Rotation reverted = r.revert();
90 checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
91 checkRotation(reverted.applyTo(r), 1, 0, 0, 0);
92 assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
93 assertEquals(-1, Vector3D.dotProduct(r.getAxis(), reverted.getAxis()), 1.0e-12);
94 }
95
96 public void testVectorOnePair() {
97
98 Vector3D u = new Vector3D(3, 2, 1);
99 Vector3D v = new Vector3D(-4, 2, 2);
100 Rotation r = new Rotation(u, v);
101 checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));
102
103 checkAngle(new Rotation(u, u.negate()).getAngle(), Math.PI);
104
105 try {
106 new Rotation(u, new Vector3D());
107 fail("an exception should have been thrown");
108 } catch (IllegalArgumentException e) {
109
110 } catch (Exception e) {
111 fail("unexpected exception");
112 }
113
114 }
115
116 public void testVectorTwoPairs() {
117
118 Vector3D u1 = new Vector3D(3, 0, 0);
119 Vector3D u2 = new Vector3D(0, 5, 0);
120 Vector3D v1 = new Vector3D(0, 0, 2);
121 Vector3D v2 = new Vector3D(-2, 0, 2);
122 Rotation r = new Rotation(u1, u2, v1, v2);
123 checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusK);
124 checkVector(r.applyTo(Vector3D.plusJ), Vector3D.minusI);
125
126 r = new Rotation(u1, u2, u1.negate(), u2.negate());
127 Vector3D axis = r.getAxis();
128 if (Vector3D.dotProduct(axis, Vector3D.plusK) > 0) {
129 checkVector(axis, Vector3D.plusK);
130 } else {
131 checkVector(axis, Vector3D.minusK);
132 }
133 checkAngle(r.getAngle(), Math.PI);
134
135 double sqrt = Math.sqrt(2) / 2;
136 r = new Rotation(Vector3D.plusI, Vector3D.plusJ,
137 new Vector3D(0.5, 0.5, sqrt),
138 new Vector3D(0.5, 0.5, -sqrt));
139 checkRotation(r, sqrt, 0.5, 0.5, 0);
140
141 r = new Rotation(u1, u2, u1, Vector3D.crossProduct(u1, u2));
142 checkRotation(r, sqrt, -sqrt, 0, 0);
143
144 checkRotation(new Rotation(u1, u2, u1, u2), 1, 0, 0, 0);
145
146 try {
147 new Rotation(u1, u2, new Vector3D(), v2);
148 fail("an exception should have been thrown");
149 } catch (IllegalArgumentException e) {
150
151 } catch (Exception e) {
152 fail("unexpected exception");
153 }
154
155 }
156
157 public void testMatrix()
158 throws NotARotationMatrixException {
159
160 try {
161 new Rotation(new double[][] {
162 { 0.0, 1.0, 0.0 },
163 { 1.0, 0.0, 0.0 }
164 }, 1.0e-7);
165 } catch (NotARotationMatrixException nrme) {
166
167 } catch (Exception e) {
168 fail("wrong exception caught: " + e.getMessage());
169 }
170
171 try {
172 new Rotation(new double[][] {
173 { 0.445888, 0.797184, -0.407040 },
174 { 0.821760, -0.184320, 0.539200 },
175 { -0.354816, 0.574912, 0.737280 }
176 }, 1.0e-7);
177 } catch (NotARotationMatrixException nrme) {
178
179 } catch (Exception e) {
180 fail("wrong exception caught: " + e.getMessage());
181 }
182
183 try {
184 new Rotation(new double[][] {
185 { 0.4, 0.8, -0.4 },
186 { -0.4, 0.6, 0.7 },
187 { 0.8, -0.2, 0.5 }
188 }, 1.0e-15);
189 } catch (NotARotationMatrixException nrme) {
190
191 } catch (Exception e) {
192 fail("wrong exception caught: " + e.getMessage());
193 }
194
195 checkRotation(new Rotation(new double[][] {
196 { 0.445888, 0.797184, -0.407040 },
197 { -0.354816, 0.574912, 0.737280 },
198 { 0.821760, -0.184320, 0.539200 }
199 }, 1.0e-10),
200 0.8, 0.288, 0.384, 0.36);
201
202 checkRotation(new Rotation(new double[][] {
203 { 0.539200, 0.737280, 0.407040 },
204 { 0.184320, -0.574912, 0.797184 },
205 { 0.821760, -0.354816, -0.445888 }
206 }, 1.0e-10),
207 0.36, 0.8, 0.288, 0.384);
208
209 checkRotation(new Rotation(new double[][] {
210 { -0.445888, 0.797184, -0.407040 },
211 { 0.354816, 0.574912, 0.737280 },
212 { 0.821760, 0.184320, -0.539200 }
213 }, 1.0e-10),
214 0.384, 0.36, 0.8, 0.288);
215
216 checkRotation(new Rotation(new double[][] {
217 { -0.539200, 0.737280, 0.407040 },
218 { -0.184320, -0.574912, 0.797184 },
219 { 0.821760, 0.354816, 0.445888 }
220 }, 1.0e-10),
221 0.288, 0.384, 0.36, 0.8);
222
223 double[][] m1 = { { 0.0, 1.0, 0.0 },
224 { 0.0, 0.0, 1.0 },
225 { 1.0, 0.0, 0.0 } };
226 Rotation r = new Rotation(m1, 1.0e-7);
227 checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusK);
228 checkVector(r.applyTo(Vector3D.plusJ), Vector3D.plusI);
229 checkVector(r.applyTo(Vector3D.plusK), Vector3D.plusJ);
230
231 double[][] m2 = { { 0.83203, -0.55012, -0.07139 },
232 { 0.48293, 0.78164, -0.39474 },
233 { 0.27296, 0.29396, 0.91602 } };
234 r = new Rotation(m2, 1.0e-12);
235
236 double[][] m3 = r.getMatrix();
237 double d00 = m2[0][0] - m3[0][0];
238 double d01 = m2[0][1] - m3[0][1];
239 double d02 = m2[0][2] - m3[0][2];
240 double d10 = m2[1][0] - m3[1][0];
241 double d11 = m2[1][1] - m3[1][1];
242 double d12 = m2[1][2] - m3[1][2];
243 double d20 = m2[2][0] - m3[2][0];
244 double d21 = m2[2][1] - m3[2][1];
245 double d22 = m2[2][2] - m3[2][2];
246
247 assertTrue(Math.abs(d00) < 6.0e-6);
248 assertTrue(Math.abs(d01) < 6.0e-6);
249 assertTrue(Math.abs(d02) < 6.0e-6);
250 assertTrue(Math.abs(d10) < 6.0e-6);
251 assertTrue(Math.abs(d11) < 6.0e-6);
252 assertTrue(Math.abs(d12) < 6.0e-6);
253 assertTrue(Math.abs(d20) < 6.0e-6);
254 assertTrue(Math.abs(d21) < 6.0e-6);
255 assertTrue(Math.abs(d22) < 6.0e-6);
256
257 assertTrue(Math.abs(d00) > 4.0e-7);
258 assertTrue(Math.abs(d01) > 4.0e-7);
259 assertTrue(Math.abs(d02) > 4.0e-7);
260 assertTrue(Math.abs(d10) > 4.0e-7);
261 assertTrue(Math.abs(d11) > 4.0e-7);
262 assertTrue(Math.abs(d12) > 4.0e-7);
263 assertTrue(Math.abs(d20) > 4.0e-7);
264 assertTrue(Math.abs(d21) > 4.0e-7);
265 assertTrue(Math.abs(d22) > 4.0e-7);
266
267 for (int i = 0; i < 3; ++i) {
268 for (int j = 0; j < 3; ++j) {
269 double m3tm3 = m3[i][0] * m3[j][0]
270 + m3[i][1] * m3[j][1]
271 + m3[i][2] * m3[j][2];
272 if (i == j) {
273 assertTrue(Math.abs(m3tm3 - 1.0) < 1.0e-10);
274 } else {
275 assertTrue(Math.abs(m3tm3) < 1.0e-10);
276 }
277 }
278 }
279
280 checkVector(r.applyTo(Vector3D.plusI),
281 new Vector3D(m3[0][0], m3[1][0], m3[2][0]));
282 checkVector(r.applyTo(Vector3D.plusJ),
283 new Vector3D(m3[0][1], m3[1][1], m3[2][1]));
284 checkVector(r.applyTo(Vector3D.plusK),
285 new Vector3D(m3[0][2], m3[1][2], m3[2][2]));
286
287 double[][] m4 = { { 1.0, 0.0, 0.0 },
288 { 0.0, -1.0, 0.0 },
289 { 0.0, 0.0, -1.0 } };
290 r = new Rotation(m4, 1.0e-7);
291 checkAngle(r.getAngle(), Math.PI);
292
293 try {
294 double[][] m5 = { { 0.0, 0.0, 1.0 },
295 { 0.0, 1.0, 0.0 },
296 { 1.0, 0.0, 0.0 } };
297 r = new Rotation(m5, 1.0e-7);
298 fail("got " + r + ", should have caught an exception");
299 } catch (NotARotationMatrixException e) {
300
301 } catch (Exception e) {
302 fail("wrong exception caught");
303 }
304
305 }
306
307 public void testAngles()
308 throws CardanEulerSingularityException {
309
310 RotationOrder[] CardanOrders = {
311 RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
312 RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
313 };
314
315 for (int i = 0; i < CardanOrders.length; ++i) {
316 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
317 for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
318 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
319 Rotation r = new Rotation(CardanOrders[i], alpha1, alpha2, alpha3);
320 double[] angles = r.getAngles(CardanOrders[i]);
321 checkAngle(angles[0], alpha1);
322 checkAngle(angles[1], alpha2);
323 checkAngle(angles[2], alpha3);
324 }
325 }
326 }
327 }
328
329 RotationOrder[] EulerOrders = {
330 RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
331 RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
332 };
333
334 for (int i = 0; i < EulerOrders.length; ++i) {
335 for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
336 for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
337 for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
338 Rotation r = new Rotation(EulerOrders[i],
339 alpha1, alpha2, alpha3);
340 double[] angles = r.getAngles(EulerOrders[i]);
341 checkAngle(angles[0], alpha1);
342 checkAngle(angles[1], alpha2);
343 checkAngle(angles[2], alpha3);
344 }
345 }
346 }
347 }
348
349 }
350
351 public void testSingularities()
352 throws CardanEulerSingularityException {
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 = new Rotation();
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.plusK, 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 a2 -= 2 * Math.PI * Math.floor((a2 + Math.PI - a1) / (2 * Math.PI));
496 assertTrue(Math.abs(a1 - a2) < 1.0e-10);
497 }
498
499 private void checkRotation(Rotation r, double q0, double q1, double q2, double q3) {
500 Rotation reference = new Rotation(q0, q1, q2, q3, false);
501 assertEquals(0, r.applyInverseTo(reference).getAngle(), 1.0e-12);
502 }
503
504 public static Test suite() {
505 return new TestSuite(RotationTest.class);
506 }
507
508 }