Added a RotationConvention enumerate. This enumerate allows specifying the semantics or axis/angle for rotations. This enumerate has two values: VECTOR_OPERATOR and FRAME_TRANSFORM.
JIRA: MATH-1302, MATH-1303 Project: http://git-wip-us.apache.org/repos/asf/commons-math/repo Commit: http://git-wip-us.apache.org/repos/asf/commons-math/commit/971a7786 Tree: http://git-wip-us.apache.org/repos/asf/commons-math/tree/971a7786 Diff: http://git-wip-us.apache.org/repos/asf/commons-math/diff/971a7786 Branch: refs/heads/MATH_3_X Commit: 971a778650f2cfcb78edef7f43d24f9453f2fbec Parents: cc893e4 Author: Luc Maisonobe <l...@apache.org> Authored: Sat Dec 26 21:21:44 2015 +0100 Committer: Luc Maisonobe <l...@apache.org> Committed: Sat Dec 26 21:21:44 2015 +0100 ---------------------------------------------------------------------- src/changes/changes.xml | 5 + .../euclidean/threed/FieldRotation.java | 739 ++++++++++++----- .../geometry/euclidean/threed/Rotation.java | 793 +++++++++++++------ .../euclidean/threed/RotationConvention.java | 79 ++ .../spherical/twod/SphericalPolygonsSet.java | 6 +- src/site/xdoc/userguide/geometry.xml | 14 +- .../commons/math3/complex/QuaternionTest.java | 13 +- .../euclidean/threed/FieldRotationDSTest.java | 270 +++++-- .../euclidean/threed/FieldRotationDfpTest.java | 266 +++++-- .../geometry/euclidean/threed/PlaneTest.java | 6 +- .../euclidean/threed/PolyhedronsSetTest.java | 2 +- .../geometry/euclidean/threed/RotationTest.java | 248 +++++- .../geometry/spherical/twod/CircleTest.java | 4 +- 13 files changed, 1849 insertions(+), 596 deletions(-) ---------------------------------------------------------------------- http://git-wip-us.apache.org/repos/asf/commons-math/blob/971a7786/src/changes/changes.xml ---------------------------------------------------------------------- diff --git a/src/changes/changes.xml b/src/changes/changes.xml index 263184a..cece47e 100644 --- a/src/changes/changes.xml +++ b/src/changes/changes.xml @@ -51,6 +51,11 @@ If the output is not quite correct, check for invisible trailing spaces! </properties> <body> <release version="3.6" date="XXXX-XX-XX" description=""> + <action dev="luc" type="add" issue="MATH-1302,MATH-1303"> + Added a RotationConvention enumerate to allow specifying the semantics + or axis/angle for rotations. This enumerate has two values: + VECTOR_OPERATOR and FRAME_TRANSFORM. + </action> <action dev="luc" type="fix" due-to="Julien Queyrel"> Fixed stability issues with Adams-Bashforth and Adams-Moulton ODE integrators. The integrators did not estimate the local error properly and were sometimes http://git-wip-us.apache.org/repos/asf/commons-math/blob/971a7786/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java ---------------------------------------------------------------------- diff --git a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java index dd6f3e4..230ecbc 100644 --- a/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java +++ b/src/main/java/org/apache/commons/math3/geometry/euclidean/threed/FieldRotation.java @@ -111,16 +111,47 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl * @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 + * #FieldRotation(FieldVector3D, RealFieldElement, RotationConvention) */ + @Deprecated public FieldRotation(final FieldVector3D<T> axis, final T angle) throws MathIllegalArgumentException { + this(axis, angle, RotationConvention.VECTOR_OPERATOR); + } + + /** Build a rotation from an axis and an angle. + * <p>We use the convention that angles are oriented according to + * the effect of the rotation on vectors around the axis. That means + * that if (i, j, k) is a direct frame and if we first provide +k as + * the axis and π/2 as the angle to this constructor, and then + * {@link #applyTo(FieldVector3D) apply} the instance to +i, we will get + * +j.</p> + * <p>Another way to represent our convention is to say that a rotation + * of angle θ about the unit vector (x, y, z) is the same as the + * rotation build from quaternion components { cos(-θ/2), + * x * sin(-θ/2), y * sin(-θ/2), z * sin(-θ/2) }. + * Note the minus sign on the angle!</p> + * <p>On the one hand this convention is consistent with a vectorial + * perspective (moving vectors in fixed frames), on the other hand it + * is different from conventions with a frame perspective (fixed vectors + * viewed from different frames) like the ones used for example in spacecraft + * attitude community or in the graphics community.</p> + * @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 FieldRotation(final FieldVector3D<T> axis, final T angle, final RotationConvention convention) + throws MathIllegalArgumentException { final T norm = axis.getNorm(); if (norm.getReal() == 0) { throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_AXIS); } - final T halfAngle = angle.multiply(-0.5); + final T halfAngle = angle.multiply(convention == RotationConvention.VECTOR_OPERATOR ? -0.5 : 0.5); final T coeff = halfAngle.sin().divide(norm); q0 = halfAngle.cos(); @@ -194,7 +225,7 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl } - /** 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 @@ -203,27 +234,27 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl * <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 FieldRotation(FieldVector3D<T> u1, FieldVector3D<T> u2, FieldVector3D<T> v1, FieldVector3D<T> 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 FieldVector3D<T> u3 = FieldVector3D.crossProduct(u1, u2).normalize(); u2 = FieldVector3D.crossProduct(u3, 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 FieldVector3D<T> v3 = FieldVector3D.crossProduct(v1, v2).normalize(); v2 = FieldVector3D.crossProduct(v3, v1).normalize(); v1 = v1.normalize(); @@ -254,7 +285,7 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl * 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 @@ -309,13 +340,45 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl * @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 + * #FieldRotation(RotationOrder, RotationConvention, + * RealFieldElement, RealFieldElement, RealFieldElement)} */ + @Deprecated public FieldRotation(final RotationOrder order, final T alpha1, final T alpha2, final T alpha3) { + 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 FieldRotation(final RotationOrder order, final RotationConvention convention, + final T alpha1, final T alpha2, final T alpha3) { final T one = alpha1.getField().getOne(); - final FieldRotation<T> r1 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA1()), alpha1); - final FieldRotation<T> r2 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA2()), alpha2); - final FieldRotation<T> r3 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA3()), alpha3); - final FieldRotation<T> composed = r1.applyTo(r2.applyTo(r3)); + final FieldRotation<T> r1 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA1()), alpha1, convention); + final FieldRotation<T> r2 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA2()), alpha2, convention); + final FieldRotation<T> r3 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA3()), alpha3, convention); + final FieldRotation<T> composed = convention == RotationConvention.FRAME_TRANSFORM ? + r3.applyTo(r2.applyTo(r1)) : + r1.applyTo(r2.applyTo(r3)); q0 = composed.q0; q1 = composed.q1; q2 = composed.q2; @@ -425,18 +488,40 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl /** Get the normalized axis of the rotation. * @return normalized axis of the rotation * @see #FieldRotation(FieldVector3D, RealFieldElement) + * @deprecated as of 3.6, replaced with {@link #getAxis(RotationConvention)} */ + @Deprecated public FieldVector3D<T> getAxis() { + 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 #FieldRotation(FieldVector3D, RealFieldElement) + * @since 3.6 + */ + public FieldVector3D<T> getAxis(final RotationConvention convention) { final T squaredSine = q1.multiply(q1).add(q2.multiply(q2)).add(q3.multiply(q3)); if (squaredSine.getReal() == 0) { final Field<T> field = squaredSine.getField(); - return new FieldVector3D<T>(field.getOne(), field.getZero(), field.getZero()); - } else if (q0.getReal() < 0) { - T inverse = squaredSine.sqrt().reciprocal(); + return new FieldVector3D<T>(convention == RotationConvention.VECTOR_OPERATOR ? field.getOne(): field.getOne().negate(), + field.getZero(), + field.getZero()); + } else { + final double sgn = convention == RotationConvention.VECTOR_OPERATOR ? +1 : -1; + if (q0.getReal() < 0) { + T inverse = squaredSine.sqrt().reciprocal().multiply(sgn); + return new FieldVector3D<T>(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); + } + final T inverse = squaredSine.sqrt().reciprocal().negate().multiply(sgn); return new FieldVector3D<T>(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); } - final T inverse = squaredSine.sqrt().reciprocal().negate(); - return new FieldVector3D<T>(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse)); } /** Get the angle of the rotation. @@ -486,202 +571,442 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl * @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 T[] getAngles(final RotationOrder order) throws CardanEulerSingularityException { + return getAngles(order, RotationConvention.VECTOR_OPERATOR); + } + + /** Get the Cardan or Euler angles corresponding to the instance. + + * <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> + + * <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> + + * @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 T[] getAngles(final RotationOrder order, RotationConvention convention) + throws CardanEulerSingularityException { + + if (convention == RotationConvention.VECTOR_OPERATOR) { + if (order == RotationOrder.XYZ) { + + // r (+K) coordinates are : + // sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi) + // (-r) (+I) coordinates are : + // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta) + final // and we can choose to have theta in the interval [-PI/2 ; +PI/2] + FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getY().negate().atan2(v1.getZ()), + v2.getZ().asin(), + v2.getY().negate().atan2(v2.getX())); + + } else if (order == RotationOrder.XZY) { + + // r (+J) coordinates are : + // -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi) + // (-r) (+I) 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] + final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getZ().atan2(v1.getY()), + v2.getY().asin().negate(), + v2.getZ().atan2(v2.getX())); + + } else if (order == RotationOrder.YXZ) { + + // r (+K) coordinates are : + // cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta) + // (-r) (+J) 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] + final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getX().atan2(v1.getZ()), + v2.getZ().asin().negate(), + v2.getX().atan2(v2.getY())); + + } else if (order == RotationOrder.YZX) { + + // r (+I) coordinates are : + // cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta) + // (-r) (+J) 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] + final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getZ().negate().atan2(v1.getX()), + v2.getX().asin(), + v2.getZ().negate().atan2(v2.getY())); + + } else if (order == RotationOrder.ZXY) { + + // r (+J) coordinates are : + // -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi) + // (-r) (+K) 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] + final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getX().negate().atan2(v1.getY()), + v2.getY().asin(), + v2.getX().negate().atan2(v2.getZ())); + + } else if (order == RotationOrder.ZYX) { + + // r (+I) coordinates are : + // cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta) + // (-r) (+K) 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] + final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v1.getY().atan2(v1.getX()), + v2.getX().asin().negate(), + v2.getY().atan2(v2.getZ())); + + } else if (order == RotationOrder.XYX) { + + // r (+I) coordinates are : + // cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta) + // (-r) (+I) coordinates are : + // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2) + // and we can choose to have theta in the interval [0 ; PI] + final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getY().atan2(v1.getZ().negate()), + v2.getX().acos(), + v2.getY().atan2(v2.getZ())); + + } else if (order == RotationOrder.XZX) { + + // r (+I) coordinates are : + // cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi) + // (-r) (+I) coordinates are : + // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2) + // and we can choose to have psi in the interval [0 ; PI] + final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getZ().atan2(v1.getY()), + v2.getX().acos(), + v2.getZ().atan2(v2.getY().negate())); + + } else if (order == RotationOrder.YXY) { + + // r (+J) coordinates are : + // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) + // (-r) (+J) coordinates are : + // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) + // and we can choose to have phi in the interval [0 ; PI] + final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getX().atan2(v1.getZ()), + v2.getY().acos(), + v2.getX().atan2(v2.getZ().negate())); + + } else if (order == RotationOrder.YZY) { + + // r (+J) coordinates are : + // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) + // (-r) (+J) coordinates are : + // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) + // and we can choose to have psi in the interval [0 ; PI] + final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getZ().atan2(v1.getX().negate()), + v2.getY().acos(), + v2.getZ().atan2(v2.getX())); + + } else if (order == RotationOrder.ZXZ) { + + // r (+K) coordinates are : + // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) + // (-r) (+K) coordinates are : + // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) + // and we can choose to have phi in the interval [0 ; PI] + final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getX().atan2(v1.getY().negate()), + v2.getZ().acos(), + v2.getX().atan2(v2.getY())); + + } else { // last possibility is ZYZ + + // r (+K) coordinates are : + // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) + // (-r) (+K) coordinates are : + // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) + // and we can choose to have theta in the interval [0 ; PI] + final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); + final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v1.getY().atan2(v1.getX()), + v2.getZ().acos(), + v2.getY().atan2(v2.getX().negate())); - if (order == RotationOrder.XYZ) { - - // r (+K) coordinates are : - // sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi) - // (-r) (+I) coordinates are : - // cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta) - final // and we can choose to have theta in the interval [-PI/2 ; +PI/2] - FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); - final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); - if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(true); - } - return buildArray(v1.getY().negate().atan2(v1.getZ()), - v2.getZ().asin(), - v2.getY().negate().atan2(v2.getX())); - - } else if (order == RotationOrder.XZY) { - - // r (+J) coordinates are : - // -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi) - // (-r) (+I) 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] - final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); - final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); - if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(true); - } - return buildArray(v1.getZ().atan2(v1.getY()), - v2.getY().asin().negate(), - v2.getZ().atan2(v2.getX())); - - } else if (order == RotationOrder.YXZ) { - - // r (+K) coordinates are : - // cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta) - // (-r) (+J) 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] - final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); - final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); - if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(true); - } - return buildArray(v1.getX().atan2(v1.getZ()), - v2.getZ().asin().negate(), - v2.getX().atan2(v2.getY())); - - } else if (order == RotationOrder.YZX) { - - // r (+I) coordinates are : - // cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta) - // (-r) (+J) 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] - final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); - final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); - if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(true); - } - return buildArray(v1.getZ().negate().atan2(v1.getX()), - v2.getX().asin(), - v2.getZ().negate().atan2(v2.getY())); - - } else if (order == RotationOrder.ZXY) { - - // r (+J) coordinates are : - // -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi) - // (-r) (+K) 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] - final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); - final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); - if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(true); - } - return buildArray(v1.getX().negate().atan2(v1.getY()), - v2.getY().asin(), - v2.getX().negate().atan2(v2.getZ())); - - } else if (order == RotationOrder.ZYX) { - - // r (+I) coordinates are : - // cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta) - // (-r) (+K) 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] - final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); - final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); - if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(true); - } - return buildArray(v1.getY().atan2(v1.getX()), - v2.getX().asin().negate(), - v2.getY().atan2(v2.getZ())); - - } else if (order == RotationOrder.XYX) { - - // r (+I) coordinates are : - // cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta) - // (-r) (+I) coordinates are : - // cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2) - // and we can choose to have theta in the interval [0 ; PI] - final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); - final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); - if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(false); - } - return buildArray(v1.getY().atan2(v1.getZ().negate()), - v2.getX().acos(), - v2.getY().atan2(v2.getZ())); - - } else if (order == RotationOrder.XZX) { - - // r (+I) coordinates are : - // cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi) - // (-r) (+I) coordinates are : - // cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2) - // and we can choose to have psi in the interval [0 ; PI] - final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0)); - final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0)); - if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(false); - } - return buildArray(v1.getZ().atan2(v1.getY()), - v2.getX().acos(), - v2.getZ().atan2(v2.getY().negate())); - - } else if (order == RotationOrder.YXY) { - - // r (+J) coordinates are : - // sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi) - // (-r) (+J) coordinates are : - // sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2) - // and we can choose to have phi in the interval [0 ; PI] - final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); - final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); - if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(false); - } - return buildArray(v1.getX().atan2(v1.getZ()), - v2.getY().acos(), - v2.getX().atan2(v2.getZ().negate())); - - } else if (order == RotationOrder.YZY) { - - // r (+J) coordinates are : - // -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi) - // (-r) (+J) coordinates are : - // sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2) - // and we can choose to have psi in the interval [0 ; PI] - final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0)); - final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0)); - if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(false); - } - return buildArray(v1.getZ().atan2(v1.getX().negate()), - v2.getY().acos(), - v2.getZ().atan2(v2.getX())); - - } else if (order == RotationOrder.ZXZ) { - - // r (+K) coordinates are : - // sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi) - // (-r) (+K) coordinates are : - // sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi) - // and we can choose to have phi in the interval [0 ; PI] - final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); - final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); - if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(false); - } - return buildArray(v1.getX().atan2(v1.getY().negate()), - v2.getZ().acos(), - v2.getX().atan2(v2.getY())); - - } else { // last possibility is ZYZ - - // r (+K) coordinates are : - // cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta) - // (-r) (+K) coordinates are : - // -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta) - // and we can choose to have theta in the interval [0 ; PI] - final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1)); - final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1)); - if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { - throw new CardanEulerSingularityException(false); } - return buildArray(v1.getY().atan2(v1.getX()), - v2.getZ().acos(), - v2.getY().atan2(v2.getX().negate())); + } 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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getY().negate().atan2(v2.getZ()), + v2.getX().asin(), + v1.getY().negate().atan2(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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getZ().atan2(v2.getY()), + v2.getX().asin().negate(), + v1.getZ().atan2(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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getX().atan2(v2.getZ()), + v2.getY().asin().negate(), + v1.getX().atan2(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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getZ().negate().atan2(v2.getX()), + v2.getY().asin(), + v1.getZ().negate().atan2(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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getX().negate().atan2(v2.getY()), + v2.getZ().asin(), + v1.getX().negate().atan2(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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(true); + } + return buildArray(v2.getY().atan2(v2.getX()), + v2.getZ().asin().negate(), + v1.getY().atan2(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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getY().atan2(v2.getZ().negate()), + v2.getX().acos(), + v1.getY().atan2(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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I); + if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getZ().atan2(v2.getY()), + v2.getX().acos(), + v1.getZ().atan2(v1.getY().negate())); + + } 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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getX().atan2(v2.getZ()), + v2.getY().acos(), + v1.getX().atan2(v1.getZ().negate())); + + } 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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J); + if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getZ().atan2(v2.getX().negate()), + v2.getY().acos(), + v1.getZ().atan2(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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getX().atan2(v2.getY().negate()), + v2.getZ().acos(), + v1.getX().atan2(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] + FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K); + FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K); + if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) { + throw new CardanEulerSingularityException(false); + } + return buildArray(v2.getY().atan2(v2.getX()), + v2.getZ().acos(), + v1.getY().atan2(v1.getX().negate())); + } } }
