1   /*
2    * Licensed to the Apache Software Foundation (ASF) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * The ASF licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *      http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
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         // expected behavior
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       // expected behavior
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       // expected behavior
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       // expected behavior
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         // expected behavior
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       // expected
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           // expected behavior
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           // expected behavior
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 }