You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@commons.apache.org by lu...@apache.org on 2015/12/26 21:29:28 UTC
[3/3] [math] Added a RotationConvention enumerate.
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 <lu...@apache.org>
Authored: Sat Dec 26 21:21:44 2015 +0100
Committer: Luc Maisonobe <lu...@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()));
+ }
}
}