Rotation implements Serializable { public double[] getAngles(RotationOrder order) throws CardanEulerSingularityException { if (order == RotationOrder.XYZ) { Vector3D v1 = applyTo(Vector3D.PLUS_K); Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } return new double[] { FastMath.atan2(-(v1.getY()), v1.getZ()), FastMath.asin(v2.getZ()), FastMath.atan2(-(v2.getY()), v2.getX()) }; } else if (order == RotationOrder.XZY) { Vector3D v1 = applyTo(Vector3D.PLUS_J); Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } return new double[] { FastMath.atan2(v1.getZ(), v1.getY()), -FastMath.asin(v2.getY()), FastMath.atan2(v2.getZ(), v2.getX()) }; } else if (order == RotationOrder.YXZ) { Vector3D v1 = applyTo(Vector3D.PLUS_K); Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } return new double[] { FastMath.atan2(v1.getX(), v1.getZ()), -FastMath.asin(v2.getZ()), FastMath.atan2(v2.getX(), v2.getY()) }; } else if (order == RotationOrder.YZX) { Vector3D v1 = applyTo(Vector3D.PLUS_I); Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } return new double[] { FastMath.atan2(-(v1.getZ()), v1.getX()), FastMath.asin(v2.getX()), FastMath.atan2(-(v2.getZ()), v2.getY()) }; } else if (order == RotationOrder.ZXY) { Vector3D v1 = applyTo(Vector3D.PLUS_J); Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } return new double[] { FastMath.atan2(-(v1.getX()), v1.getY()), FastMath.asin(v2.getY()), FastMath.atan2(-(v2.getX()), v2.getZ()) }; } else if (order == RotationOrder.ZYX) { Vector3D v1 = applyTo(Vector3D.PLUS_I); Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { throw new CardanEulerSingularityException(true); } return new double[] { FastMath.atan2(v1.getY(), v1.getX()), -FastMath.asin(v2.getX()), FastMath.atan2(v2.getY(), v2.getZ()) }; } else if (order == RotationOrder.XYX) { Vector3D v1 = applyTo(Vector3D.PLUS_I); Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } return new double[] { FastMath.atan2(v1.getY(), -v1.getZ()), FastMath.acos(v2.getX()), FastMath.atan2(v2.getY(), v2.getZ()) }; } else if (order == RotationOrder.XZX) { Vector3D v1 = applyTo(Vector3D.PLUS_I); Vector3D v2 = applyInverseTo(Vector3D.PLUS_I); if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } return new double[] { FastMath.atan2(v1.getZ(), v1.getY()), FastMath.acos(v2.getX()), FastMath.atan2(v2.getZ(), -v2.getY()) }; } else if (order == RotationOrder.YXY) { Vector3D v1 = applyTo(Vector3D.PLUS_J); Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } return new double[] { FastMath.atan2(v1.getX(), v1.getZ()), FastMath.acos(v2.getY()), FastMath.atan2(v2.getX(), -v2.getZ()) }; } else if (order == RotationOrder.YZY) { Vector3D v1 = applyTo(Vector3D.PLUS_J); Vector3D v2 = applyInverseTo(Vector3D.PLUS_J); if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } return new double[] { FastMath.atan2(v1.getZ(), -v1.getX()), FastMath.acos(v2.getY()), FastMath.atan2(v2.getZ(), v2.getX()) }; } else if (order == RotationOrder.ZXZ) { Vector3D v1 = applyTo(Vector3D.PLUS_K); Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } return new double[] { FastMath.atan2(v1.getX(), -v1.getY()), FastMath.acos(v2.getZ()), FastMath.atan2(v2.getX(), v2.getY()) }; } else { Vector3D v1 = applyTo(Vector3D.PLUS_K); Vector3D v2 = applyInverseTo(Vector3D.PLUS_K); if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) { throw new CardanEulerSingularityException(false); } return new double[] { FastMath.atan2(v1.getY(), v1.getX()), FastMath.acos(v2.getZ()), FastMath.atan2(v2.getY(), -v2.getX()) }; } } Rotation(double q0, double q1, double q2, double q3,
                  boolean needsNormalization);  Rotation(Vector3D axis, double angle);  Rotation(double[][] m, double threshold);  Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2);  Rotation(Vector3D u, Vector3D v);  Rotation(RotationOrder order,
                  double alpha1, double alpha2, double alpha3); Rotation revert(); double getQ0(); double getQ1(); double getQ2(); double getQ3(); Vector3D getAxis(); double getAngle(); double[] getAngles(RotationOrder order); double[][] getMatrix(); Vector3D applyTo(Vector3D u); void applyTo(final double[] in, final double[] out); Vector3D applyInverseTo(Vector3D u); void applyInverseTo(final double[] in, final double[] out); Rotation applyTo(Rotation r); Rotation applyInverseTo(Rotation r); static double distance(Rotation r1, Rotation r2); static final Rotation IDENTITY; }
@Test 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); } } } } }
@Test public void testSingularities() { RotationOrder[] CardanOrders = { RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX }; double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.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]); Assert.fail("an exception should have been caught"); } catch (CardanEulerSingularityException cese) { } } } RotationOrder[] EulerOrders = { RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY, RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ }; double[] singularEulerAngle = { 0, FastMath.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]); Assert.fail("an exception should have been caught"); } catch (CardanEulerSingularityException cese) { } } } }