001    /*
002     * Licensed to the Apache Software Foundation (ASF) under one or more
003     * contributor license agreements.  See the NOTICE file distributed with
004     * this work for additional information regarding copyright ownership.
005     * The ASF licenses this file to You under the Apache License, Version 2.0
006     * (the "License"); you may not use this file except in compliance with
007     * the License.  You may obtain a copy of the License at
008     *
009     *      http://www.apache.org/licenses/LICENSE-2.0
010     *
011     * Unless required by applicable law or agreed to in writing, software
012     * distributed under the License is distributed on an "AS IS" BASIS,
013     * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
014     * See the License for the specific language governing permissions and
015     * limitations under the License.
016     */
017    
018    package org.apache.commons.math.geometry;
019    
020    import org.apache.commons.math.geometry.CardanEulerSingularityException;
021    import org.apache.commons.math.geometry.NotARotationMatrixException;
022    import org.apache.commons.math.geometry.Rotation;
023    import org.apache.commons.math.geometry.RotationOrder;
024    import org.apache.commons.math.geometry.Vector3D;
025    import org.apache.commons.math.util.MathUtils;
026    
027    import junit.framework.*;
028    
029    public class RotationTest
030      extends TestCase {
031    
032      public RotationTest(String name) {
033        super(name);
034      }
035    
036      public void testIdentity() {
037    
038        Rotation r = Rotation.IDENTITY;
039        checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
040        checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
041        checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
042        checkAngle(r.getAngle(), 0);
043    
044        r = new Rotation(-1, 0, 0, 0, false);
045        checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
046        checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
047        checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
048        checkAngle(r.getAngle(), 0);
049    
050        r = new Rotation(42, 0, 0, 0, true);
051        checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_I);
052        checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_J);
053        checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_K);
054        checkAngle(r.getAngle(), 0);
055    
056      }
057    
058      public void testAxisAngle() {
059    
060        Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * Math.PI / 3);
061        checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J);
062        checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_K);
063        checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I);
064        double s = 1 / Math.sqrt(3);
065        checkVector(r.getAxis(), new Vector3D(s, s, s));
066        checkAngle(r.getAngle(), 2 * Math.PI / 3);
067    
068        try {
069          new Rotation(new Vector3D(0, 0, 0), 2 * Math.PI / 3);
070          fail("an exception should have been thrown");
071        } catch (ArithmeticException e) {
072        } catch (Exception e) {
073          fail("unexpected exception");
074        }
075    
076        r = new Rotation(Vector3D.PLUS_K, 1.5 * Math.PI);
077        checkVector(r.getAxis(), new Vector3D(0, 0, -1));
078        checkAngle(r.getAngle(), 0.5 * Math.PI);
079    
080        r = new Rotation(Vector3D.PLUS_J, Math.PI);
081        checkVector(r.getAxis(), Vector3D.PLUS_J);
082        checkAngle(r.getAngle(), Math.PI);
083    
084        checkVector(Rotation.IDENTITY.getAxis(), Vector3D.PLUS_I);
085    
086      }
087    
088      public void testRevert() {
089        Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
090        Rotation reverted = r.revert();
091        checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
092        checkRotation(reverted.applyTo(r), 1, 0, 0, 0);
093        assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
094        assertEquals(-1, Vector3D.dotProduct(r.getAxis(), reverted.getAxis()), 1.0e-12);
095      }
096    
097      public void testVectorOnePair() {
098    
099        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    }