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/27 13:09:55 UTC
[math] Added compose and composeInverse to rotations.
Repository: commons-math
Updated Branches:
refs/heads/master 9ce4e1a37 -> c9b1c8f96
Added compose and composeInverse to rotations.
These method are more flexible than the existing applyTo and
applyInverseTo (which are still present), because they allow caller
to specify a RotationConvention.
JIRA: MATH-1302, MATH-1303
Github: closes #22
Project: http://git-wip-us.apache.org/repos/asf/commons-math/repo
Commit: http://git-wip-us.apache.org/repos/asf/commons-math/commit/c9b1c8f9
Tree: http://git-wip-us.apache.org/repos/asf/commons-math/tree/c9b1c8f9
Diff: http://git-wip-us.apache.org/repos/asf/commons-math/diff/c9b1c8f9
Branch: refs/heads/master
Commit: c9b1c8f9662f865a613632e1d390922050130b60
Parents: 9ce4e1a
Author: Luc Maisonobe <lu...@apache.org>
Authored: Sun Dec 27 13:09:13 2015 +0100
Committer: Luc Maisonobe <lu...@apache.org>
Committed: Sun Dec 27 13:09:13 2015 +0100
----------------------------------------------------------------------
.../euclidean/threed/FieldRotation.java | 217 ++++++++++++--
.../geometry/euclidean/threed/Rotation.java | 182 +++++++-----
src/site/xdoc/userguide/geometry.xml | 8 +-
.../euclidean/threed/FieldRotationDSTest.java | 284 ++++++++++++++++++-
.../euclidean/threed/FieldRotationDfpTest.java | 166 ++++++++++-
.../geometry/euclidean/threed/RotationTest.java | 112 +++++++-
6 files changed, 846 insertions(+), 123 deletions(-)
----------------------------------------------------------------------
http://git-wip-us.apache.org/repos/asf/commons-math/blob/c9b1c8f9/src/main/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotation.java
----------------------------------------------------------------------
diff --git a/src/main/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotation.java b/src/main/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotation.java
index 22e1c17..4c7c888 100644
--- a/src/main/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotation.java
+++ b/src/main/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotation.java
@@ -363,7 +363,8 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
* 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 order order of rotations to compose, from left to right
+ * (i.e. we will use {@code r1.compose(r2.compose(r3, convention), convention)})
* @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
@@ -376,9 +377,7 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
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));
+ final FieldRotation<T> composed = r1.compose(r2.compose(r3, convention), convention);
q0 = composed.q0;
q1 = composed.q1;
q2 = composed.q2;
@@ -1271,15 +1270,53 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
}
/** Apply the instance to another rotation.
- * Applying the instance to a rotation is computing the composition
- * in an order compliant with the following rule : let u be any
- * vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image
- * of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u),
- * where comp = applyTo(r).
+ * <p>
+ * Calling this method is equivalent to call
+ * {@link #compose(FieldRotation, RotationConvention)
+ * compose(r, RotationConvention.VECTOR_OPERATOR)}.
+ * </p>
* @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the instance
*/
public FieldRotation<T> applyTo(final FieldRotation<T> r) {
+ return compose(r, RotationConvention.VECTOR_OPERATOR);
+ }
+
+ /** Compose the instance with another rotation.
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
+ * applying the instance to a rotation is computing the composition
+ * in an order compliant with the following rule : let {@code u} be any
+ * vector and {@code v} its image by {@code r1} (i.e.
+ * {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by
+ * rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then
+ * {@code w = comp.applyTo(u)}, where
+ * {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}.
+ * </p>
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
+ * the application order will be reversed. So keeping the exact same
+ * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
+ * and {@code comp} as above, {@code comp} could also be computed as
+ * {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}.
+ * </p>
+ * @param r rotation to apply the rotation to
+ * @param convention convention to use for the semantics of the angle
+ * @return a new rotation which is the composition of r by the instance
+ */
+ public FieldRotation<T> compose(final FieldRotation<T> r, final RotationConvention convention) {
+ return convention == RotationConvention.VECTOR_OPERATOR ?
+ composeInternal(r) : r.composeInternal(this);
+ }
+
+ /** Compose the instance with another rotation using vector operator convention.
+ * @param r rotation to apply the rotation to
+ * @return a new rotation which is the composition of r by the instance
+ * using vector operator convention
+ */
+ private FieldRotation<T> composeInternal(final FieldRotation<T> r) {
return new FieldRotation<T>(r.q0.multiply(q0).subtract(r.q1.multiply(q1).add(r.q2.multiply(q2)).add(r.q3.multiply(q3))),
r.q1.multiply(q0).add(r.q0.multiply(q1)).add(r.q2.multiply(q3).subtract(r.q3.multiply(q2))),
r.q2.multiply(q0).add(r.q0.multiply(q2)).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))),
@@ -1288,20 +1325,58 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
}
/** Apply the instance to another rotation.
- * Applying the instance to a rotation is computing the composition
- * in an order compliant with the following rule : let u be any
- * vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image
- * of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u),
- * where comp = applyTo(r).
+ * <p>
+ * Calling this method is equivalent to call
+ * {@link #compose(Rotation, RotationConvention)
+ * compose(r, RotationConvention.VECTOR_OPERATOR)}.
+ * </p>
* @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the instance
*/
public FieldRotation<T> applyTo(final Rotation r) {
+ return compose(r, RotationConvention.VECTOR_OPERATOR);
+ }
+
+ /** Compose the instance with another rotation.
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
+ * applying the instance to a rotation is computing the composition
+ * in an order compliant with the following rule : let {@code u} be any
+ * vector and {@code v} its image by {@code r1} (i.e.
+ * {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by
+ * rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then
+ * {@code w = comp.applyTo(u)}, where
+ * {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}.
+ * </p>
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
+ * the application order will be reversed. So keeping the exact same
+ * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
+ * and {@code comp} as above, {@code comp} could also be computed as
+ * {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}.
+ * </p>
+ * @param r rotation to apply the rotation to
+ * @param convention convention to use for the semantics of the angle
+ * @return a new rotation which is the composition of r by the instance
+ */
+ public FieldRotation<T> compose(final Rotation r, final RotationConvention convention) {
+ return convention == RotationConvention.VECTOR_OPERATOR ?
+ composeInternal(r) : applyTo(r, this);
+ }
+
+ /** Compose the instance with another rotation using vector operator convention.
+ * @param r rotation to apply the rotation to
+ * @return a new rotation which is the composition of r by the instance
+ * using vector operator convention
+ */
+ private FieldRotation<T> composeInternal(final Rotation r) {
return new FieldRotation<T>(q0.multiply(r.getQ0()).subtract(q1.multiply(r.getQ1()).add(q2.multiply(r.getQ2())).add(q3.multiply(r.getQ3()))),
- q0.multiply(r.getQ1()).add(q1.multiply(r.getQ0())).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))),
- q0.multiply(r.getQ2()).add(q2.multiply(r.getQ0())).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))),
- q0.multiply(r.getQ3()).add(q3.multiply(r.getQ0())).add(q2.multiply(r.getQ1()).subtract(q1.multiply(r.getQ2()))),
- false);
+ q0.multiply(r.getQ1()).add(q1.multiply(r.getQ0())).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))),
+ q0.multiply(r.getQ2()).add(q2.multiply(r.getQ0())).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))),
+ q0.multiply(r.getQ3()).add(q3.multiply(r.getQ0())).add(q2.multiply(r.getQ1()).subtract(q1.multiply(r.getQ2()))),
+ false);
}
/** Apply a rotation to another rotation.
@@ -1324,17 +1399,57 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
}
/** Apply the inverse of the instance to another rotation.
- * Applying the inverse of the instance to a rotation is computing
- * the composition in an order compliant with the following rule :
- * let u be any vector and v its image by r (i.e. r.applyTo(u) = v),
- * let w be the inverse image of v by the instance
- * (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where
- * comp = applyInverseTo(r).
+ * <p>
+ * Calling this method is equivalent to call
+ * {@link #composeInverse(FieldRotation<T>, RotationConvention)
+ * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}.
+ * </p>
* @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the inverse
* of the instance
*/
public FieldRotation<T> applyInverseTo(final FieldRotation<T> r) {
+ return composeInverse(r, RotationConvention.VECTOR_OPERATOR);
+ }
+
+ /** Compose the inverse of the instance with another rotation.
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
+ * applying the inverse of the instance to a rotation is computing
+ * the composition in an order compliant with the following rule :
+ * let {@code u} be any vector and {@code v} its image by {@code r1}
+ * (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image
+ * of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}).
+ * Then {@code w = comp.applyTo(u)}, where
+ * {@code comp = r2.composeInverse(r1)}.
+ * </p>
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
+ * the application order will be reversed, which means it is the
+ * <em>innermost</em> rotation that will be reversed. So keeping the exact same
+ * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
+ * and {@code comp} as above, {@code comp} could also be computed as
+ * {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}.
+ * </p>
+ * @param r rotation to apply the rotation to
+ * @param convention convention to use for the semantics of the angle
+ * @return a new rotation which is the composition of r by the inverse
+ * of the instance
+ */
+ public FieldRotation<T> composeInverse(final FieldRotation<T> r, final RotationConvention convention) {
+ return convention == RotationConvention.VECTOR_OPERATOR ?
+ composeInverseInternal(r) : r.composeInternal(revert());
+ }
+
+ /** Compose the inverse of the instance with another rotation
+ * using vector operator convention.
+ * @param r rotation to apply the rotation to
+ * @return a new rotation which is the composition of r by the inverse
+ * of the instance using vector operator convention
+ */
+ private FieldRotation<T> composeInverseInternal(FieldRotation<T> r) {
return new FieldRotation<T>(r.q0.multiply(q0).add(r.q1.multiply(q1).add(r.q2.multiply(q2)).add(r.q3.multiply(q3))).negate(),
r.q0.multiply(q1).add(r.q2.multiply(q3).subtract(r.q3.multiply(q2))).subtract(r.q1.multiply(q0)),
r.q0.multiply(q2).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))).subtract(r.q2.multiply(q0)),
@@ -1343,17 +1458,57 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
}
/** Apply the inverse of the instance to another rotation.
- * Applying the inverse of the instance to a rotation is computing
- * the composition in an order compliant with the following rule :
- * let u be any vector and v its image by r (i.e. r.applyTo(u) = v),
- * let w be the inverse image of v by the instance
- * (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where
- * comp = applyInverseTo(r).
+ * <p>
+ * Calling this method is equivalent to call
+ * {@link #composeInverse(Rotation, RotationConvention)
+ * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}.
+ * </p>
* @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the inverse
* of the instance
*/
public FieldRotation<T> applyInverseTo(final Rotation r) {
+ return composeInverse(r, RotationConvention.VECTOR_OPERATOR);
+ }
+
+ /** Compose the inverse of the instance with another rotation.
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
+ * applying the inverse of the instance to a rotation is computing
+ * the composition in an order compliant with the following rule :
+ * let {@code u} be any vector and {@code v} its image by {@code r1}
+ * (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image
+ * of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}).
+ * Then {@code w = comp.applyTo(u)}, where
+ * {@code comp = r2.composeInverse(r1)}.
+ * </p>
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
+ * the application order will be reversed, which means it is the
+ * <em>innermost</em> rotation that will be reversed. So keeping the exact same
+ * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
+ * and {@code comp} as above, {@code comp} could also be computed as
+ * {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}.
+ * </p>
+ * @param r rotation to apply the rotation to
+ * @param convention convention to use for the semantics of the angle
+ * @return a new rotation which is the composition of r by the inverse
+ * of the instance
+ */
+ public FieldRotation<T> composeInverse(final Rotation r, final RotationConvention convention) {
+ return convention == RotationConvention.VECTOR_OPERATOR ?
+ composeInverseInternal(r) : applyTo(r, revert());
+ }
+
+ /** Compose the inverse of the instance with another rotation
+ * using vector operator convention.
+ * @param r rotation to apply the rotation to
+ * @return a new rotation which is the composition of r by the inverse
+ * of the instance using vector operator convention
+ */
+ private FieldRotation<T> composeInverseInternal(Rotation r) {
return new FieldRotation<T>(q0.multiply(r.getQ0()).add(q1.multiply(r.getQ1()).add(q2.multiply(r.getQ2())).add(q3.multiply(r.getQ3()))).negate(),
q1.multiply(r.getQ0()).add(q3.multiply(r.getQ2()).subtract(q2.multiply(r.getQ3()))).subtract(q0.multiply(r.getQ1())),
q2.multiply(r.getQ0()).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))).subtract(q0.multiply(r.getQ2())),
@@ -1502,7 +1657,7 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
* @return <i>distance</i> between r1 and r2
*/
public static <T extends RealFieldElement<T>> T distance(final FieldRotation<T> r1, final FieldRotation<T> r2) {
- return r1.applyInverseTo(r2).getAngle();
+ return r1.composeInverseInternal(r2).getAngle();
}
}
http://git-wip-us.apache.org/repos/asf/commons-math/blob/c9b1c8f9/src/main/java/org/apache/commons/math4/geometry/euclidean/threed/Rotation.java
----------------------------------------------------------------------
diff --git a/src/main/java/org/apache/commons/math4/geometry/euclidean/threed/Rotation.java b/src/main/java/org/apache/commons/math4/geometry/euclidean/threed/Rotation.java
index a7705d1..5de70a5 100644
--- a/src/main/java/org/apache/commons/math4/geometry/euclidean/threed/Rotation.java
+++ b/src/main/java/org/apache/commons/math4/geometry/euclidean/threed/Rotation.java
@@ -151,22 +151,11 @@ public class Rotation implements Serializable {
}
/** 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(Vector3D) 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>
+ * <p>
+ * Calling this constructor is equivalent to call
+ * {@link #Rotation(Vector3D, double, RotationConvention)
+ * new Rotation(axis, angle, RotationConvention.VECTOR_OPERATOR)}
+ * </p>
* @param axis axis around which to rotate
* @param angle rotation angle.
* @exception MathIllegalArgumentException if the axis norm is zero
@@ -370,17 +359,11 @@ public class Rotation implements Serializable {
/** 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>
+ * <p>
+ * Calling this constructor is equivalent to call
+ * {@link #Rotation(RotationOrder, RotationConvention, double, double, double)
+ * new Rotation(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3)}
+ * </p>
* @param order order of rotations to use
* @param alpha1 angle of the first elementary rotation
@@ -409,7 +392,8 @@ public class Rotation implements Serializable {
* 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 order order of rotations to compose, from left to right
+ * (i.e. we will use {@code r1.compose(r2.compose(r3, convention), convention)})
* @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
@@ -421,9 +405,7 @@ public class Rotation implements Serializable {
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));
+ Rotation composed = r1.compose(r2.compose(r3, convention), convention);
q0 = composed.q0;
q1 = composed.q1;
q2 = composed.q2;
@@ -531,6 +513,10 @@ public class Rotation implements Serializable {
}
/** Get the normalized axis of the rotation.
+ * <p>
+ * Calling this method is equivalent to call
+ * {@link #getAxis(RotationConvention) getAxis(RotationConvention.VECTOR_OPERATOR)}
+ * </p>
* @return normalized axis of the rotation
* @see #Rotation(Vector3D, double, RotationConvention)
* @deprecated as of 3.6, replaced with {@link #getAxis(RotationConvention)}
@@ -581,33 +567,11 @@ public class Rotation implements Serializable {
/** 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>
+ * <p>
+ * Calling this method is equivalent to call
+ * {@link #getAngles(RotationOrder, RotationConvention)
+ * getAngles(order, RotationConvention.VECTOR_OPERATOR)}
+ * </p>
* @param order rotation order to use
* @return an array of three angles, in the order specified by the set
@@ -1217,15 +1181,53 @@ public class Rotation implements Serializable {
}
/** Apply the instance to another rotation.
- * Applying the instance to a rotation is computing the composition
- * in an order compliant with the following rule : let u be any
- * vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image
- * of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u),
- * where comp = applyTo(r).
+ * <p>
+ * Calling this method is equivalent to call
+ * {@link #compose(Rotation, RotationConvention)
+ * compose(r, RotationConvention.VECTOR_OPERATOR)}.
+ * </p>
* @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the instance
*/
public Rotation applyTo(Rotation r) {
+ return compose(r, RotationConvention.VECTOR_OPERATOR);
+ }
+
+ /** Compose the instance with another rotation.
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
+ * applying the instance to a rotation is computing the composition
+ * in an order compliant with the following rule : let {@code u} be any
+ * vector and {@code v} its image by {@code r1} (i.e.
+ * {@code r1.applyTo(u) = v}). Let {@code w} be the image of {@code v} by
+ * rotation {@code r2} (i.e. {@code r2.applyTo(v) = w}). Then
+ * {@code w = comp.applyTo(u)}, where
+ * {@code comp = r2.compose(r1, RotationConvention.VECTOR_OPERATOR)}.
+ * </p>
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
+ * the application order will be reversed. So keeping the exact same
+ * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
+ * and {@code comp} as above, {@code comp} could also be computed as
+ * {@code comp = r1.compose(r2, RotationConvention.FRAME_TRANSFORM)}.
+ * </p>
+ * @param r rotation to apply the rotation to
+ * @param convention convention to use for the semantics of the angle
+ * @return a new rotation which is the composition of r by the instance
+ */
+ public Rotation compose(final Rotation r, final RotationConvention convention) {
+ return convention == RotationConvention.VECTOR_OPERATOR ?
+ composeInternal(r) : r.composeInternal(this);
+ }
+
+ /** Compose the instance with another rotation using vector operator convention.
+ * @param r rotation to apply the rotation to
+ * @return a new rotation which is the composition of r by the instance
+ * using vector operator convention
+ */
+ private Rotation composeInternal(final Rotation r) {
return new Rotation(r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3),
r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2),
r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3),
@@ -1234,17 +1236,57 @@ public class Rotation implements Serializable {
}
/** Apply the inverse of the instance to another rotation.
- * Applying the inverse of the instance to a rotation is computing
- * the composition in an order compliant with the following rule :
- * let u be any vector and v its image by r (i.e. r.applyTo(u) = v),
- * let w be the inverse image of v by the instance
- * (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where
- * comp = applyInverseTo(r).
+ * <p>
+ * Calling this method is equivalent to call
+ * {@link #composeInverse(Rotation, RotationConvention)
+ * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}.
+ * </p>
* @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the inverse
* of the instance
*/
public Rotation applyInverseTo(Rotation r) {
+ return composeInverse(r, RotationConvention.VECTOR_OPERATOR);
+ }
+
+ /** Compose the inverse of the instance with another rotation.
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#VECTOR_OPERATOR vector operator} convention,
+ * applying the inverse of the instance to a rotation is computing
+ * the composition in an order compliant with the following rule :
+ * let {@code u} be any vector and {@code v} its image by {@code r1}
+ * (i.e. {@code r1.applyTo(u) = v}). Let {@code w} be the inverse image
+ * of {@code v} by {@code r2} (i.e. {@code r2.applyInverseTo(v) = w}).
+ * Then {@code w = comp.applyTo(u)}, where
+ * {@code comp = r2.composeInverse(r1)}.
+ * </p>
+ * <p>
+ * If the semantics of the rotations composition corresponds to a
+ * {@link RotationConvention#FRAME_TRANSFORM frame transform} convention,
+ * the application order will be reversed, which means it is the
+ * <em>innermost</em> rotation that will be reversed. So keeping the exact same
+ * meaning of all {@code r1}, {@code r2}, {@code u}, {@code v}, {@code w}
+ * and {@code comp} as above, {@code comp} could also be computed as
+ * {@code comp = r1.revert().composeInverse(r2.revert(), RotationConvention.FRAME_TRANSFORM)}.
+ * </p>
+ * @param r rotation to apply the rotation to
+ * @param convention convention to use for the semantics of the angle
+ * @return a new rotation which is the composition of r by the inverse
+ * of the instance
+ */
+ public Rotation composeInverse(final Rotation r, final RotationConvention convention) {
+ return convention == RotationConvention.VECTOR_OPERATOR ?
+ composeInverseInternal(r) : r.composeInternal(revert());
+ }
+
+ /** Compose the inverse of the instance with another rotation
+ * using vector operator convention.
+ * @param r rotation to apply the rotation to
+ * @return a new rotation which is the composition of r by the inverse
+ * of the instance using vector operator convention
+ */
+ private Rotation composeInverseInternal(Rotation r) {
return new Rotation(-r.q0 * q0 - (r.q1 * q1 + r.q2 * q2 + r.q3 * q3),
-r.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2),
-r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3),
@@ -1376,7 +1418,7 @@ public class Rotation implements Serializable {
* @return <i>distance</i> between r1 and r2
*/
public static double distance(Rotation r1, Rotation r2) {
- return r1.applyInverseTo(r2).getAngle();
+ return r1.composeInverseInternal(r2).getAngle();
}
}
http://git-wip-us.apache.org/repos/asf/commons-math/blob/c9b1c8f9/src/site/xdoc/userguide/geometry.xml
----------------------------------------------------------------------
diff --git a/src/site/xdoc/userguide/geometry.xml b/src/site/xdoc/userguide/geometry.xml
index 214d85b..fc6c635 100644
--- a/src/site/xdoc/userguide/geometry.xml
+++ b/src/site/xdoc/userguide/geometry.xml
@@ -187,8 +187,12 @@
previous notations, we would say we can apply <code>r<sub>1</sub></code> to
<code>r<sub>2</sub></code> and the result we get is <code>r =
r<sub>1</sub> o r<sub>2</sub></code>. For this purpose, the class
- provides the methods: <code>applyTo(Rotation)</code> and
- <code>applyInverseTo(Rotation)</code>.
+ provides the methods: <code>compose(Rotation, RotationConvention)</code> and
+ <code>composeInverse(Rotation, RotationConvention)</code>. There are also
+ shortcuts <code>applyTo(Rotation)</code> which is equivalent to
+ <code>compose(Rotation, RotationConvention.VECTOR_OPERATOR)</code> and
+ <code>applyInverseTo(Rotation)</code> which is equivalent to
+ <code>composeInverse(Rotation, RotationConvention.VECTOR_OPERATOR)</code>.
</p>
</subsection>
<subsection name="11.3 n-Sphere" href="sphere">
http://git-wip-us.apache.org/repos/asf/commons-math/blob/c9b1c8f9/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDSTest.java
----------------------------------------------------------------------
diff --git a/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDSTest.java b/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDSTest.java
index 394ee2a..5b352eb 100644
--- a/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDSTest.java
+++ b/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDSTest.java
@@ -247,6 +247,150 @@ public class FieldRotationDSTest {
}
@Test
+ public void testRevertVectorOperator() {
+ double a = 0.001;
+ double b = 0.36;
+ double c = 0.48;
+ double d = 0.8;
+ FieldRotation<DerivativeStructure> r = createRotation(a, b, c, d, true);
+ double a2 = a * a;
+ double b2 = b * b;
+ double c2 = c * c;
+ double d2 = d * d;
+ double den = (a2 + b2 + c2 + d2) * FastMath.sqrt(a2 + b2 + c2 + d2);
+ Assert.assertEquals((b2 + c2 + d2) / den, r.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(-a * b / den, r.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(-a * c / den, r.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(-a * d / den, r.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(-b * a / den, r.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals((a2 + c2 + d2) / den, r.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(-b * c / den, r.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(-b * d / den, r.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(-c * a / den, r.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(-c * b / den, r.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals((a2 + b2 + d2) / den, r.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(-c * d / den, r.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(-d * a / den, r.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(-d * b / den, r.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(-d * c / den, r.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals((a2 + b2 + c2) / den, r.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ FieldRotation<DerivativeStructure> reverted = r.revert();
+ FieldRotation<DerivativeStructure> rrT = r.compose(reverted, RotationConvention.VECTOR_OPERATOR);
+ checkRotationDS(rrT, 1, 0, 0, 0);
+ Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ FieldRotation<DerivativeStructure> rTr = reverted.compose(r, RotationConvention.VECTOR_OPERATOR);
+ checkRotationDS(rTr, 1, 0, 0, 0);
+ Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ 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(RotationConvention.VECTOR_OPERATOR),
+ reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(),
+ 1.0e-15);
+ }
+
+ @Test
+ public void testRevertFrameTransform() {
+ double a = 0.001;
+ double b = 0.36;
+ double c = 0.48;
+ double d = 0.8;
+ FieldRotation<DerivativeStructure> r = createRotation(a, b, c, d, true);
+ double a2 = a * a;
+ double b2 = b * b;
+ double c2 = c * c;
+ double d2 = d * d;
+ double den = (a2 + b2 + c2 + d2) * FastMath.sqrt(a2 + b2 + c2 + d2);
+ Assert.assertEquals((b2 + c2 + d2) / den, r.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(-a * b / den, r.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(-a * c / den, r.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(-a * d / den, r.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(-b * a / den, r.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals((a2 + c2 + d2) / den, r.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(-b * c / den, r.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(-b * d / den, r.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(-c * a / den, r.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(-c * b / den, r.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals((a2 + b2 + d2) / den, r.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(-c * d / den, r.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(-d * a / den, r.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(-d * b / den, r.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(-d * c / den, r.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals((a2 + b2 + c2) / den, r.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ FieldRotation<DerivativeStructure> reverted = r.revert();
+ FieldRotation<DerivativeStructure> rrT = r.compose(reverted, RotationConvention.FRAME_TRANSFORM);
+ checkRotationDS(rrT, 1, 0, 0, 0);
+ Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rrT.getQ3().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ FieldRotation<DerivativeStructure> rTr = reverted.compose(r, RotationConvention.FRAME_TRANSFORM);
+ checkRotationDS(rTr, 1, 0, 0, 0);
+ Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ0().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ1().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 1, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ2().getPartialDerivative(0, 0, 0, 1), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(1, 0, 0, 0), 1.0e-15);
+ Assert.assertEquals(0, rTr.getQ3().getPartialDerivative(0, 1, 0, 0), 1.0e-15);
+ 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(RotationConvention.FRAME_TRANSFORM),
+ reverted.getAxis(RotationConvention.FRAME_TRANSFORM)).getReal(),
+ 1.0e-15);
+ }
+
+ @Test
public void testVectorOnePair() throws MathArithmeticException {
FieldVector3D<DerivativeStructure> u = createVector(3, 2, 1);
@@ -642,7 +786,7 @@ public class FieldRotationDSTest {
}
@Test
- public void testCompose() throws MathIllegalArgumentException {
+ public void testApplyToRotation() throws MathIllegalArgumentException {
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
createAngle(1.7),
@@ -652,10 +796,39 @@ public class FieldRotationDSTest {
RotationConvention.VECTOR_OPERATOR);
FieldRotation<DerivativeStructure> r3 = r2.applyTo(r1);
FieldRotation<DerivativeStructure> r3Double = r2.applyTo(new Rotation(r1.getQ0().getReal(),
- r1.getQ1().getReal(),
- r1.getQ2().getReal(),
- r1.getQ3().getReal(),
- false));
+ r1.getQ1().getReal(),
+ r1.getQ2().getReal(),
+ r1.getQ3().getReal(),
+ false));
+
+ for (double x = -0.9; x < 0.9; x += 0.2) {
+ for (double y = -0.9; y < 0.9; y += 0.2) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
+ checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
+ checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
+ public void testComposeVectorOperator() throws MathIllegalArgumentException {
+
+ 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.compose(r1, RotationConvention.VECTOR_OPERATOR);
+ FieldRotation<DerivativeStructure> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
+ r1.getQ1().getReal(),
+ r1.getQ2().getReal(),
+ r1.getQ3().getReal(),
+ false),
+ 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) {
@@ -670,7 +843,36 @@ public class FieldRotationDSTest {
}
@Test
- public void testComposeInverse() throws MathIllegalArgumentException {
+ public void testComposeFrameTransform() throws MathIllegalArgumentException {
+
+ FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
+ createAngle(1.7),
+ RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
+ createAngle(0.3),
+ RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<DerivativeStructure> r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<DerivativeStructure> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
+ r1.getQ1().getReal(),
+ r1.getQ2().getReal(),
+ r1.getQ3().getReal(),
+ false),
+ RotationConvention.FRAME_TRANSFORM);
+
+ for (double x = -0.9; x < 0.9; x += 0.2) {
+ for (double y = -0.9; y < 0.9; y += 0.2) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
+ checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
+ checkVector(r1.applyTo(r2.applyTo(u)), r3Double.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
+ public void testApplyInverseToRotation() throws MathIllegalArgumentException {
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
createAngle(1.7),
@@ -680,10 +882,10 @@ public class FieldRotationDSTest {
RotationConvention.VECTOR_OPERATOR);
FieldRotation<DerivativeStructure> r3 = r2.applyInverseTo(r1);
FieldRotation<DerivativeStructure> r3Double = r2.applyInverseTo(new Rotation(r1.getQ0().getReal(),
- r1.getQ1().getReal(),
- r1.getQ2().getReal(),
- r1.getQ3().getReal(),
- false));
+ r1.getQ1().getReal(),
+ r1.getQ2().getReal(),
+ r1.getQ3().getReal(),
+ false));
for (double x = -0.9; x < 0.9; x += 0.2) {
for (double y = -0.9; y < 0.9; y += 0.2) {
@@ -698,6 +900,64 @@ public class FieldRotationDSTest {
}
@Test
+ public void testComposeInverseVectorOperator() throws MathIllegalArgumentException {
+
+ 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.composeInverse(r1, RotationConvention.VECTOR_OPERATOR);
+ FieldRotation<DerivativeStructure> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
+ r1.getQ1().getReal(),
+ r1.getQ2().getReal(),
+ r1.getQ3().getReal(),
+ false),
+ 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) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
+ checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
+ checkVector(r2.applyInverseTo(r1.applyTo(u)), r3Double.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
+ public void testComposeInverseframeTransform() throws MathIllegalArgumentException {
+
+ FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
+ createAngle(1.7),
+ RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2),
+ createAngle(0.3),
+ RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<DerivativeStructure> r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<DerivativeStructure> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
+ r1.getQ1().getReal(),
+ r1.getQ2().getReal(),
+ r1.getQ3().getReal(),
+ false),
+ RotationConvention.FRAME_TRANSFORM);
+
+ for (double x = -0.9; x < 0.9; x += 0.2) {
+ for (double y = -0.9; y < 0.9; y += 0.2) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ FieldVector3D<DerivativeStructure> u = createVector(x, y, z);
+ checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
+ checkVector(r1.applyTo(r2.applyInverseTo(u)), r3Double.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
public void testDoubleVectors() throws MathIllegalArgumentException {
Well1024a random = new Well1024a(0x180b41cfeeffaf67l);
@@ -752,9 +1012,9 @@ public class FieldRotationDSTest {
RotationConvention.VECTOR_OPERATOR);
FieldRotation<DerivativeStructure> rA = FieldRotation.applyTo(r1, r2);
- FieldRotation<DerivativeStructure> rB = r1Prime.applyTo(r2);
+ FieldRotation<DerivativeStructure> rB = r1Prime.compose(r2, RotationConvention.VECTOR_OPERATOR);
FieldRotation<DerivativeStructure> rC = FieldRotation.applyInverseTo(r1, r2);
- FieldRotation<DerivativeStructure> rD = r1Prime.applyInverseTo(r2);
+ FieldRotation<DerivativeStructure> rD = r1Prime.composeInverse(r2, 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) {
http://git-wip-us.apache.org/repos/asf/commons-math/blob/c9b1c8f9/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDfpTest.java
----------------------------------------------------------------------
diff --git a/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDfpTest.java b/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDfpTest.java
index 8f6738c..e7df1b0 100644
--- a/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDfpTest.java
+++ b/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDfpTest.java
@@ -193,6 +193,44 @@ public class FieldRotationDfpTest {
}
@Test
+ public void testRevertVectorOperator() {
+ double a = 0.001;
+ double b = 0.36;
+ double c = 0.48;
+ double d = 0.8;
+ FieldRotation<Dfp> r = createRotation(a, b, c, d, true);
+ FieldRotation<Dfp> reverted = r.revert();
+ FieldRotation<Dfp> rrT = r.compose(reverted, RotationConvention.VECTOR_OPERATOR);
+ checkRotationDS(rrT, 1, 0, 0, 0);
+ FieldRotation<Dfp> rTr = reverted.compose(r, RotationConvention.VECTOR_OPERATOR);
+ checkRotationDS(rTr, 1, 0, 0, 0);
+ Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
+ Assert.assertEquals(-1,
+ FieldVector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
+ reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(),
+ 1.0e-15);
+ }
+
+ @Test
+ public void testRevertFrameTransform() {
+ double a = 0.001;
+ double b = 0.36;
+ double c = 0.48;
+ double d = 0.8;
+ FieldRotation<Dfp> r = createRotation(a, b, c, d, true);
+ FieldRotation<Dfp> reverted = r.revert();
+ FieldRotation<Dfp> rrT = r.compose(reverted, RotationConvention.FRAME_TRANSFORM);
+ checkRotationDS(rrT, 1, 0, 0, 0);
+ FieldRotation<Dfp> rTr = reverted.compose(r, RotationConvention.FRAME_TRANSFORM);
+ checkRotationDS(rTr, 1, 0, 0, 0);
+ Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
+ Assert.assertEquals(-1,
+ FieldVector3D.dotProduct(r.getAxis(RotationConvention.FRAME_TRANSFORM),
+ reverted.getAxis(RotationConvention.FRAME_TRANSFORM)).getReal(),
+ 1.0e-15);
+ }
+
+ @Test
public void testVectorOnePair() throws MathArithmeticException {
FieldVector3D<Dfp> u = createVector(3, 2, 1);
@@ -585,7 +623,7 @@ public class FieldRotationDfpTest {
}
@Test
- public void testCompose() throws MathIllegalArgumentException {
+ public void testApplyToRotation() throws MathIllegalArgumentException {
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
createAngle(1.7),
@@ -613,7 +651,67 @@ public class FieldRotationDfpTest {
}
@Test
- public void testComposeInverse() throws MathIllegalArgumentException {
+ public void testComposeVectorOperator() throws MathIllegalArgumentException {
+
+ FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
+ createAngle(1.7),
+ RotationConvention.VECTOR_OPERATOR);
+ FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2),
+ createAngle(0.3),
+ RotationConvention.VECTOR_OPERATOR);
+ FieldRotation<Dfp> r3 = r2.compose(r1, RotationConvention.VECTOR_OPERATOR);
+ FieldRotation<Dfp> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
+ r1.getQ1().getReal(),
+ r1.getQ2().getReal(),
+ r1.getQ3().getReal(),
+ false),
+ 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) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ FieldVector3D<Dfp> u = createVector(x, y, z);
+ checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
+ checkVector(r2.applyTo(r1.applyTo(u)), r3Double.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
+ public void testComposeFrameTransform() throws MathIllegalArgumentException {
+
+ FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
+ createAngle(1.7),
+ RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2),
+ createAngle(0.3),
+ RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<Dfp> r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<Dfp> r3Double = r2.compose(new Rotation(r1.getQ0().getReal(),
+ r1.getQ1().getReal(),
+ r1.getQ2().getReal(),
+ r1.getQ3().getReal(),
+ false),
+ RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<Dfp> r4 = r1.compose(r2, RotationConvention.VECTOR_OPERATOR);
+ Assert.assertEquals(0.0, FieldRotation.distance(r3, r4).getReal(), 1.0e-15);
+
+ for (double x = -0.9; x < 0.9; x += 0.2) {
+ for (double y = -0.9; y < 0.9; y += 0.2) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ FieldVector3D<Dfp> u = createVector(x, y, z);
+ checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
+ checkVector(r1.applyTo(r2.applyTo(u)), r3Double.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
+ public void testApplyInverseToRotation() throws MathIllegalArgumentException {
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
createAngle(1.7),
@@ -641,6 +739,66 @@ public class FieldRotationDfpTest {
}
@Test
+ public void testComposeInverseVectorOperator() throws MathIllegalArgumentException {
+
+ FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
+ createAngle(1.7),
+ RotationConvention.VECTOR_OPERATOR);
+ FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2),
+ createAngle(0.3),
+ RotationConvention.VECTOR_OPERATOR);
+ FieldRotation<Dfp> r3 = r2.composeInverse(r1, RotationConvention.VECTOR_OPERATOR);
+ FieldRotation<Dfp> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
+ r1.getQ1().getReal(),
+ r1.getQ2().getReal(),
+ r1.getQ3().getReal(),
+ false),
+ 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) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ FieldVector3D<Dfp> u = createVector(x, y, z);
+ checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
+ checkVector(r2.applyInverseTo(r1.applyTo(u)), r3Double.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
+ public void testComposeInverseFrameTransform() throws MathIllegalArgumentException {
+
+ FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
+ createAngle(1.7),
+ RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2),
+ createAngle(0.3),
+ RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<Dfp> r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<Dfp> r3Double = r2.composeInverse(new Rotation(r1.getQ0().getReal(),
+ r1.getQ1().getReal(),
+ r1.getQ2().getReal(),
+ r1.getQ3().getReal(),
+ false),
+ RotationConvention.FRAME_TRANSFORM);
+ FieldRotation<Dfp> r4 = r1.revert().composeInverse(r2.revert(), RotationConvention.VECTOR_OPERATOR);
+ Assert.assertEquals(0.0, FieldRotation.distance(r3, r4).getReal(), 1.0e-15);
+
+ for (double x = -0.9; x < 0.9; x += 0.2) {
+ for (double y = -0.9; y < 0.9; y += 0.2) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ FieldVector3D<Dfp> u = createVector(x, y, z);
+ checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
+ checkVector(r1.applyTo(r2.applyInverseTo(u)), r3Double.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
public void testDoubleVectors() throws MathIllegalArgumentException {
Well1024a random = new Well1024a(0x180b41cfeeffaf67l);
@@ -696,9 +854,9 @@ public class FieldRotationDfpTest {
RotationConvention.VECTOR_OPERATOR);
FieldRotation<Dfp> rA = FieldRotation.applyTo(r1, r2);
- FieldRotation<Dfp> rB = r1Prime.applyTo(r2);
+ FieldRotation<Dfp> rB = r1Prime.compose(r2, RotationConvention.VECTOR_OPERATOR);
FieldRotation<Dfp> rC = FieldRotation.applyInverseTo(r1, r2);
- FieldRotation<Dfp> rD = r1Prime.applyInverseTo(r2);
+ FieldRotation<Dfp> rD = r1Prime.composeInverse(r2, RotationConvention.VECTOR_OPERATOR);
for (double x = -0.9; x < 0.9; x += 0.4) {
for (double y = -0.9; y < 0.9; y += 0.4) {
http://git-wip-us.apache.org/repos/asf/commons-math/blob/c9b1c8f9/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/RotationTest.java
----------------------------------------------------------------------
diff --git a/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/RotationTest.java b/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/RotationTest.java
index c7894fd..5f57326 100644
--- a/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/RotationTest.java
+++ b/src/test/java/org/apache/commons/math4/geometry/euclidean/threed/RotationTest.java
@@ -152,7 +152,7 @@ public class RotationTest {
}
@Test
- public void testRevert() {
+ public void testRevertDeprecated() {
Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
Rotation reverted = r.revert();
checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
@@ -165,6 +165,32 @@ public class RotationTest {
}
@Test
+ public void testRevertVectorOperator() {
+ Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
+ Rotation reverted = r.revert();
+ checkRotation(r.compose(reverted, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0);
+ checkRotation(reverted.compose(r, RotationConvention.VECTOR_OPERATOR), 1, 0, 0, 0);
+ Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
+ Assert.assertEquals(-1,
+ Vector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
+ reverted.getAxis(RotationConvention.VECTOR_OPERATOR)),
+ 1.0e-12);
+ }
+
+ @Test
+ public void testRevertFrameTransform() {
+ Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
+ Rotation reverted = r.revert();
+ checkRotation(r.compose(reverted, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0);
+ checkRotation(reverted.compose(r, RotationConvention.FRAME_TRANSFORM), 1, 0, 0, 0);
+ Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
+ Assert.assertEquals(-1,
+ Vector3D.dotProduct(r.getAxis(RotationConvention.FRAME_TRANSFORM),
+ reverted.getAxis(RotationConvention.FRAME_TRANSFORM)),
+ 1.0e-12);
+ }
+
+ @Test
public void testVectorOnePair() throws MathArithmeticException {
Vector3D u = new Vector3D(3, 2, 1);
@@ -529,7 +555,7 @@ public class RotationTest {
}
@Test
- public void testCompose() throws MathIllegalArgumentException {
+ public void testApplyTo() throws MathIllegalArgumentException {
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
@@ -547,7 +573,45 @@ public class RotationTest {
}
@Test
- public void testComposeInverse() throws MathIllegalArgumentException {
+ public void testComposeVectorOperator() throws MathIllegalArgumentException {
+
+ Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
+ Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
+ Rotation r3 = r2.compose(r1, 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) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ Vector3D u = new Vector3D(x, y, z);
+ checkVector(r2.applyTo(r1.applyTo(u)), r3.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
+ public void testComposeFrameTransform() throws MathIllegalArgumentException {
+
+ Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM);
+ Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM);
+ Rotation r3 = r2.compose(r1, RotationConvention.FRAME_TRANSFORM);
+ Rotation r4 = r1.compose(r2, RotationConvention.VECTOR_OPERATOR);
+ Assert.assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15);
+
+ for (double x = -0.9; x < 0.9; x += 0.2) {
+ for (double y = -0.9; y < 0.9; y += 0.2) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ Vector3D u = new Vector3D(x, y, z);
+ checkVector(r1.applyTo(r2.applyTo(u)), r3.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
+ public void testApplyInverseToRotation() throws MathIllegalArgumentException {
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
@@ -565,6 +629,44 @@ public class RotationTest {
}
@Test
+ public void testComposeInverseVectorOperator() throws MathIllegalArgumentException {
+
+ Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
+ Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
+ Rotation r3 = r2.composeInverse(r1, 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) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ Vector3D u = new Vector3D(x, y, z);
+ checkVector(r2.applyInverseTo(r1.applyTo(u)), r3.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
+ public void testComposeInverseFrameTransform() throws MathIllegalArgumentException {
+
+ Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.FRAME_TRANSFORM);
+ Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.FRAME_TRANSFORM);
+ Rotation r3 = r2.composeInverse(r1, RotationConvention.FRAME_TRANSFORM);
+ Rotation r4 = r1.revert().composeInverse(r2.revert(), RotationConvention.VECTOR_OPERATOR);
+ Assert.assertEquals(0.0, Rotation.distance(r3, r4), 1.0e-15);
+
+ for (double x = -0.9; x < 0.9; x += 0.2) {
+ for (double y = -0.9; y < 0.9; y += 0.2) {
+ for (double z = -0.9; z < 0.9; z += 0.2) {
+ Vector3D u = new Vector3D(x, y, z);
+ checkVector(r1.applyTo(r2.applyInverseTo(u)), r3.applyTo(u));
+ }
+ }
+ }
+
+ }
+
+ @Test
public void testArray() throws MathIllegalArgumentException {
Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
@@ -696,7 +798,9 @@ public class RotationTest {
final Rotation r1 = new Rotation(order.getA1(), zRotation, RotationConvention.FRAME_TRANSFORM);
final Rotation r2 = new Rotation(order.getA2(), yRotation, RotationConvention.FRAME_TRANSFORM);
final Rotation r3 = new Rotation(order.getA3(), xRotation, RotationConvention.FRAME_TRANSFORM);
- final Rotation composite = r3.applyTo(r2.applyTo(r1));
+ final Rotation composite = r1.compose(r2.compose(r3,
+ RotationConvention.FRAME_TRANSFORM),
+ RotationConvention.FRAME_TRANSFORM);
final Vector3D good = composite.applyTo(startingVector);
Assert.assertEquals(good.getX(), appliedIndividually.getX(), 1e-12);