gt:     for (double x = -0.9; x < 0.9; x += 0.2) {
context: /*
   * Licensed to the Apache Software Foundation (ASF) under one or more
   * contributor license agreements.  See the NOTICE file distributed with
   * this work for additional information regarding copyright ownership.
   * The ASF licenses this file to You under the Apache License, Version 2.0
   * (the "License"); you may not use this file except in compliance with
   * the License.  You may obtain a copy of the License at
   *
   *      http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  
  package org.apache.commons.math.geometry;
  
  import org.apache.commons.math.geometry.CardanEulerSingularityException;
  import org.apache.commons.math.geometry.NotARotationMatrixException;
  import org.apache.commons.math.geometry.Rotation;
  import org.apache.commons.math.geometry.RotationOrder;
  import org.apache.commons.math.geometry.Vector3D;
  
  import junit.framework.*;
  
  public class RotationTest
    extends TestCase {
  
    public RotationTest(String name) {
      super(name);
    }
  
    public void testIdentity() {
  
      Rotation r = new Rotation();
      checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusI);
      checkVector(r.applyTo(Vector3D.plusJ), Vector3D.plusJ);
      checkVector(r.applyTo(Vector3D.plusK), Vector3D.plusK);
      checkAngle(r.getAngle(), 0);
  
      r = new Rotation(-1, 0, 0, 0, false);
      checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusI);
      checkVector(r.applyTo(Vector3D.plusJ), Vector3D.plusJ);
      checkVector(r.applyTo(Vector3D.plusK), Vector3D.plusK);
      checkAngle(r.getAngle(), 0);
  
      r = new Rotation(42, 0, 0, 0, true);
      checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusI);
      checkVector(r.applyTo(Vector3D.plusJ), Vector3D.plusJ);
      checkVector(r.applyTo(Vector3D.plusK), Vector3D.plusK);
      checkAngle(r.getAngle(), 0);
  
    }
  
    public void testAxisAngle() {
  
      Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * Math.PI / 3);
      checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusJ);
      checkVector(r.applyTo(Vector3D.plusJ), Vector3D.plusK);
      checkVector(r.applyTo(Vector3D.plusK), Vector3D.plusI);
      double s = 1 / Math.sqrt(3);
      checkVector(r.getAxis(), new Vector3D(s, s, s));
      checkAngle(r.getAngle(), 2 * Math.PI / 3);
  
      try {
        new Rotation(new Vector3D(0, 0, 0), 2 * Math.PI / 3);
        fail("an exception should have been thrown");
      } catch (ArithmeticException e) {
      } catch (Exception e) {
        fail("unexpected exception");
      }
  
      r = new Rotation(Vector3D.plusK, 1.5 * Math.PI);
      checkVector(r.getAxis(), new Vector3D(0, 0, -1));
      checkAngle(r.getAngle(), 0.5 * Math.PI);
  
      r = new Rotation(Vector3D.plusJ, Math.PI);
      checkVector(r.getAxis(), Vector3D.plusJ);
      checkAngle(r.getAngle(), Math.PI);
  
      checkVector(new Rotation().getAxis(), Vector3D.plusI);
  
    }
  
    public void testRevert() {
      Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
      Rotation reverted = r.revert();
      checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
      checkRotation(reverted.applyTo(r), 1, 0, 0, 0);
      assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
      assertEquals(-1, Vector3D.dotProduct(r.getAxis(), reverted.getAxis()), 1.0e-12);
    }
  
    public void testVectorOnePair() {
  
      Vector3D u = new Vector3D(3, 2, 1);
      Vector3D v = new Vector3D(-4, 2, 2);
      Rotation r = new Rotation(u, v);
      checkVector(r.applyTo(u.scalarMultiply(v.getNorm())), v.scalarMultiply(u.getNorm()));
  
      checkAngle(new Rotation(u, u.negate()).getAngle(), Math.PI);
  
      try {
          new Rotation(u, new Vector3D());
          fail("an exception should have been thrown");
        } catch (IllegalArgumentException e) {
          // expected behavior
        } catch (Exception e) {
          fail("unexpected exception");
      }
  
    }
  
    public void testVectorTwoPairs() {
  
      Vector3D u1 = new Vector3D(3, 0, 0);
      Vector3D u2 = new Vector3D(0, 5, 0);
      Vector3D v1 = new Vector3D(0, 0, 2);
      Vector3D v2 = new Vector3D(-2, 0, 2);
      Rotation r = new Rotation(u1, u2, v1, v2);
      checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusK);
      checkVector(r.applyTo(Vector3D.plusJ), Vector3D.minusI);
  
      r = new Rotation(u1, u2, u1.negate(), u2.negate());
      Vector3D axis = r.getAxis();
      if (Vector3D.dotProduct(axis, Vector3D.plusK) > 0) {
        checkVector(axis, Vector3D.plusK);
      } else {
        checkVector(axis, Vector3D.minusK);
      }
      checkAngle(r.getAngle(), Math.PI);
  
      double sqrt = Math.sqrt(2) / 2;
      r = new Rotation(Vector3D.plusI,  Vector3D.plusJ,
                       new Vector3D(0.5, 0.5,  sqrt),
                       new Vector3D(0.5, 0.5, -sqrt));
      checkRotation(r, sqrt, 0.5, 0.5, 0);
  
      r = new Rotation(u1, u2, u1, Vector3D.crossProduct(u1, u2));
      checkRotation(r, sqrt, -sqrt, 0, 0);
  
      checkRotation(new Rotation(u1, u2, u1, u2), 1, 0, 0, 0);
  
      try {
          new Rotation(u1, u2, new Vector3D(), v2);
          fail("an exception should have been thrown");
      } catch (IllegalArgumentException e) {
        // expected behavior
      } catch (Exception e) {
          fail("unexpected exception");
      }
  
    }
  
    public void testMatrix()
      throws NotARotationMatrixException {
  
      try {
        new Rotation(new double[][] {
                       { 0.0, 1.0, 0.0 },
                       { 1.0, 0.0, 0.0 }
                     }, 1.0e-7);
      } catch (NotARotationMatrixException nrme) {
        // expected behavior
      } catch (Exception e) {
        fail("wrong exception caught: " + e.getMessage());
      }
  
      try {
        new Rotation(new double[][] {
                       {  0.445888,  0.797184, -0.407040 },
                       {  0.821760, -0.184320,  0.539200 },
                       { -0.354816,  0.574912,  0.737280 }
                     }, 1.0e-7);
      } catch (NotARotationMatrixException nrme) {
        // expected behavior
      } catch (Exception e) {
        fail("wrong exception caught: " + e.getMessage());
      }
  
      try {
          new Rotation(new double[][] {
                         {  0.4,  0.8, -0.4 },
                         { -0.4,  0.6,  0.7 },
                         {  0.8, -0.2,  0.5 }
                       }, 1.0e-15);
        } catch (NotARotationMatrixException nrme) {
          // expected behavior
        } catch (Exception e) {
          fail("wrong exception caught: " + e.getMessage());
        }
  
      checkRotation(new Rotation(new double[][] {
                                   {  0.445888,  0.797184, -0.407040 },
                                   { -0.354816,  0.574912,  0.737280 },
                                   {  0.821760, -0.184320,  0.539200 }
                                 }, 1.0e-10),
                    0.8, 0.288, 0.384, 0.36);
  
      checkRotation(new Rotation(new double[][] {
                                   {  0.539200,  0.737280,  0.407040 },
                                   {  0.184320, -0.574912,  0.797184 },
                                   {  0.821760, -0.354816, -0.445888 }
                                }, 1.0e-10),
                    0.36, 0.8, 0.288, 0.384);
  
      checkRotation(new Rotation(new double[][] {
                                   { -0.445888,  0.797184, -0.407040 },
                                   {  0.354816,  0.574912,  0.737280 },
                                   {  0.821760,  0.184320, -0.539200 }
                                 }, 1.0e-10),
                    0.384, 0.36, 0.8, 0.288);
  
      checkRotation(new Rotation(new double[][] {
                                   { -0.539200,  0.737280,  0.407040 },
                                   { -0.184320, -0.574912,  0.797184 },
                                   {  0.821760,  0.354816,  0.445888 }
                                 }, 1.0e-10),
                    0.288, 0.384, 0.36, 0.8);
  
      double[][] m1 = { { 0.0, 1.0, 0.0 },
                        { 0.0, 0.0, 1.0 },
                        { 1.0, 0.0, 0.0 } };
      Rotation r = new Rotation(m1, 1.0e-7);
      checkVector(r.applyTo(Vector3D.plusI), Vector3D.plusK);
      checkVector(r.applyTo(Vector3D.plusJ), Vector3D.plusI);
      checkVector(r.applyTo(Vector3D.plusK), Vector3D.plusJ);
  
      double[][] m2 = { { 0.83203, -0.55012, -0.07139 },
                        { 0.48293,  0.78164, -0.39474 },
                        { 0.27296,  0.29396,  0.91602 } };
      r = new Rotation(m2, 1.0e-12);
  
      double[][] m3 = r.getMatrix();
      double d00 = m2[0][0] - m3[0][0];
      double d01 = m2[0][1] - m3[0][1];
      double d02 = m2[0][2] - m3[0][2];
      double d10 = m2[1][0] - m3[1][0];
      double d11 = m2[1][1] - m3[1][1];
      double d12 = m2[1][2] - m3[1][2];
      double d20 = m2[2][0] - m3[2][0];
      double d21 = m2[2][1] - m3[2][1];
      double d22 = m2[2][2] - m3[2][2];
  
      assertTrue(Math.abs(d00) < 6.0e-6);
      assertTrue(Math.abs(d01) < 6.0e-6);
      assertTrue(Math.abs(d02) < 6.0e-6);
      assertTrue(Math.abs(d10) < 6.0e-6);
      assertTrue(Math.abs(d11) < 6.0e-6);
      assertTrue(Math.abs(d12) < 6.0e-6);
      assertTrue(Math.abs(d20) < 6.0e-6);
      assertTrue(Math.abs(d21) < 6.0e-6);
      assertTrue(Math.abs(d22) < 6.0e-6);
  
      assertTrue(Math.abs(d00) > 4.0e-7);
      assertTrue(Math.abs(d01) > 4.0e-7);
      assertTrue(Math.abs(d02) > 4.0e-7);
      assertTrue(Math.abs(d10) > 4.0e-7);
      assertTrue(Math.abs(d11) > 4.0e-7);
      assertTrue(Math.abs(d12) > 4.0e-7);
      assertTrue(Math.abs(d20) > 4.0e-7);
      assertTrue(Math.abs(d21) > 4.0e-7);
      assertTrue(Math.abs(d22) > 4.0e-7);
  
      for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
          double m3tm3 = m3[i][0] * m3[j][0]
                       + m3[i][1] * m3[j][1]
                       + m3[i][2] * m3[j][2];
          if (i == j) {
            assertTrue(Math.abs(m3tm3 - 1.0) < 1.0e-10);
          } else {
            assertTrue(Math.abs(m3tm3) < 1.0e-10);
          }
        }
      }
  
      checkVector(r.applyTo(Vector3D.plusI),
                  new Vector3D(m3[0][0], m3[1][0], m3[2][0]));
      checkVector(r.applyTo(Vector3D.plusJ),
                  new Vector3D(m3[0][1], m3[1][1], m3[2][1]));
      checkVector(r.applyTo(Vector3D.plusK),
                  new Vector3D(m3[0][2], m3[1][2], m3[2][2]));
  
      double[][] m4 = { { 1.0,  0.0,  0.0 },
                        { 0.0, -1.0,  0.0 },
                        { 0.0,  0.0, -1.0 } };
      r = new Rotation(m4, 1.0e-7);
      checkAngle(r.getAngle(), Math.PI);
  
      try {
        double[][] m5 = { { 0.0, 0.0, 1.0 },
                          { 0.0, 1.0, 0.0 },
                          { 1.0, 0.0, 0.0 } };
        r = new Rotation(m5, 1.0e-7);
        fail("got " + r + ", should have caught an exception");
      } catch (NotARotationMatrixException e) {
        // expected
      } catch (Exception e) {
        fail("wrong exception caught");
      }
  
    }
  
    public void testAngles()
      throws CardanEulerSingularityException {
  
      RotationOrder[] CardanOrders = {
        RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
        RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
      };
  
      for (int i = 0; i < CardanOrders.length; ++i) {
        for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
          for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
            for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
              Rotation r = new Rotation(CardanOrders[i], alpha1, alpha2, alpha3);
              double[] angles = r.getAngles(CardanOrders[i]);
              checkAngle(angles[0], alpha1);
              checkAngle(angles[1], alpha2);
              checkAngle(angles[2], alpha3);
            }
          }
        }
      }
  
      RotationOrder[] EulerOrders = {
              RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
              RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
            };
  
      for (int i = 0; i < EulerOrders.length; ++i) {
        for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
          for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
            for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
              Rotation r = new Rotation(EulerOrders[i],
                                        alpha1, alpha2, alpha3);
              double[] angles = r.getAngles(EulerOrders[i]);
              checkAngle(angles[0], alpha1);
              checkAngle(angles[1], alpha2);
              checkAngle(angles[2], alpha3);
            }
          }
        }
      }
  
    }
  
    public void testSingularities()
      throws CardanEulerSingularityException {
  
      RotationOrder[] CardanOrders = {
        RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
        RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
      };
  
      double[] singularCardanAngle = { Math.PI / 2, -Math.PI / 2 };
      for (int i = 0; i < CardanOrders.length; ++i) {
        for (int j = 0; j < singularCardanAngle.length; ++j) {
          Rotation r = new Rotation(CardanOrders[i], 0.1, singularCardanAngle[j], 0.3);
          try {
            r.getAngles(CardanOrders[i]);
            fail("an exception should have been caught");
          } catch (CardanEulerSingularityException cese) {
            // expected behavior
          } catch (Exception e) {
            fail("wrong exception caught: " + e.getMessage());
          }
        }
      }
  
      RotationOrder[] EulerOrders = {
              RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
              RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
            };
  
      double[] singularEulerAngle = { 0, Math.PI };
      for (int i = 0; i < EulerOrders.length; ++i) {
        for (int j = 0; j < singularEulerAngle.length; ++j) {
          Rotation r = new Rotation(EulerOrders[i], 0.1, singularEulerAngle[j], 0.3);
          try {
            r.getAngles(EulerOrders[i]);
            fail("an exception should have been caught");
          } catch (CardanEulerSingularityException cese) {
            // expected behavior
          } catch (Exception e) {
            fail("wrong exception caught: " + e.getMessage());
          }
        }
      }
  
  
    }
  
    public void testQuaternion() {
  
      Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7);
      double n = 23.5;
      Rotation r2 = new Rotation(n * r1.getQ0(), n * r1.getQ1(),
                                 n * r1.getQ2(), n * r1.getQ3(),
                                 true);
  