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
parent
9ce4e1a371
commit
c9b1c8f966
|
|
@ -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();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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">
|
||||
|
|
|
|||
|
|
@ -246,6 +246,150 @@ public class FieldRotationDSTest {
|
|||
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
|
||||
public void testVectorOnePair() throws MathArithmeticException {
|
||||
|
||||
|
|
@ -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,10 @@ 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) {
|
||||
|
|
@ -670,7 +814,65 @@ public class FieldRotationDSTest {
|
|||
}
|
||||
|
||||
@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),
|
||||
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) {
|
||||
|
|
@ -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
|
||||
public void testDoubleVectors() throws MathIllegalArgumentException {
|
||||
|
||||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -192,6 +192,44 @@ public class FieldRotationDfpTest {
|
|||
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
|
||||
public void testVectorOnePair() throws MathArithmeticException {
|
||||
|
||||
|
|
@ -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),
|
||||
|
|
@ -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
|
||||
public void testDoubleVectors() throws MathIllegalArgumentException {
|
||||
|
||||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
@ -164,6 +164,32 @@ public class RotationTest {
|
|||
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
|
||||
public void testVectorOnePair() throws MathArithmeticException {
|
||||
|
||||
|
|
@ -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);
|
||||
|
|
@ -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
|
||||
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 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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue