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
This commit is contained in:
Luc Maisonobe 2015-12-27 13:09:13 +01:00
parent 9ce4e1a371
commit c9b1c8f966
6 changed files with 846 additions and 123 deletions

View File

@ -363,7 +363,8 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
* widespread in the aerospace business where Roll, Pitch and Yaw angles * widespread in the aerospace business where Roll, Pitch and Yaw angles
* are often wrongly tagged as Euler angles).</p> * 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 convention convention to use for the semantics of the angle
* @param alpha1 angle of the first elementary rotation * @param alpha1 angle of the first elementary rotation
* @param alpha2 angle of the second 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> 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> 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> r3 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA3()), alpha3, convention);
final FieldRotation<T> composed = convention == RotationConvention.FRAME_TRANSFORM ? final FieldRotation<T> composed = r1.compose(r2.compose(r3, convention), convention);
r3.applyTo(r2.applyTo(r1)) :
r1.applyTo(r2.applyTo(r3));
q0 = composed.q0; q0 = composed.q0;
q1 = composed.q1; q1 = composed.q1;
q2 = composed.q2; q2 = composed.q2;
@ -1271,15 +1270,53 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
} }
/** Apply the instance to another rotation. /** Apply the instance to another rotation.
* Applying the instance to a rotation is computing the composition * <p>
* in an order compliant with the following rule : let u be any * Calling this method is equivalent to call
* vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image * {@link #compose(FieldRotation, RotationConvention)
* of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u), * compose(r, RotationConvention.VECTOR_OPERATOR)}.
* where comp = applyTo(r). * </p>
* @param r rotation to apply the rotation to * @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the instance * @return a new rotation which is the composition of r by the instance
*/ */
public FieldRotation<T> applyTo(final FieldRotation<T> r) { 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))), 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.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))), r.q2.multiply(q0).add(r.q0.multiply(q2)).add(r.q3.multiply(q1).subtract(r.q1.multiply(q3))),
@ -1288,15 +1325,53 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
} }
/** Apply the instance to another rotation. /** Apply the instance to another rotation.
* Applying the instance to a rotation is computing the composition * <p>
* in an order compliant with the following rule : let u be any * Calling this method is equivalent to call
* vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image * {@link #compose(Rotation, RotationConvention)
* of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u), * compose(r, RotationConvention.VECTOR_OPERATOR)}.
* where comp = applyTo(r). * </p>
* @param r rotation to apply the rotation to * @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the instance * @return a new rotation which is the composition of r by the instance
*/ */
public FieldRotation<T> applyTo(final Rotation r) { 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()))), 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.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.getQ2()).add(q2.multiply(r.getQ0())).add(q1.multiply(r.getQ3()).subtract(q3.multiply(r.getQ1()))),
@ -1324,17 +1399,57 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
} }
/** Apply the inverse of the instance to another rotation. /** Apply the inverse of the instance to another rotation.
* Applying the inverse of the instance to a rotation is computing * <p>
* the composition in an order compliant with the following rule : * Calling this method is equivalent to call
* let u be any vector and v its image by r (i.e. r.applyTo(u) = v), * {@link #composeInverse(FieldRotation<T>, RotationConvention)
* let w be the inverse image of v by the instance * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}.
* (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where * </p>
* comp = applyInverseTo(r).
* @param r rotation to apply the rotation to * @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the inverse * @return a new rotation which is the composition of r by the inverse
* of the instance * of the instance
*/ */
public FieldRotation<T> applyInverseTo(final FieldRotation<T> r) { 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(), 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(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)), 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. /** Apply the inverse of the instance to another rotation.
* Applying the inverse of the instance to a rotation is computing * <p>
* the composition in an order compliant with the following rule : * Calling this method is equivalent to call
* let u be any vector and v its image by r (i.e. r.applyTo(u) = v), * {@link #composeInverse(Rotation, RotationConvention)
* let w be the inverse image of v by the instance * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}.
* (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where * </p>
* comp = applyInverseTo(r).
* @param r rotation to apply the rotation to * @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the inverse * @return a new rotation which is the composition of r by the inverse
* of the instance * of the instance
*/ */
public FieldRotation<T> applyInverseTo(final Rotation r) { 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(), 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())), 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())), 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 * @return <i>distance</i> between r1 and r2
*/ */
public static <T extends RealFieldElement<T>> T distance(final FieldRotation<T> r1, final FieldRotation<T> 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();
} }
} }

View File

@ -151,22 +151,11 @@ public class Rotation implements Serializable {
} }
/** Build a rotation from an axis and an angle. /** Build a rotation from an axis and an angle.
* <p>We use the convention that angles are oriented according to * <p>
* the effect of the rotation on vectors around the axis. That means * Calling this constructor is equivalent to call
* that if (i, j, k) is a direct frame and if we first provide +k as * {@link #Rotation(Vector3D, double, RotationConvention)
* the axis and &pi;/2 as the angle to this constructor, and then * new Rotation(axis, angle, RotationConvention.VECTOR_OPERATOR)}
* {@link #applyTo(Vector3D) apply} the instance to +i, we will get * </p>
* +j.</p>
* <p>Another way to represent our convention is to say that a rotation
* of angle &theta; about the unit vector (x, y, z) is the same as the
* rotation build from quaternion components { cos(-&theta;/2),
* x * sin(-&theta;/2), y * sin(-&theta;/2), z * sin(-&theta;/2) }.
* Note the minus sign on the angle!</p>
* <p>On the one hand this convention is consistent with a vectorial
* perspective (moving vectors in fixed frames), on the other hand it
* is different from conventions with a frame perspective (fixed vectors
* viewed from different frames) like the ones used for example in spacecraft
* attitude community or in the graphics community.</p>
* @param axis axis around which to rotate * @param axis axis around which to rotate
* @param angle rotation angle. * @param angle rotation angle.
* @exception MathIllegalArgumentException if the axis norm is zero * @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. /** Build a rotation from three Cardan or Euler elementary rotations.
* <p>Cardan rotations are three successive rotations around the * <p>
* canonical axes X, Y and Z, each axis being used once. There are * Calling this constructor is equivalent to call
* 6 such sets of rotations (XYZ, XZY, YXZ, YZX, ZXY and ZYX). Euler * {@link #Rotation(RotationOrder, RotationConvention, double, double, double)
* rotations are three successive rotations around the canonical * new Rotation(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3)}
* axes X, Y and Z, the first and last rotations being around the * </p>
* same axis. There are 6 such sets of rotations (XYX, XZX, YXY,
* YZY, ZXZ and ZYZ), the most popular one being ZXZ.</p>
* <p>Beware that many people routinely use the term Euler angles even
* for what really are Cardan angles (this confusion is especially
* widespread in the aerospace business where Roll, Pitch and Yaw angles
* are often wrongly tagged as Euler angles).</p>
* @param order order of rotations to use * @param order order of rotations to use
* @param alpha1 angle of the first elementary rotation * @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 * widespread in the aerospace business where Roll, Pitch and Yaw angles
* are often wrongly tagged as Euler angles).</p> * 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 convention convention to use for the semantics of the angle
* @param alpha1 angle of the first elementary rotation * @param alpha1 angle of the first elementary rotation
* @param alpha2 angle of the second 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 r1 = new Rotation(order.getA1(), alpha1, convention);
Rotation r2 = new Rotation(order.getA2(), alpha2, convention); Rotation r2 = new Rotation(order.getA2(), alpha2, convention);
Rotation r3 = new Rotation(order.getA3(), alpha3, convention); Rotation r3 = new Rotation(order.getA3(), alpha3, convention);
Rotation composed = convention == RotationConvention.FRAME_TRANSFORM ? Rotation composed = r1.compose(r2.compose(r3, convention), convention);
r3.applyTo(r2.applyTo(r1)) :
r1.applyTo(r2.applyTo(r3));
q0 = composed.q0; q0 = composed.q0;
q1 = composed.q1; q1 = composed.q1;
q2 = composed.q2; q2 = composed.q2;
@ -531,6 +513,10 @@ public class Rotation implements Serializable {
} }
/** Get the normalized axis of the rotation. /** 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 * @return normalized axis of the rotation
* @see #Rotation(Vector3D, double, RotationConvention) * @see #Rotation(Vector3D, double, RotationConvention)
* @deprecated as of 3.6, replaced with {@link #getAxis(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. /** Get the Cardan or Euler angles corresponding to the instance.
* <p>The equations show that each rotation can be defined by two * <p>
* different values of the Cardan or Euler angles set. For example * Calling this method is equivalent to call
* if Cardan angles are used, the rotation defined by the angles * {@link #getAngles(RotationOrder, RotationConvention)
* a<sub>1</sub>, a<sub>2</sub> and a<sub>3</sub> is the same as * getAngles(order, RotationConvention.VECTOR_OPERATOR)}
* the rotation defined by the angles &pi; + a<sub>1</sub>, &pi; * </p>
* - a<sub>2</sub> and &pi; + 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 -&pi;/2 and &pi;/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 &pi; (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
* -&pi;/2 or +&pi;/2, for Euler angle singularities occur when the
* second angle is close to 0 or &pi;, this implies that the identity
* rotation is always singular for Euler angles!</p>
* @param order rotation order to use * @param order rotation order to use
* @return an array of three angles, in the order specified by the set * @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. /** Apply the instance to another rotation.
* Applying the instance to a rotation is computing the composition * <p>
* in an order compliant with the following rule : let u be any * Calling this method is equivalent to call
* vector and v its image by r (i.e. r.applyTo(u) = v), let w be the image * {@link #compose(Rotation, RotationConvention)
* of v by the instance (i.e. applyTo(v) = w), then w = comp.applyTo(u), * compose(r, RotationConvention.VECTOR_OPERATOR)}.
* where comp = applyTo(r). * </p>
* @param r rotation to apply the rotation to * @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the instance * @return a new rotation which is the composition of r by the instance
*/ */
public Rotation applyTo(Rotation r) { 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), 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.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2),
r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3), 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. /** Apply the inverse of the instance to another rotation.
* Applying the inverse of the instance to a rotation is computing * <p>
* the composition in an order compliant with the following rule : * Calling this method is equivalent to call
* let u be any vector and v its image by r (i.e. r.applyTo(u) = v), * {@link #composeInverse(Rotation, RotationConvention)
* let w be the inverse image of v by the instance * composeInverse(r, RotationConvention.VECTOR_OPERATOR)}.
* (i.e. applyInverseTo(v) = w), then w = comp.applyTo(u), where * </p>
* comp = applyInverseTo(r).
* @param r rotation to apply the rotation to * @param r rotation to apply the rotation to
* @return a new rotation which is the composition of r by the inverse * @return a new rotation which is the composition of r by the inverse
* of the instance * of the instance
*/ */
public Rotation applyInverseTo(Rotation r) { 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), 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.q1 * q0 + r.q0 * q1 + (r.q2 * q3 - r.q3 * q2),
-r.q2 * q0 + r.q0 * q2 + (r.q3 * q1 - r.q1 * q3), -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 * @return <i>distance</i> between r1 and r2
*/ */
public static double distance(Rotation r1, Rotation r2) { public static double distance(Rotation r1, Rotation r2) {
return r1.applyInverseTo(r2).getAngle(); return r1.composeInverseInternal(r2).getAngle();
} }
} }

View File

@ -187,8 +187,12 @@
previous notations, we would say we can apply <code>r<sub>1</sub></code> to 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 = <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 r<sub>1</sub> o r<sub>2</sub></code>. For this purpose, the class
provides the methods: <code>applyTo(Rotation)</code> and provides the methods: <code>compose(Rotation, RotationConvention)</code> and
<code>applyInverseTo(Rotation)</code>. <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> </p>
</subsection> </subsection>
<subsection name="11.3 n-Sphere" href="sphere"> <subsection name="11.3 n-Sphere" href="sphere">

View File

@ -246,6 +246,150 @@ public class FieldRotationDSTest {
1.0e-15); 1.0e-15);
} }
@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 @Test
public void testVectorOnePair() throws MathArithmeticException { public void testVectorOnePair() throws MathArithmeticException {
@ -642,7 +786,7 @@ public class FieldRotationDSTest {
} }
@Test @Test
public void testCompose() throws MathIllegalArgumentException { public void testApplyToRotation() throws MathIllegalArgumentException {
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
createAngle(1.7), createAngle(1.7),
@ -670,7 +814,65 @@ public class FieldRotationDSTest {
} }
@Test @Test
public void testComposeInverse() throws MathIllegalArgumentException { 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) {
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 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), FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
createAngle(1.7), createAngle(1.7),
@ -697,6 +899,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 @Test
public void testDoubleVectors() throws MathIllegalArgumentException { public void testDoubleVectors() throws MathIllegalArgumentException {
@ -752,9 +1012,9 @@ public class FieldRotationDSTest {
RotationConvention.VECTOR_OPERATOR); RotationConvention.VECTOR_OPERATOR);
FieldRotation<DerivativeStructure> rA = FieldRotation.applyTo(r1, r2); 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> 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 x = -0.9; x < 0.9; x += 0.2) {
for (double y = -0.9; y < 0.9; y += 0.2) { for (double y = -0.9; y < 0.9; y += 0.2) {

View File

@ -192,6 +192,44 @@ public class FieldRotationDfpTest {
1.0e-15); 1.0e-15);
} }
@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 @Test
public void testVectorOnePair() throws MathArithmeticException { public void testVectorOnePair() throws MathArithmeticException {
@ -585,7 +623,7 @@ public class FieldRotationDfpTest {
} }
@Test @Test
public void testCompose() throws MathIllegalArgumentException { public void testApplyToRotation() throws MathIllegalArgumentException {
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5), FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
createAngle(1.7), createAngle(1.7),
@ -613,7 +651,67 @@ public class FieldRotationDfpTest {
} }
@Test @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), FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
createAngle(1.7), createAngle(1.7),
@ -640,6 +738,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 @Test
public void testDoubleVectors() throws MathIllegalArgumentException { public void testDoubleVectors() throws MathIllegalArgumentException {
@ -696,9 +854,9 @@ public class FieldRotationDfpTest {
RotationConvention.VECTOR_OPERATOR); RotationConvention.VECTOR_OPERATOR);
FieldRotation<Dfp> rA = FieldRotation.applyTo(r1, r2); 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> 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 x = -0.9; x < 0.9; x += 0.4) {
for (double y = -0.9; y < 0.9; y += 0.4) { for (double y = -0.9; y < 0.9; y += 0.4) {

View File

@ -152,7 +152,7 @@ public class RotationTest {
} }
@Test @Test
public void testRevert() { public void testRevertDeprecated() {
Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true); Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
Rotation reverted = r.revert(); Rotation reverted = r.revert();
checkRotation(r.applyTo(reverted), 1, 0, 0, 0); checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
@ -164,6 +164,32 @@ public class RotationTest {
1.0e-12); 1.0e-12);
} }
@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 @Test
public void testVectorOnePair() throws MathArithmeticException { public void testVectorOnePair() throws MathArithmeticException {
@ -529,7 +555,7 @@ public class RotationTest {
} }
@Test @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 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 r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
@ -547,7 +573,45 @@ public class RotationTest {
} }
@Test @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 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 r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3, RotationConvention.VECTOR_OPERATOR);
@ -564,6 +628,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 @Test
public void testArray() throws MathIllegalArgumentException { public void testArray() throws MathIllegalArgumentException {
@ -696,7 +798,9 @@ public class RotationTest {
final Rotation r1 = new Rotation(order.getA1(), zRotation, RotationConvention.FRAME_TRANSFORM); final Rotation r1 = new Rotation(order.getA1(), zRotation, RotationConvention.FRAME_TRANSFORM);
final Rotation r2 = new Rotation(order.getA2(), yRotation, 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 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); final Vector3D good = composite.applyTo(startingVector);
Assert.assertEquals(good.getX(), appliedIndividually.getX(), 1e-12); Assert.assertEquals(good.getX(), appliedIndividually.getX(), 1e-12);