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 <l...@apache.org> Authored: Sun Dec 27 13:09:13 2015 +0100 Committer: Luc Maisonobe <l...@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);
