http://git-wip-us.apache.org/repos/asf/commons-math/blob/971a7786/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java ---------------------------------------------------------------------- diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java index 7945c7c..e772ecb 100644 --- a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/Rotation.java @@ -170,15 +170,29 @@ public class Rotation implements Serializable { * @param axis axis around which to rotate * @param angle rotation angle. * @exception MathIllegalArgumentException if the axis norm is zero + * @deprecated as of 3.6, replaced with {@link #Rotation(Vector3D, double, RotationConvention)} */ + @Deprecated public Rotation(Vector3D axis, double angle) throws MathIllegalArgumentException { + this(axis, angle, RotationConvention.VECTOR_OPERATOR); + } + + /** Build a rotation from an axis and an angle. + * @param axis axis around which to rotate + * @param angle rotation angle + * @param convention convention to use for the semantics of the angle + * @exception MathIllegalArgumentException if the axis norm is zero + * @since 3.6 + */ + public Rotation(final Vector3D axis, final double angle, final RotationConvention convention) + throws MathIllegalArgumentException { double norm = axis.getNorm(); if (norm == 0) { throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_AXIS); } - double halfAngle = -0.5 * angle; + double halfAngle = convention == RotationConvention.VECTOR_OPERATOR ? -0.5 * angle : +0.5 * angle; double coeff = FastMath.sin(halfAngle) / norm; q0 = FastMath.cos (halfAngle); @@ -250,7 +264,7 @@ public class Rotation implements Serializable { } - /** Build the rotation that transforms a pair of vector into another pair. + /** Build the rotation that transforms a pair of vectors into another pair. * <p>Except for possible scale factors, if the instance were applied to * the pair (u<sub>1</sub>, u<sub>2</sub>) it will produce the pair @@ -259,27 +273,27 @@ public class Rotation implements Serializable { * <p>If the angular separation between u<sub>1</sub> and u<sub>2</sub> is * not the same as the angular separation between v<sub>1</sub> and * v<sub>2</sub>, then a corrected v'<sub>2</sub> will be used rather than - * v<sub>2</sub>, the corrected vector will be in the (v<sub>1</sub>, - * v<sub>2</sub>) plane.</p> + * v<sub>2</sub>, the corrected vector will be in the (±v<sub>1</sub>, + * +v<sub>2</sub>) half-plane.</p> * @param u1 first vector of the origin pair * @param u2 second vector of the origin pair * @param v1 desired image of u1 by the rotation * @param v2 desired image of u2 by the rotation * @exception MathArithmeticException if the norm of one of the vectors is zero, - * or if one of the pair is degenerated (i.e. the vectors of the pair are colinear) + * or if one of the pair is degenerated (i.e. the vectors of the pair are collinear) */ public Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2) throws MathArithmeticException { // build orthonormalized base from u1, u2 - // this fails when vectors are null or colinear, which is forbidden to define a rotation + // this fails when vectors are null or collinear, which is forbidden to define a rotation final Vector3D u3 = u1.crossProduct(u2).normalize(); u2 = u3.crossProduct(u1).normalize(); u1 = u1.normalize(); // build an orthonormalized base from v1, v2 - // this fails when vectors are null or colinear, which is forbidden to define a rotation + // this fails when vectors are null or collinear, which is forbidden to define a rotation final Vector3D v3 = v1.crossProduct(v2).normalize(); v2 = v3.crossProduct(v1).normalize(); v1 = v1.normalize(); @@ -317,7 +331,7 @@ public class Rotation implements Serializable { * applied to the vector u it will produce the vector v. There is an * infinite number of such rotations, this constructor choose the * one with the smallest associated angle (i.e. the one whose axis - * is orthogonal to the (u, v) plane). If u and v are colinear, an + * is orthogonal to the (u, v) plane). If u and v are collinear, an * arbitrary rotation axis is chosen.</p> * @param u origin vector @@ -372,13 +386,44 @@ public class Rotation implements Serializable { * @param alpha1 angle of the first elementary rotation * @param alpha2 angle of the second elementary rotation * @param alpha3 angle of the third elementary rotation + * @deprecated as of 3.6, replaced with {@link + * #Rotation(RotationOrder, RotationConvention, double, double, double)} */ + @Deprecated public Rotation(RotationOrder order, double alpha1, double alpha2, double alpha3) { - Rotation r1 = new Rotation(order.getA1(), alpha1); - Rotation r2 = new Rotation(order.getA2(), alpha2); - Rotation r3 = new Rotation(order.getA3(), alpha3); - Rotation composed = r1.applyTo(r2.applyTo(r3)); + this(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3); + } + + /** Build a rotation from three Cardan or Euler elementary rotations. + + * <p>Cardan rotations are three successive rotations around the + * canonical axes X, Y and Z, each axis being used once. There are + * 6 such sets of rotations (XYZ, XZY, YXZ, YZX, ZXY and ZYX). Euler + * rotations are three successive rotations around the canonical + * axes X, Y and Z, the first and last rotations being around the + * same axis. There are 6 such sets of rotations (XYX, XZX, YXY, + * YZY, ZXZ and ZYZ), the most popular one being ZXZ.</p> + * <p>Beware that many people routinely use the term Euler angles even + * for what really are Cardan angles (this confusion is especially + * widespread in the aerospace business where Roll, Pitch and Yaw angles + * are often wrongly tagged as Euler angles).</p> + + * @param order order of rotations to use + * @param convention convention to use for the semantics of the angle + * @param alpha1 angle of the first elementary rotation + * @param alpha2 angle of the second elementary rotation + * @param alpha3 angle of the third elementary rotation + * @since 3.6 + */ + public Rotation(RotationOrder order, RotationConvention convention, + double alpha1, double alpha2, double alpha3) { + Rotation r1 = new Rotation(order.getA1(), alpha1, convention); + Rotation r2 = new Rotation(order.getA2(), alpha2, convention); + Rotation r3 = new Rotation(order.getA3(), alpha3, convention); + Rotation composed = convention == RotationConvention.FRAME_TRANSFORM ? + r3.applyTo(r2.applyTo(r1)) : + r1.applyTo(r2.applyTo(r3)); q0 = composed.q0; q1 = composed.q1; q2 = composed.q2; @@ -487,18 +532,38 @@ public class Rotation implements Serializable { /** Get the normalized axis of the rotation. * @return normalized axis of the rotation - * @see #Rotation(Vector3D, double) + * @see #Rotation(Vector3D, double, RotationConvention) + * @deprecated as of 3.6, replaced with {@link #getAxis(RotationConvention)} */ + @Deprecated public Vector3D getAxis() { - double squaredSine = q1 * q1 + q2 * q2 + q3 * q3; + return getAxis(RotationConvention.VECTOR_OPERATOR); + } + + /** Get the normalized axis of the rotation. + * <p> + * Note that as {@link #getAngle()} always returns an angle + * between 0 and π, changing the convention changes the + * direction of the axis, not the sign of the angle. + * </p> + * @param convention convention to use for the semantics of the angle + * @return normalized axis of the rotation + * @see #Rotation(Vector3D, double, RotationConvention) + * @since 3.6 + */ + public Vector3D getAxis(final RotationConvention convention) { + final double squaredSine = q1 * q1 + q2 * q2 + q3 * q3; if (squaredSine == 0) { - return new Vector3D(1, 0, 0); - } else if (q0 < 0) { - double inverse = 1 / FastMath.sqrt(squaredSine); - return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); + return convention == RotationConvention.VECTOR_OPERATOR ? Vector3D.PLUS_I : Vector3D.MINUS_I; + } else { + final double sgn = convention == RotationConvention.VECTOR_OPERATOR ? +1 : -1; + if (q0 < 0) { + final double inverse = sgn / FastMath.sqrt(squaredSine); + return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); + } + final double inverse = -sgn / FastMath.sqrt(squaredSine); + return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); } - double inverse = -1 / FastMath.sqrt(squaredSine); - return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse); } /** Get the angle of the rotation. @@ -548,227 +613,491 @@ public class Rotation implements Serializable { * @return an array of three angles, in the order specified by the set * @exception CardanEulerSingularityException if the rotation is * singular with respect to the angles set specified + * @deprecated as of 3.6, replaced with {@link #getAngles(RotationOrder, RotationConvention)} */ + @Deprecated public double[] getAngles(RotationOrder order) - throws CardanEulerSingularityException { - - if (order == RotationOrder.XYZ) { - - // r (Vector3D.plusK) coordinates are : - // sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi) - // (-r) (Vector3D.plusI) coordinates are : - // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta) - // and we can choose to have theta in the interval [-PI/2 ; +PI/2] - 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) { - - // r (Vector3D.plusJ) coordinates are : - // -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi) - // (-r) (Vector3D.plusI) coordinates are : - // cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi) - // and we can choose to have psi in the interval [-PI/2 ; +PI/2] - 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) { - - // r (Vector3D.plusK) coordinates are : - // cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta) - // (-r) (Vector3D.plusJ) coordinates are : - // sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi) - // and we can choose to have phi in the interval [-PI/2 ; +PI/2] - 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) { - - // r (Vector3D.plusI) coordinates are : - // cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta) - // (-r) (Vector3D.plusJ) coordinates are : - // sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi) - // and we can choose to have psi in the interval [-PI/2 ; +PI/2] - 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) { - - // r (Vector3D.plusJ) coordinates are : - // -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi) - // (-r) (Vector3D.plusK) coordinates are : - // -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi) - // and we can choose to have phi in the interval [-PI/2 ; +PI/2] - 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) { - - // r (Vector3D.plusI) coordinates are : - // cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta) - // (-r) (Vector3D.plusK) coordinates are : - // -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta) - // and we can choose to have theta in the interval [-PI/2 ; +PI/2] - 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()) - }; + throws CardanEulerSingularityException { + return getAngles(order, RotationConvention.VECTOR_OPERATOR); + } - } else if (order == RotationOrder.XYX) { - - // r (Vector3D.plusI) coordinates are : - // cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta) - // (-r) (Vector3D.plusI) coordinates are : - // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2) - // and we can choose to have theta in the interval [0 ; PI] - 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()) - }; + /** Get the Cardan or Euler angles corresponding to the instance. - } else if (order == RotationOrder.XZX) { - - // r (Vector3D.plusI) coordinates are : - // cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi) - // (-r) (Vector3D.plusI) coordinates are : - // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2) - // and we can choose to have psi in the interval [0 ; PI] - 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()) - }; + * <p>The equations show that each rotation can be defined by two + * different values of the Cardan or Euler angles set. For example + * if Cardan angles are used, the rotation defined by the angles + * a<sub>1</sub>, a<sub>2</sub> and a<sub>3</sub> is the same as + * the rotation defined by the angles π + a<sub>1</sub>, π + * - a<sub>2</sub> and π + a<sub>3</sub>. This method implements + * the following arbitrary choices:</p> + * <ul> + * <li>for Cardan angles, the chosen set is the one for which the + * second angle is between -π/2 and π/2 (i.e its cosine is + * positive),</li> + * <li>for Euler angles, the chosen set is the one for which the + * second angle is between 0 and π (i.e its sine is positive).</li> + * </ul> - } else if (order == RotationOrder.YXY) { - - // r (Vector3D.plusJ) coordinates are : - // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) - // (-r) (Vector3D.plusJ) coordinates are : - // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) - // and we can choose to have phi in the interval [0 ; PI] - 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()) - }; + * <p>Cardan and Euler angle have a very disappointing drawback: all + * of them have singularities. This means that if the instance is + * too close to the singularities corresponding to the given + * rotation order, it will be impossible to retrieve the angles. For + * Cardan angles, this is often called gimbal lock. There is + * <em>nothing</em> to do to prevent this, it is an intrinsic problem + * with Cardan and Euler representation (but not a problem with the + * rotation itself, which is perfectly well defined). For Cardan + * angles, singularities occur when the second angle is close to + * -π/2 or +π/2, for Euler angle singularities occur when the + * second angle is close to 0 or π, this implies that the identity + * rotation is always singular for Euler angles!</p> - } else if (order == RotationOrder.YZY) { - - // r (Vector3D.plusJ) coordinates are : - // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) - // (-r) (Vector3D.plusJ) coordinates are : - // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) - // and we can choose to have psi in the interval [0 ; PI] - 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()) - }; + * @param order rotation order to use + * @param convention convention to use for the semantics of the angle + * @return an array of three angles, in the order specified by the set + * @exception CardanEulerSingularityException if the rotation is + * singular with respect to the angles set specified + * @since 3.6 + */ + public double[] getAngles(RotationOrder order, RotationConvention convention) + throws CardanEulerSingularityException { + + if (convention == RotationConvention.VECTOR_OPERATOR) { + if (order == RotationOrder.XYZ) { + + // r (Vector3D.plusK) coordinates are : + // sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta) + // and we can choose to have theta in the interval [-PI/2 ; +PI/2] + 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) { + + // r (Vector3D.plusJ) coordinates are : + // -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi) + // and we can choose to have psi in the interval [-PI/2 ; +PI/2] + 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) { + + // r (Vector3D.plusK) coordinates are : + // cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta) + // (-r) (Vector3D.plusJ) coordinates are : + // sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi) + // and we can choose to have phi in the interval [-PI/2 ; +PI/2] + 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) { + + // r (Vector3D.plusI) coordinates are : + // cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta) + // (-r) (Vector3D.plusJ) coordinates are : + // sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi) + // and we can choose to have psi in the interval [-PI/2 ; +PI/2] + 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) { + + // r (Vector3D.plusJ) coordinates are : + // -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi) + // (-r) (Vector3D.plusK) coordinates are : + // -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi) + // and we can choose to have phi in the interval [-PI/2 ; +PI/2] + 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) { + + // r (Vector3D.plusI) coordinates are : + // cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta) + // (-r) (Vector3D.plusK) coordinates are : + // -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta) + // and we can choose to have theta in the interval [-PI/2 ; +PI/2] + 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) { + + // r (Vector3D.plusI) coordinates are : + // cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta) + // (-r) (Vector3D.plusI) coordinates are : + // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2) + // and we can choose to have theta in the interval [0 ; PI] + 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) { + + // r (Vector3D.plusI) coordinates are : + // cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2) + // and we can choose to have psi in the interval [0 ; PI] + 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) { + + // r (Vector3D.plusJ) coordinates are : + // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) + // (-r) (Vector3D.plusJ) coordinates are : + // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) + // and we can choose to have phi in the interval [0 ; PI] + 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) { + + // r (Vector3D.plusJ) coordinates are : + // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) + // (-r) (Vector3D.plusJ) coordinates are : + // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) + // and we can choose to have psi in the interval [0 ; PI] + 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) { + + // r (Vector3D.plusK) coordinates are : + // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) + // (-r) (Vector3D.plusK) coordinates are : + // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) + // and we can choose to have phi in the interval [0 ; PI] + 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 { // last possibility is ZYZ + + // r (Vector3D.plusK) coordinates are : + // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) + // (-r) (Vector3D.plusK) coordinates are : + // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) + // and we can choose to have theta in the interval [0 ; PI] + 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()) + }; - } else if (order == RotationOrder.ZXZ) { - - // r (Vector3D.plusK) coordinates are : - // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) - // (-r) (Vector3D.plusK) coordinates are : - // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) - // and we can choose to have phi in the interval [0 ; PI] - 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 { + if (order == RotationOrder.XYZ) { + + // r (Vector3D.plusI) coordinates are : + // cos (theta) cos (psi), -cos (theta) sin (psi), sin (theta) + // (-r) (Vector3D.plusK) coordinates are : + // sin (theta), -sin (phi) cos (theta), cos (phi) cos (theta) + // and we can choose to have theta in the interval [-PI/2 ; +PI/2] + 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(-v2.getY(), v2.getZ()), + FastMath.asin(v2.getX()), + FastMath.atan2(-v1.getY(), v1.getX()) + }; + + } else if (order == RotationOrder.XZY) { + + // r (Vector3D.plusI) coordinates are : + // cos (psi) cos (theta), -sin (psi), cos (psi) sin (theta) + // (-r) (Vector3D.plusJ) coordinates are : + // -sin (psi), cos (phi) cos (psi), sin (phi) cos (psi) + // and we can choose to have psi in the interval [-PI/2 ; +PI/2] + 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(v2.getZ(), v2.getY()), + -FastMath.asin(v2.getX()), + FastMath.atan2(v1.getZ(), v1.getX()) + }; + + } else if (order == RotationOrder.YXZ) { + + // r (Vector3D.plusJ) coordinates are : + // cos (phi) sin (psi), cos (phi) cos (psi), -sin (phi) + // (-r) (Vector3D.plusK) coordinates are : + // sin (theta) cos (phi), -sin (phi), cos (theta) cos (phi) + // and we can choose to have phi in the interval [-PI/2 ; +PI/2] + 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(v2.getX(), v2.getZ()), + -FastMath.asin(v2.getY()), + FastMath.atan2(v1.getX(), v1.getY()) + }; + + } else if (order == RotationOrder.YZX) { + + // r (Vector3D.plusJ) coordinates are : + // sin (psi), cos (psi) cos (phi), -cos (psi) sin (phi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (theta) cos (psi), sin (psi), -sin (theta) cos (psi) + // and we can choose to have psi in the interval [-PI/2 ; +PI/2] + 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(-v2.getZ(), v2.getX()), + FastMath.asin(v2.getY()), + FastMath.atan2(-v1.getZ(), v1.getY()) + }; + + } else if (order == RotationOrder.ZXY) { + + // r (Vector3D.plusK) coordinates are : + // -cos (phi) sin (theta), sin (phi), cos (phi) cos (theta) + // (-r) (Vector3D.plusJ) coordinates are : + // -sin (psi) cos (phi), cos (psi) cos (phi), sin (phi) + // and we can choose to have phi in the interval [-PI/2 ; +PI/2] + 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(-v2.getX(), v2.getY()), + FastMath.asin(v2.getZ()), + FastMath.atan2(-v1.getX(), v1.getZ()) + }; + + } else if (order == RotationOrder.ZYX) { + + // r (Vector3D.plusK) coordinates are : + // -sin (theta), cos (theta) sin (phi), cos (theta) cos (phi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (psi) cos (theta), sin (psi) cos (theta), -sin (theta) + // and we can choose to have theta in the interval [-PI/2 ; +PI/2] + 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(v2.getY(), v2.getX()), + -FastMath.asin(v2.getZ()), + FastMath.atan2(v1.getY(), v1.getZ()) + }; + + } else if (order == RotationOrder.XYX) { + + // r (Vector3D.plusI) coordinates are : + // cos (theta), sin (phi2) sin (theta), cos (phi2) sin (theta) + // (-r) (Vector3D.plusI) coordinates are : + // cos (theta), sin (theta) sin (phi1), -sin (theta) cos (phi1) + // and we can choose to have theta in the interval [0 ; PI] + 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(v2.getY(), -v2.getZ()), + FastMath.acos(v2.getX()), + FastMath.atan2(v1.getY(), v1.getZ()) + }; + + } else if (order == RotationOrder.XZX) { + + // r (Vector3D.plusI) coordinates are : + // cos (psi), -cos (phi2) sin (psi), sin (phi2) sin (psi) + // (-r) (Vector3D.plusI) coordinates are : + // cos (psi), sin (psi) cos (phi1), sin (psi) sin (phi1) + // and we can choose to have psi in the interval [0 ; PI] + 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(v2.getZ(), v2.getY()), + FastMath.acos(v2.getX()), + FastMath.atan2(v1.getZ(), -v1.getY()) + }; + + } else if (order == RotationOrder.YXY) { + + // r (Vector3D.plusJ) coordinates are : + // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) + // (-r) (Vector3D.plusJ) coordinates are : + // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) + // and we can choose to have phi in the interval [0 ; PI] + 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(v2.getX(), v2.getZ()), + FastMath.acos(v2.getY()), + FastMath.atan2(v1.getX(), -v1.getZ()) + }; + + } else if (order == RotationOrder.YZY) { + + // r (Vector3D.plusJ) coordinates are : + // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) + // (-r) (Vector3D.plusJ) coordinates are : + // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) + // and we can choose to have psi in the interval [0 ; PI] + 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(v2.getZ(), -v2.getX()), + FastMath.acos(v2.getY()), + FastMath.atan2(v1.getZ(), v1.getX()) + }; + + } else if (order == RotationOrder.ZXZ) { + + // r (Vector3D.plusK) coordinates are : + // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) + // (-r) (Vector3D.plusK) coordinates are : + // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) + // and we can choose to have phi in the interval [0 ; PI] + 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(v2.getX(), -v2.getY()), + FastMath.acos(v2.getZ()), + FastMath.atan2(v1.getX(), v1.getY()) + }; + + } else { // last possibility is ZYZ + + // r (Vector3D.plusK) coordinates are : + // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) + // (-r) (Vector3D.plusK) coordinates are : + // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) + // and we can choose to have theta in the interval [0 ; PI] + 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(v2.getY(), v2.getX()), + FastMath.acos(v2.getZ()), + FastMath.atan2(v1.getY(), -v1.getX()) + }; - } else { // last possibility is ZYZ - - // r (Vector3D.plusK) coordinates are : - // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) - // (-r) (Vector3D.plusK) coordinates are : - // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) - // and we can choose to have theta in the interval [0 ; PI] - 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()) - }; - - } }
http://git-wip-us.apache.org/repos/asf/commons-math/blob/971a7786/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationConvention.java ---------------------------------------------------------------------- diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationConvention.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationConvention.java new file mode 100644 index 0000000..6111ac3 --- /dev/null +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/RotationConvention.java @@ -0,0 +1,79 @@ +/* + * 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.math3.geometry.euclidean.threed; + +/** + * This enumerates is used to differentiate the semantics of a rotation. + * @see Rotation + * @since 3.6 + */ +public enum RotationConvention { + + /** Constant for rotation that have the semantics of a vector operator. + * <p> + * According to this convention, the rotation moves vectors with respect + * to a fixed reference frame. + * </p> + * <p> + * This means that if we define rotation r is a 90 degrees rotation around + * the Z axis, the image of vector {@link Vector3D#PLUS_I} would be + * {@link Vector3D#PLUS_J}, the image of vector {@link Vector3D#PLUS_J} + * would be {@link Vector3D#MINUS_I}, the image of vector {@link Vector3D#PLUS_K} + * would be {@link Vector3D#PLUS_K}, and the image of vector with coordinates (1, 2, 3) + * would be vector (-2, 1, 3). This means that the vector rotates counterclockwise. + * </p> + * <p> + * This convention was the only one supported by Apache Commons Math up to version 3.5. + * </p> + * <p> + * The difference with {@link #FRAME_TRANSFORM} is only the semantics of the sign + * of the angle. It is always possible to create or use a rotation using either + * convention to really represent a rotation that would have been best created or + * used with the other convention, by changing accordingly the sign of the + * rotation angle. This is how things were done up to version 3.5. + * </p> + */ + VECTOR_OPERATOR, + + /** Constant for rotation that have the semantics of a frame conversion. + * <p> + * According to this convention, the rotation considered vectors to be fixed, + * but their coordinates change as they are converted from an initial frame to + * a destination frame rotated with respect to the initial frame. + * </p> + * <p> + * This means that if we define rotation r is a 90 degrees rotation around + * the Z axis, the image of vector {@link Vector3D#PLUS_I} would be + * {@link Vector3D#MINUS_J}, the image of vector {@link Vector3D#PLUS_J} + * would be {@link Vector3D#PLUS_I}, the image of vector {@link Vector3D#PLUS_K} + * would be {@link Vector3D#PLUS_K}, and the image of vector with coordinates (1, 2, 3) + * would be vector (2, -1, 3). This means that the coordinates of the vector rotates + * clockwise, because they are expressed with respect to a destination frame that is rotated + * counterclockwise. + * </p> + * <p> + * The difference with {@link #VECTOR_OPERATOR} is only the semantics of the sign + * of the angle. It is always possible to create or use a rotation using either + * convention to really represent a rotation that would have been best created or + * used with the other convention, by changing accordingly the sign of the + * rotation angle. This is how things were done up to version 3.5. + * </p> + */ + FRAME_TRANSFORM; + +} http://git-wip-us.apache.org/repos/asf/commons-math/blob/971a7786/src/main/java/org/apache/commons/math3/geometry/spherical/twod/SphericalPolygonsSet.java ---------------------------------------------------------------------- diff --git a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/SphericalPolygonsSet.java b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/SphericalPolygonsSet.java index 5fdb1c3..8e41659 100644 --- a/src/main/java/org/apache/commons/math3/geometry/spherical/twod/SphericalPolygonsSet.java +++ b/src/main/java/org/apache/commons/math3/geometry/spherical/twod/SphericalPolygonsSet.java @@ -27,6 +27,7 @@ import org.apache.commons.math3.geometry.enclosing.EnclosingBall; import org.apache.commons.math3.geometry.enclosing.WelzlEncloser; import org.apache.commons.math3.geometry.euclidean.threed.Euclidean3D; import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; import org.apache.commons.math3.geometry.euclidean.threed.SphereGenerator; import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import org.apache.commons.math3.geometry.partitioning.AbstractRegion; @@ -161,10 +162,11 @@ public class SphericalPolygonsSet extends AbstractRegion<Sphere2D, Sphere1D> { private static S2Point[] createRegularPolygonVertices(final Vector3D center, final Vector3D meridian, final double outsideRadius, final int n) { final S2Point[] array = new S2Point[n]; - final Rotation r0 = new Rotation(Vector3D.crossProduct(center, meridian), outsideRadius); + final Rotation r0 = new Rotation(Vector3D.crossProduct(center, meridian), + outsideRadius, RotationConvention.VECTOR_OPERATOR); array[0] = new S2Point(r0.applyTo(center)); - final Rotation r = new Rotation(center, MathUtils.TWO_PI / n); + final Rotation r = new Rotation(center, MathUtils.TWO_PI / n, RotationConvention.VECTOR_OPERATOR); for (int i = 1; i < n; ++i) { array[i] = new S2Point(r.applyTo(array[i - 1].getVector())); } http://git-wip-us.apache.org/repos/asf/commons-math/blob/971a7786/src/site/xdoc/userguide/geometry.xml ---------------------------------------------------------------------- diff --git a/src/site/xdoc/userguide/geometry.xml b/src/site/xdoc/userguide/geometry.xml index 213d74e..870f635 100644 --- a/src/site/xdoc/userguide/geometry.xml +++ b/src/site/xdoc/userguide/geometry.xml @@ -129,7 +129,7 @@ matrix into a set of Cardan angles can be done using the following single line of code: </p> - <source>double[] angles = new Rotation(matrix, 1.0e-10).getAngles(RotationOrder.XYZ);</source> + <source>double[] angles = new Rotation(matrix, 1.0e-10).getAngles(RotationOrder.XYZ, RotationConvention.FRAME_TRANSFORM);</source> <p> Focus is oriented on what a rotation <em>does</em> rather than on its underlying representation. Once it has been built, and regardless of @@ -157,6 +157,18 @@ observatory location and the Earth rotation. </p> <p> + In order to support naturally both views, the enumerate RotationConvention + has been introduced in version 3.6. This enumerate can take two values: + <code>VECTOR_OPERATOR</code> or <code>FRAME_TRANSFORM</code>. This + enumerate must be passed as an argument to the few methods that depend + on an interpretation of the semantics of the angle/axis. The value + <code>VECTOR_OPERATOR</code> corresponds to rotations that are + considered to move vectors within a fixed frame. The value + <code>FRAME_TRANSFORM</code> corresponds to rotations that are + considered to represent frame rotations, so fixed vectors coordinates + change as their reference frame changes. + </p> + <p> These examples show that a rotation means what the user wants it to mean, so this class does not push the user towards one specific definition and hence does not provide methods like http://git-wip-us.apache.org/repos/asf/commons-math/blob/971a7786/src/test/java/org/apache/commons/math3/complex/QuaternionTest.java ---------------------------------------------------------------------- diff --git a/src/test/java/org/apache/commons/math3/complex/QuaternionTest.java b/src/test/java/org/apache/commons/math3/complex/QuaternionTest.java index 0f9cf3a..6741c31 100644 --- a/src/test/java/org/apache/commons/math3/complex/QuaternionTest.java +++ b/src/test/java/org/apache/commons/math3/complex/QuaternionTest.java @@ -21,6 +21,7 @@ import org.apache.commons.math3.complex.Quaternion; import org.apache.commons.math3.exception.DimensionMismatchException; import org.apache.commons.math3.exception.ZeroException; import org.apache.commons.math3.geometry.euclidean.threed.Rotation; +import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; import org.apache.commons.math3.geometry.euclidean.threed.Vector3D; import org.apache.commons.math3.util.FastMath; import org.junit.Test; @@ -412,9 +413,15 @@ public class QuaternionTest { final Rotation rotP = new Rotation(qP.getQ0(), qP.getQ1(), qP.getQ2(), qP.getQ3(), true); Assert.assertEquals(rot.getAngle(), rotP.getAngle(), COMPARISON_EPS); - Assert.assertEquals(rot.getAxis().getX(), rot.getAxis().getX(), COMPARISON_EPS); - Assert.assertEquals(rot.getAxis().getY(), rot.getAxis().getY(), COMPARISON_EPS); - Assert.assertEquals(rot.getAxis().getZ(), rot.getAxis().getZ(), COMPARISON_EPS); + Assert.assertEquals(rot.getAxis(RotationConvention.VECTOR_OPERATOR).getX(), + rot.getAxis(RotationConvention.VECTOR_OPERATOR).getX(), + COMPARISON_EPS); + Assert.assertEquals(rot.getAxis(RotationConvention.VECTOR_OPERATOR).getY(), + rot.getAxis(RotationConvention.VECTOR_OPERATOR).getY(), + COMPARISON_EPS); + Assert.assertEquals(rot.getAxis(RotationConvention.VECTOR_OPERATOR).getZ(), + rot.getAxis(RotationConvention.VECTOR_OPERATOR).getZ(), + COMPARISON_EPS); } } http://git-wip-us.apache.org/repos/asf/commons-math/blob/971a7786/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDSTest.java ---------------------------------------------------------------------- diff --git a/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDSTest.java b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDSTest.java index bcbe91e..d845b7d 100644 --- a/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDSTest.java +++ b/src/test/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotationDSTest.java @@ -56,7 +56,8 @@ public class FieldRotationDSTest { } @Test - public void testAxisAngle() throws MathIllegalArgumentException { + @Deprecated + public void testAxisAngleDeprecated() throws MathIllegalArgumentException { FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(10, 10, 10), createAngle(2 * FastMath.PI / 3)); checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 1, 0)); @@ -85,6 +86,88 @@ public class FieldRotationDSTest { } @Test + public void testAxisAngleVectorOperator() throws MathIllegalArgumentException { + + FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(10, 10, 10), + createAngle(2 * FastMath.PI / 3) , + RotationConvention.VECTOR_OPERATOR); + checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 1, 0)); + checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 0, 1)); + checkVector(r.applyTo(createVector(0, 0, 1)), createVector(1, 0, 0)); + double s = 1 / FastMath.sqrt(3); + checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector( s, s, s)); + checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-s, -s, -s)); + checkAngle(r.getAngle(), 2 * FastMath.PI / 3); + + try { + new FieldRotation<DerivativeStructure>(createAxis(0, 0, 0), + createAngle(2 * FastMath.PI / 3), + RotationConvention.VECTOR_OPERATOR); + Assert.fail("an exception should have been thrown"); + } catch (MathIllegalArgumentException e) { + } + + r = new FieldRotation<DerivativeStructure>(createAxis(0, 0, 1), + createAngle(1.5 * FastMath.PI), + RotationConvention.VECTOR_OPERATOR); + checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, 0, -1)); + checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, 0, +1)); + checkAngle(r.getAngle(), 0.5 * FastMath.PI); + + r = new FieldRotation<DerivativeStructure>(createAxis(0, 1, 0), + createAngle(FastMath.PI), + RotationConvention.VECTOR_OPERATOR); + checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, +1, 0)); + checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, -1, 0)); + checkAngle(r.getAngle(), FastMath.PI); + + checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.VECTOR_OPERATOR), createVector(+1, 0, 0)); + checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-1, 0, 0)); + + } + + @Test + public void testAxisAngleFrameTransform() throws MathIllegalArgumentException { + + FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(10, 10, 10), + createAngle(2 * FastMath.PI / 3) , + RotationConvention.FRAME_TRANSFORM); + checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1)); + checkVector(r.applyTo(createVector(0, 1, 0)), createVector(1, 0, 0)); + checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 1, 0)); + double s = 1 / FastMath.sqrt(3); + checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector( s, s, s)); + checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(-s, -s, -s)); + checkAngle(r.getAngle(), 2 * FastMath.PI / 3); + + try { + new FieldRotation<DerivativeStructure>(createAxis(0, 0, 0), + createAngle(2 * FastMath.PI / 3), + RotationConvention.FRAME_TRANSFORM); + Assert.fail("an exception should have been thrown"); + } catch (MathIllegalArgumentException e) { + } + + r = new FieldRotation<DerivativeStructure>(createAxis(0, 0, 1), + createAngle(1.5 * FastMath.PI), + RotationConvention.FRAME_TRANSFORM); + checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, 0, -1)); + checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, 0, +1)); + checkAngle(r.getAngle(), 0.5 * FastMath.PI); + + r = new FieldRotation<DerivativeStructure>(createAxis(0, 1, 0), + createAngle(FastMath.PI), + RotationConvention.FRAME_TRANSFORM); + checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, +1, 0)); + checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, -1, 0)); + checkAngle(r.getAngle(), FastMath.PI); + + checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-1, 0, 0)); + checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.VECTOR_OPERATOR), createVector(+1, 0, 0)); + + } + + @Test public void testRevert() { double a = 0.001; double b = 0.36; @@ -150,7 +233,10 @@ public class FieldRotationDSTest { Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15); Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15); Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15); - Assert.assertEquals(-1, FieldVector3D.dotProduct(r.getAxis(), reverted.getAxis()).getReal(), 1.0e-15); + Assert.assertEquals(-1, + FieldVector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR), + reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(), + 1.0e-15); } @Test @@ -184,7 +270,7 @@ public class FieldRotationDSTest { checkVector(r.applyTo(createVector(0, 1, 0)), createVector(-1, 0, 0)); r = new FieldRotation<DerivativeStructure>(u1, u2, u1.negate(), u2.negate()); - FieldVector3D<DerivativeStructure> axis = r.getAxis(); + FieldVector3D<DerivativeStructure> axis = r.getAxis(RotationConvention.VECTOR_OPERATOR); if (FieldVector3D.dotProduct(axis, createVector(0, 0, 1)).getReal() > 0) { checkVector(axis, createVector(0, 0, 1)); } else { @@ -359,7 +445,8 @@ public class FieldRotationDSTest { } @Test - public void testAngles() + @Deprecated + public void testAnglesDeprecated() throws CardanEulerSingularityException { RotationOrder[] CardanOrders = { @@ -409,57 +496,120 @@ public class FieldRotationDSTest { } @Test - public void testSingularities() { - - RotationOrder[] CardanOrders = { - RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ, - RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX - }; + public void testAngles() + throws CardanEulerSingularityException { + + for (RotationConvention convention : RotationConvention.values()) { + 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) { + FieldRotation<DerivativeStructure> r = + new FieldRotation<DerivativeStructure>(CardanOrders[i], + convention, + new DerivativeStructure(3, 1, 0, alpha1), + new DerivativeStructure(3, 1, 1, alpha2), + new DerivativeStructure(3, 1, 2, alpha3)); + DerivativeStructure[] angles = r.getAngles(CardanOrders[i], convention); + checkAngle(angles[0], alpha1); + checkAngle(angles[1], alpha2); + checkAngle(angles[2], alpha3); + } + } + } + } - double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.PI / 2 }; - for (int i = 0; i < CardanOrders.length; ++i) { - for (int j = 0; j < singularCardanAngle.length; ++j) { - FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(CardanOrders[i], - new DerivativeStructure(3, 1, 0, 0.1), - new DerivativeStructure(3, 1, 1, singularCardanAngle[j]), - new DerivativeStructure(3, 1, 2, 0.3)); - try { - r.getAngles(CardanOrders[i]); - Assert.fail("an exception should have been caught"); - } catch (CardanEulerSingularityException cese) { - // expected behavior + 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) { + FieldRotation<DerivativeStructure> r = + new FieldRotation<DerivativeStructure>(EulerOrders[i], + convention, + new DerivativeStructure(3, 1, 0, alpha1), + new DerivativeStructure(3, 1, 1, alpha2), + new DerivativeStructure(3, 1, 2, alpha3)); + DerivativeStructure[] angles = r.getAngles(EulerOrders[i], convention); + 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 - }; + } - double[] singularEulerAngle = { 0, FastMath.PI }; - for (int i = 0; i < EulerOrders.length; ++i) { - for (int j = 0; j < singularEulerAngle.length; ++j) { - FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(EulerOrders[i], - new DerivativeStructure(3, 1, 0, 0.1), - new DerivativeStructure(3, 1, 1, singularEulerAngle[j]), - new DerivativeStructure(3, 1, 2, 0.3)); - try { - r.getAngles(EulerOrders[i]); - Assert.fail("an exception should have been caught"); - } catch (CardanEulerSingularityException cese) { - // expected behavior + @Test + public void testSingularities() { + + for (RotationConvention convention : RotationConvention.values()) { + 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) { + FieldRotation<DerivativeStructure> r = + new FieldRotation<DerivativeStructure>(CardanOrders[i], + convention, + new DerivativeStructure(3, 1, 0, 0.1), + new DerivativeStructure(3, 1, 1, singularCardanAngle[j]), + new DerivativeStructure(3, 1, 2, 0.3)); + try { + r.getAngles(CardanOrders[i], convention); + Assert.fail("an exception should have been caught"); + } catch (CardanEulerSingularityException cese) { + // expected behavior + } } } - } + 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) { + FieldRotation<DerivativeStructure> r = + new FieldRotation<DerivativeStructure>(EulerOrders[i], + convention, + new DerivativeStructure(3, 1, 0, 0.1), + new DerivativeStructure(3, 1, 1, singularEulerAngle[j]), + new DerivativeStructure(3, 1, 2, 0.3)); + try { + r.getAngles(EulerOrders[i], convention); + Assert.fail("an exception should have been caught"); + } catch (CardanEulerSingularityException cese) { + // expected behavior + } + } + } + } } @Test public void testQuaternion() throws MathIllegalArgumentException { - FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7)); + FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), + createAngle(1.7), + RotationConvention.VECTOR_OPERATOR); double n = 23.5; FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(r1.getQ0().multiply(n), r1.getQ1().multiply(n), r1.getQ2().multiply(n), r1.getQ3().multiply(n), @@ -487,8 +637,12 @@ public class FieldRotationDSTest { @Test public void testCompose() throws MathIllegalArgumentException { - FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7)); - FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2), createAngle(0.3)); + FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), + createAngle(1.7), + RotationConvention.VECTOR_OPERATOR); + FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2), + createAngle(0.3), + RotationConvention.VECTOR_OPERATOR); FieldRotation<DerivativeStructure> r3 = r2.applyTo(r1); FieldRotation<DerivativeStructure> r3Double = r2.applyTo(new Rotation(r1.getQ0().getReal(), r1.getQ1().getReal(), @@ -511,8 +665,12 @@ public class FieldRotationDSTest { @Test public void testComposeInverse() throws MathIllegalArgumentException { - FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7)); - FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2), createAngle(0.3)); + FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), + createAngle(1.7), + RotationConvention.VECTOR_OPERATOR); + FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2), + createAngle(0.3), + RotationConvention.VECTOR_OPERATOR); FieldRotation<DerivativeStructure> r3 = r2.applyInverseTo(r1); FieldRotation<DerivativeStructure> r3Double = r2.applyInverseTo(new Rotation(r1.getQ0().getReal(), r1.getQ1().getReal(), @@ -540,7 +698,8 @@ public class FieldRotationDSTest { for (int i = 0; i < 10; ++i) { double[] unit = g.nextVector(); FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createVector(unit[0], unit[1], unit[2]), - createAngle(random.nextDouble())); + createAngle(random.nextDouble()), + RotationConvention.VECTOR_OPERATOR); for (double x = -0.9; x < 0.9; x += 0.2) { for (double y = -0.9; y < 0.9; y += 0.2) { @@ -574,7 +733,7 @@ public class FieldRotationDSTest { for (int i = 0; i < 10; ++i) { double[] unit1 = g.nextVector(); Rotation r1 = new Rotation(new Vector3D(unit1[0], unit1[1], unit1[2]), - random.nextDouble()); + random.nextDouble(), RotationConvention.VECTOR_OPERATOR); FieldRotation<DerivativeStructure> r1Prime = new FieldRotation<DerivativeStructure>(new DerivativeStructure(4, 1, 0, r1.getQ0()), new DerivativeStructure(4, 1, 1, r1.getQ1()), new DerivativeStructure(4, 1, 2, r1.getQ2()), @@ -582,7 +741,8 @@ public class FieldRotationDSTest { false); double[] unit2 = g.nextVector(); FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(unit2[0], unit2[1], unit2[2]), - createAngle(random.nextDouble())); + createAngle(random.nextDouble()), + RotationConvention.VECTOR_OPERATOR); FieldRotation<DerivativeStructure> rA = FieldRotation.applyTo(r1, r2); FieldRotation<DerivativeStructure> rB = r1Prime.applyTo(r2); @@ -620,7 +780,9 @@ public class FieldRotationDSTest { double theta = 1.7; double cosTheta = FastMath.cos(theta); double sinTheta = FastMath.sin(theta); - FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(kx, ky, kz), createAngle(theta)); + FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(kx, ky, kz), + createAngle(theta), + RotationConvention.VECTOR_OPERATOR); Vector3D a = new Vector3D(kx / n, ky / n, kz / n); // Jacobian of the normalized rotation axis a with respect to the Cartesian vector k @@ -684,7 +846,9 @@ public class FieldRotationDSTest { @Test public void testArray() throws MathIllegalArgumentException { - FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(2, -3, 5), createAngle(1.7)); + FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(2, -3, 5), + createAngle(1.7), + RotationConvention.VECTOR_OPERATOR); for (double x = -0.9; x < 0.9; x += 0.2) { for (double y = -0.9; y < 0.9; y += 0.2) { @@ -712,7 +876,9 @@ public class FieldRotationDSTest { DerivativeStructure[] in = new DerivativeStructure[3]; DerivativeStructure[] out = new DerivativeStructure[3]; DerivativeStructure[] rebuilt = new DerivativeStructure[3]; - FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7)); + FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), + createAngle(1.7), + RotationConvention.VECTOR_OPERATOR); for (double lambda = 0; lambda < 6.2; lambda += 0.2) { for (double phi = -1.55; phi < 1.55; phi += 0.2) { FieldVector3D<DerivativeStructure> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi), @@ -743,7 +909,9 @@ public class FieldRotationDSTest { } } - r = new FieldRotation<DerivativeStructure>(createVector(0, 0, 1), createAngle(FastMath.PI)); + r = new FieldRotation<DerivativeStructure>(createVector(0, 0, 1), + createAngle(FastMath.PI), + RotationConvention.VECTOR_OPERATOR); for (double lambda = 0; lambda < 6.2; lambda += 0.2) { for (double phi = -1.55; phi < 1.55; phi += 0.2) { FieldVector3D<DerivativeStructure> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),
