Apache
/
commons-math
mirror of https://github.com/apache/commons-math.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added a RotationConvention enumerate. 

This enumerate allows specifying the semantics or axis/angle for
rotations. This enumerate has two values: VECTOR_OPERATOR and
FRAME_TRANSFORM.
    
JIRA: MATH-1302, MATH-1303
This commit is contained in:
Luc Maisonobe 2015-12-26 21:25:16 +01:00
parent afa1202271
commit a7294ccd79
13 changed files with 1853 additions and 600 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/changes/changes.xml
View File

@ -54,6 +54,11 @@ If the output is not quite correct, check for invisible trailing spaces!
</release>
<release version="4.0" date="XXXX-XX-XX" description="">
<action dev="luc" type="add" issue="MATH-1302,MATH-1303"> <!-- backported to 3.6 -->
Added a RotationConvention enumerate to allow specifying the semantics
or axis/angle for rotations. This enumerate has two values:
VECTOR_OPERATOR and FRAME_TRANSFORM.
</action>
<action dev="luc" type="fix" due-to="Julien Queyrel"> <!-- backported to 3.6 -->
Fixed stability issues with Adams-Bashforth and Adams-Moulton ODE integrators.
The integrators did not estimate the local error properly and were sometimes

733
src/main/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotation.java
View File

@ -111,16 +111,47 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
* @param axis axis around which to rotate
* @param angle rotation angle.
* @exception MathIllegalArgumentException if the axis norm is zero
* @deprecated as of 3.6, replaced with {@link
* #FieldRotation(FieldVector3D, RealFieldElement, RotationConvention)
*/
@Deprecated
public FieldRotation(final FieldVector3D<T> axis, final T angle)
throws MathIllegalArgumentException {
this(axis, angle, RotationConvention.VECTOR_OPERATOR);
}
/** Build a rotation from an axis and an angle.
* <p>We use the convention that angles are oriented according to
* the effect of the rotation on vectors around the axis. That means
* that if (i, j, k) is a direct frame and if we first provide +k as
* the axis and &pi;/2 as the angle to this constructor, and then
* {@link #applyTo(FieldVector3D) apply} the instance to +i, we will get
* +j.</p>
* <p>Another way to represent our convention is to say that a rotation
* of angle &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 angle rotation angle.
* @param convention convention to use for the semantics of the angle
* @exception MathIllegalArgumentException if the axis norm is zero
* @since 3.6
*/
public FieldRotation(final FieldVector3D<T> axis, final T angle, final RotationConvention convention)
throws MathIllegalArgumentException {
final T norm = axis.getNorm();
if (norm.getReal() == 0) {
throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_AXIS);
}
final T halfAngle = angle.multiply(-0.5);
final T halfAngle = angle.multiply(convention == RotationConvention.VECTOR_OPERATOR ? -0.5 : 0.5);
final T coeff = halfAngle.sin().divide(norm);
q0 = halfAngle.cos();
@ -194,7 +225,7 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
}
/** Build the rotation that transforms a pair of vector into another pair.
/** Build the rotation that transforms a pair of vectors into another pair.
* <p>Except for possible scale factors, if the instance were applied to
* the pair (u<sub>1</sub>, u<sub>2</sub>) it will produce the pair
@ -203,27 +234,27 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
* <p>If the angular separation between u<sub>1</sub> and u<sub>2</sub> is
* not the same as the angular separation between v<sub>1</sub> and
* v<sub>2</sub>, then a corrected v'<sub>2</sub> will be used rather than
* v<sub>2</sub>, the corrected vector will be in the (v<sub>1</sub>,
* v<sub>2</sub>) plane.</p>
* v<sub>2</sub>, the corrected vector will be in the (&pm;v<sub>1</sub>,
* +v<sub>2</sub>) half-plane.</p>
* @param u1 first vector of the origin pair
* @param u2 second vector of the origin pair
* @param v1 desired image of u1 by the rotation
* @param v2 desired image of u2 by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero,
* or if one of the pair is degenerated (i.e. the vectors of the pair are colinear)
* or if one of the pair is degenerated (i.e. the vectors of the pair are collinear)
*/
public FieldRotation(FieldVector3D<T> u1, FieldVector3D<T> u2, FieldVector3D<T> v1, FieldVector3D<T> v2)
throws MathArithmeticException {
// build orthonormalized base from u1, u2
// this fails when vectors are null or colinear, which is forbidden to define a rotation
// this fails when vectors are null or collinear, which is forbidden to define a rotation
final FieldVector3D<T> u3 = FieldVector3D.crossProduct(u1, u2).normalize();
u2 = FieldVector3D.crossProduct(u3, u1).normalize();
u1 = u1.normalize();
// build an orthonormalized base from v1, v2
// this fails when vectors are null or colinear, which is forbidden to define a rotation
// this fails when vectors are null or collinear, which is forbidden to define a rotation
final FieldVector3D<T> v3 = FieldVector3D.crossProduct(v1, v2).normalize();
v2 = FieldVector3D.crossProduct(v3, v1).normalize();
v1 = v1.normalize();
@ -254,7 +285,7 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* is orthogonal to the (u, v) plane). If u and v are collinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
@ -309,13 +340,45 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
* @param alpha1 angle of the first elementary rotation
* @param alpha2 angle of the second elementary rotation
* @param alpha3 angle of the third elementary rotation
* @deprecated as of 3.6, replaced with {@link
* #FieldRotation(RotationOrder, RotationConvention,
* RealFieldElement, RealFieldElement, RealFieldElement)}
*/
@Deprecated
public FieldRotation(final RotationOrder order, final T alpha1, final T alpha2, final T alpha3) {
this(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3);
}
/** Build a rotation from three Cardan or Euler elementary rotations.
* <p>Cardan rotations are three successive rotations around the
* canonical axes X, Y and Z, each axis being used once. There are
* 6 such sets of rotations (XYZ, XZY, YXZ, YZX, ZXY and ZYX). Euler
* rotations are three successive rotations around the canonical
* axes X, Y and Z, the first and last rotations being around the
* same axis. There are 6 such sets of rotations (XYX, XZX, YXY,
* YZY, ZXZ and ZYZ), the most popular one being ZXZ.</p>
* <p>Beware that many people routinely use the term Euler angles even
* for what really are Cardan angles (this confusion is especially
* widespread in the aerospace business where Roll, Pitch and Yaw angles
* are often wrongly tagged as Euler angles).</p>
* @param order order of rotations to use
* @param convention convention to use for the semantics of the angle
* @param alpha1 angle of the first elementary rotation
* @param alpha2 angle of the second elementary rotation
* @param alpha3 angle of the third elementary rotation
* @since 3.6
*/
public FieldRotation(final RotationOrder order, final RotationConvention convention,
final T alpha1, final T alpha2, final T alpha3) {
final T one = alpha1.getField().getOne();
final FieldRotation<T> r1 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA1()), alpha1);
final FieldRotation<T> r2 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA2()), alpha2);
final FieldRotation<T> r3 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA3()), alpha3);
final FieldRotation<T> composed = r1.applyTo(r2.applyTo(r3));
final FieldRotation<T> r1 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA1()), alpha1, convention);
final FieldRotation<T> r2 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA2()), alpha2, convention);
final FieldRotation<T> r3 = new FieldRotation<T>(new FieldVector3D<T>(one, order.getA3()), alpha3, convention);
final FieldRotation<T> composed = convention == RotationConvention.FRAME_TRANSFORM ?
r3.applyTo(r2.applyTo(r1)) :
r1.applyTo(r2.applyTo(r3));
q0 = composed.q0;
q1 = composed.q1;
q2 = composed.q2;
@ -425,18 +488,40 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
/** Get the normalized axis of the rotation.
* @return normalized axis of the rotation
* @see #FieldRotation(FieldVector3D, RealFieldElement)
* @deprecated as of 3.6, replaced with {@link #getAxis(RotationConvention)}
*/
@Deprecated
public FieldVector3D<T> getAxis() {
return getAxis(RotationConvention.VECTOR_OPERATOR);
}
/** Get the normalized axis of the rotation.
* <p>
* Note that as {@link #getAngle()} always returns an angle
* between 0 and &pi;, changing the convention changes the
* direction of the axis, not the sign of the angle.
* </p>
* @param convention convention to use for the semantics of the angle
* @return normalized axis of the rotation
* @see #FieldRotation(FieldVector3D, RealFieldElement)
* @since 3.6
*/
public FieldVector3D<T> getAxis(final RotationConvention convention) {
final T squaredSine = q1.multiply(q1).add(q2.multiply(q2)).add(q3.multiply(q3));
if (squaredSine.getReal() == 0) {
final Field<T> field = squaredSine.getField();
return new FieldVector3D<T>(field.getOne(), field.getZero(), field.getZero());
} else if (q0.getReal() < 0) {
T inverse = squaredSine.sqrt().reciprocal();
return new FieldVector3D<T>(convention == RotationConvention.VECTOR_OPERATOR ? field.getOne(): field.getOne().negate(),
field.getZero(),
field.getZero());
} else {
final double sgn = convention == RotationConvention.VECTOR_OPERATOR ? +1 : -1;
if (q0.getReal() < 0) {
T inverse = squaredSine.sqrt().reciprocal().multiply(sgn);
return new FieldVector3D<T>(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse));
}
final T inverse = squaredSine.sqrt().reciprocal().negate().multiply(sgn);
return new FieldVector3D<T>(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse));
}
final T inverse = squaredSine.sqrt().reciprocal().negate();
return new FieldVector3D<T>(q1.multiply(inverse), q2.multiply(inverse), q3.multiply(inverse));
}
/** Get the angle of the rotation.
@ -486,202 +571,442 @@ public class FieldRotation<T extends RealFieldElement<T>> implements Serializabl
* @return an array of three angles, in the order specified by the set
* @exception CardanEulerSingularityException if the rotation is
* singular with respect to the angles set specified
* @deprecated as of 3.6, replaced with {@link #getAngles(RotationOrder, RotationConvention)}
*/
@Deprecated
public T[] getAngles(final RotationOrder order)
throws CardanEulerSingularityException {
return getAngles(order, RotationConvention.VECTOR_OPERATOR);
}
if (order == RotationOrder.XYZ) {
/** 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 &pi; + a<sub>1</sub>, &pi;
* - 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 convention convention to use for the semantics of the angle
* @return an array of three angles, in the order specified by the set
* @exception CardanEulerSingularityException if the rotation is
* singular with respect to the angles set specified
* @since 3.6
*/
public T[] getAngles(final RotationOrder order, RotationConvention convention)
throws CardanEulerSingularityException {
if (convention == RotationConvention.VECTOR_OPERATOR) {
if (order == RotationOrder.XYZ) {
// r (+K) coordinates are :
// sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi)
// (-r) (+I) coordinates are :
// cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta)
final // and we can choose to have theta in the interval [-PI/2 ; +PI/2]
FieldVector3D<T> v1 = applyTo(vector(0, 0, 1));
final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0));
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getY().negate().atan2(v1.getZ()),
v2.getZ().asin(),
v2.getY().negate().atan2(v2.getX()));
} else if (order == RotationOrder.XZY) {
// r (+J) coordinates are :
// -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi)
// (-r) (+I) coordinates are :
// cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0));
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getZ().atan2(v1.getY()),
v2.getY().asin().negate(),
v2.getZ().atan2(v2.getX()));
} else if (order == RotationOrder.YXZ) {
// r (+K) coordinates are :
// cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta)
// (-r) (+J) coordinates are :
// sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0));
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getX().atan2(v1.getZ()),
v2.getZ().asin().negate(),
v2.getX().atan2(v2.getY()));
} else if (order == RotationOrder.YZX) {
// r (+I) coordinates are :
// cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta)
// (-r) (+J) coordinates are :
// sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0));
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getZ().negate().atan2(v1.getX()),
v2.getX().asin(),
v2.getZ().negate().atan2(v2.getY()));
} else if (order == RotationOrder.ZXY) {
// r (+J) coordinates are :
// -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi)
// (-r) (+K) coordinates are :
// -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1));
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getX().negate().atan2(v1.getY()),
v2.getY().asin(),
v2.getX().negate().atan2(v2.getZ()));
} else if (order == RotationOrder.ZYX) {
// r (+I) coordinates are :
// cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta)
// (-r) (+K) coordinates are :
// -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta)
// and we can choose to have theta in the interval [-PI/2 ; +PI/2]
final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1));
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getY().atan2(v1.getX()),
v2.getX().asin().negate(),
v2.getY().atan2(v2.getZ()));
} else if (order == RotationOrder.XYX) {
// r (+I) coordinates are :
// cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta)
// (-r) (+I) coordinates are :
// cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2)
// and we can choose to have theta in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0));
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getY().atan2(v1.getZ().negate()),
v2.getX().acos(),
v2.getY().atan2(v2.getZ()));
} else if (order == RotationOrder.XZX) {
// r (+I) coordinates are :
// cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi)
// (-r) (+I) coordinates are :
// cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2)
// and we can choose to have psi in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0));
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getZ().atan2(v1.getY()),
v2.getX().acos(),
v2.getZ().atan2(v2.getY().negate()));
} else if (order == RotationOrder.YXY) {
// r (+J) coordinates are :
// sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi)
// (-r) (+J) coordinates are :
// sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2)
// and we can choose to have phi in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0));
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getX().atan2(v1.getZ()),
v2.getY().acos(),
v2.getX().atan2(v2.getZ().negate()));
} else if (order == RotationOrder.YZY) {
// r (+J) coordinates are :
// -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi)
// (-r) (+J) coordinates are :
// sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2)
// and we can choose to have psi in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0));
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getZ().atan2(v1.getX().negate()),
v2.getY().acos(),
v2.getZ().atan2(v2.getX()));
} else if (order == RotationOrder.ZXZ) {
// r (+K) coordinates are :
// sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi)
// (-r) (+K) coordinates are :
// sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi)
// and we can choose to have phi in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1));
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getX().atan2(v1.getY().negate()),
v2.getZ().acos(),
v2.getX().atan2(v2.getY()));
} else { // last possibility is ZYZ
// r (+K) coordinates are :
// cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta)
// (-r) (+K) coordinates are :
// -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta)
// and we can choose to have theta in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1));
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getY().atan2(v1.getX()),
v2.getZ().acos(),
v2.getY().atan2(v2.getX().negate()));
// r (+K) coordinates are :
// sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi)
// (-r) (+I) coordinates are :
// cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta)
final // and we can choose to have theta in the interval [-PI/2 ; +PI/2]
FieldVector3D<T> v1 = applyTo(vector(0, 0, 1));
final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0));
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getY().negate().atan2(v1.getZ()),
v2.getZ().asin(),
v2.getY().negate().atan2(v2.getX()));
} else {
if (order == RotationOrder.XYZ) {
} else if (order == RotationOrder.XZY) {
// r (Vector3D.plusI) coordinates are :
// cos (theta) cos (psi), -cos (theta) sin (psi), sin (theta)
// (-r) (Vector3D.plusK) coordinates are :
// sin (theta), -sin (phi) cos (theta), cos (phi) cos (theta)
// and we can choose to have theta in the interval [-PI/2 ; +PI/2]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v2.getY().negate().atan2(v2.getZ()),
v2.getX().asin(),
v1.getY().negate().atan2(v1.getX()));
} else if (order == RotationOrder.XZY) {
// r (Vector3D.plusI) coordinates are :
// cos (psi) cos (theta), -sin (psi), cos (psi) sin (theta)
// (-r) (Vector3D.plusJ) coordinates are :
// -sin (psi), cos (phi) cos (psi), sin (phi) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v2.getZ().atan2(v2.getY()),
v2.getX().asin().negate(),
v1.getZ().atan2(v1.getX()));
} else if (order == RotationOrder.YXZ) {
// r (Vector3D.plusJ) coordinates are :
// cos (phi) sin (psi), cos (phi) cos (psi), -sin (phi)
// (-r) (Vector3D.plusK) coordinates are :
// sin (theta) cos (phi), -sin (phi), cos (theta) cos (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v2.getX().atan2(v2.getZ()),
v2.getY().asin().negate(),
v1.getX().atan2(v1.getY()));
} else if (order == RotationOrder.YZX) {
// r (Vector3D.plusJ) coordinates are :
// sin (psi), cos (psi) cos (phi), -cos (psi) sin (phi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (theta) cos (psi), sin (psi), -sin (theta) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v2.getZ().negate().atan2(v2.getX()),
v2.getY().asin(),
v1.getZ().negate().atan2(v1.getY()));
} else if (order == RotationOrder.ZXY) {
// r (Vector3D.plusK) coordinates are :
// -cos (phi) sin (theta), sin (phi), cos (phi) cos (theta)
// (-r) (Vector3D.plusJ) coordinates are :
// -sin (psi) cos (phi), cos (psi) cos (phi), sin (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v2.getX().negate().atan2(v2.getY()),
v2.getZ().asin(),
v1.getX().negate().atan2(v1.getZ()));
} else if (order == RotationOrder.ZYX) {
// r (Vector3D.plusK) coordinates are :
// -sin (theta), cos (theta) sin (phi), cos (theta) cos (phi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (psi) cos (theta), sin (psi) cos (theta), -sin (theta)
// and we can choose to have theta in the interval [-PI/2 ; +PI/2]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v2.getY().atan2(v2.getX()),
v2.getZ().asin().negate(),
v1.getY().atan2(v1.getZ()));
} else if (order == RotationOrder.XYX) {
// r (Vector3D.plusI) coordinates are :
// cos (theta), sin (phi2) sin (theta), cos (phi2) sin (theta)
// (-r) (Vector3D.plusI) coordinates are :
// cos (theta), sin (theta) sin (phi1), -sin (theta) cos (phi1)
// and we can choose to have theta in the interval [0 ; PI]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v2.getY().atan2(v2.getZ().negate()),
v2.getX().acos(),
v1.getY().atan2(v1.getZ()));
} else if (order == RotationOrder.XZX) {
// r (Vector3D.plusI) coordinates are :
// cos (psi), -cos (phi2) sin (psi), sin (phi2) sin (psi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (psi), sin (psi) cos (phi1), sin (psi) sin (phi1)
// and we can choose to have psi in the interval [0 ; PI]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_I);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v2.getZ().atan2(v2.getY()),
v2.getX().acos(),
v1.getZ().atan2(v1.getY().negate()));
} else if (order == RotationOrder.YXY) {
// r (Vector3D.plusJ) coordinates are :
// sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2)
// (-r) (Vector3D.plusJ) coordinates are :
// sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi)
// and we can choose to have phi in the interval [0 ; PI]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v2.getX().atan2(v2.getZ()),
v2.getY().acos(),
v1.getX().atan2(v1.getZ().negate()));
} else if (order == RotationOrder.YZY) {
// r (Vector3D.plusJ) coordinates are :
// sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2)
// (-r) (Vector3D.plusJ) coordinates are :
// -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi)
// and we can choose to have psi in the interval [0 ; PI]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_J);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v2.getZ().atan2(v2.getX().negate()),
v2.getY().acos(),
v1.getZ().atan2(v1.getX()));
} else if (order == RotationOrder.ZXZ) {
// r (Vector3D.plusK) coordinates are :
// sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi)
// (-r) (Vector3D.plusK) coordinates are :
// sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi)
// and we can choose to have phi in the interval [0 ; PI]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v2.getX().atan2(v2.getY().negate()),
v2.getZ().acos(),
v1.getX().atan2(v1.getY()));
} else { // last possibility is ZYZ
// r (Vector3D.plusK) coordinates are :
// -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta)
// (-r) (Vector3D.plusK) coordinates are :
// cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta)
// and we can choose to have theta in the interval [0 ; PI]
FieldVector3D<T> v1 = applyTo(Vector3D.PLUS_K);
FieldVector3D<T> v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v2.getY().atan2(v2.getX()),
v2.getZ().acos(),
v1.getY().atan2(v1.getX().negate()));
// r (+J) coordinates are :
// -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi)
// (-r) (+I) coordinates are :
// cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0));
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getZ().atan2(v1.getY()),
v2.getY().asin().negate(),
v2.getZ().atan2(v2.getX()));
} else if (order == RotationOrder.YXZ) {
// r (+K) coordinates are :
// cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta)
// (-r) (+J) coordinates are :
// sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0));
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getX().atan2(v1.getZ()),
v2.getZ().asin().negate(),
v2.getX().atan2(v2.getY()));
} else if (order == RotationOrder.YZX) {
// r (+I) coordinates are :
// cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta)
// (-r) (+J) coordinates are :
// sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0));
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getZ().negate().atan2(v1.getX()),
v2.getX().asin(),
v2.getZ().negate().atan2(v2.getY()));
} else if (order == RotationOrder.ZXY) {
// r (+J) coordinates are :
// -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi)
// (-r) (+K) coordinates are :
// -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1));
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getX().negate().atan2(v1.getY()),
v2.getY().asin(),
v2.getX().negate().atan2(v2.getZ()));
} else if (order == RotationOrder.ZYX) {
// r (+I) coordinates are :
// cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta)
// (-r) (+K) coordinates are :
// -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta)
// and we can choose to have theta in the interval [-PI/2 ; +PI/2]
final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1));
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return buildArray(v1.getY().atan2(v1.getX()),
v2.getX().asin().negate(),
v2.getY().atan2(v2.getZ()));
} else if (order == RotationOrder.XYX) {
// r (+I) coordinates are :
// cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta)
// (-r) (+I) coordinates are :
// cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2)
// and we can choose to have theta in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0));
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getY().atan2(v1.getZ().negate()),
v2.getX().acos(),
v2.getY().atan2(v2.getZ()));
} else if (order == RotationOrder.XZX) {
// r (+I) coordinates are :
// cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi)
// (-r) (+I) coordinates are :
// cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2)
// and we can choose to have psi in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(1, 0, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(1, 0, 0));
if ((v2.getX().getReal() < -0.9999999999) || (v2.getX().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getZ().atan2(v1.getY()),
v2.getX().acos(),
v2.getZ().atan2(v2.getY().negate()));
} else if (order == RotationOrder.YXY) {
// r (+J) coordinates are :
// sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi)
// (-r) (+J) coordinates are :
// sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2)
// and we can choose to have phi in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0));
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getX().atan2(v1.getZ()),
v2.getY().acos(),
v2.getX().atan2(v2.getZ().negate()));
} else if (order == RotationOrder.YZY) {
// r (+J) coordinates are :
// -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi)
// (-r) (+J) coordinates are :
// sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2)
// and we can choose to have psi in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(0, 1, 0));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 1, 0));
if ((v2.getY().getReal() < -0.9999999999) || (v2.getY().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getZ().atan2(v1.getX().negate()),
v2.getY().acos(),
v2.getZ().atan2(v2.getX()));
} else if (order == RotationOrder.ZXZ) {
// r (+K) coordinates are :
// sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi)
// (-r) (+K) coordinates are :
// sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi)
// and we can choose to have phi in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1));
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getX().atan2(v1.getY().negate()),
v2.getZ().acos(),
v2.getX().atan2(v2.getY()));
} else { // last possibility is ZYZ
// r (+K) coordinates are :
// cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta)
// (-r) (+K) coordinates are :
// -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta)
// and we can choose to have theta in the interval [0 ; PI]
final FieldVector3D<T> v1 = applyTo(vector(0, 0, 1));
final FieldVector3D<T> v2 = applyInverseTo(vector(0, 0, 1));
if ((v2.getZ().getReal() < -0.9999999999) || (v2.getZ().getReal() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return buildArray(v1.getY().atan2(v1.getX()),
v2.getZ().acos(),
v2.getY().atan2(v2.getX().negate()));
}
}

801
src/main/java/org/apache/commons/math4/geometry/euclidean/threed/Rotation.java
View File

@ -170,15 +170,29 @@ public class Rotation implements Serializable {
* @param axis axis around which to rotate
* @param angle rotation angle.
* @exception MathIllegalArgumentException if the axis norm is zero
* @deprecated as of 3.6, replaced with {@link #Rotation(Vector3D, double, RotationConvention)}
*/
@Deprecated
public Rotation(Vector3D axis, double angle) throws MathIllegalArgumentException {
this(axis, angle, RotationConvention.VECTOR_OPERATOR);
}
/** Build a rotation from an axis and an angle.
* @param axis axis around which to rotate
* @param angle rotation angle
* @param convention convention to use for the semantics of the angle
* @exception MathIllegalArgumentException if the axis norm is zero
* @since 3.6
*/
public Rotation(final Vector3D axis, final double angle, final RotationConvention convention)
throws MathIllegalArgumentException {
double norm = axis.getNorm();
if (norm == 0) {
throw new MathIllegalArgumentException(LocalizedFormats.ZERO_NORM_FOR_ROTATION_AXIS);
}
double halfAngle = -0.5 * angle;
double halfAngle = convention == RotationConvention.VECTOR_OPERATOR ? -0.5 * angle : +0.5 * angle;
double coeff = FastMath.sin(halfAngle) / norm;
q0 = FastMath.cos (halfAngle);
@ -250,7 +264,7 @@ public class Rotation implements Serializable {
}
/** Build the rotation that transforms a pair of vector into another pair.
/** Build the rotation that transforms a pair of vectors into another pair.
* <p>Except for possible scale factors, if the instance were applied to
* the pair (u<sub>1</sub>, u<sub>2</sub>) it will produce the pair
@ -259,27 +273,27 @@ public class Rotation implements Serializable {
* <p>If the angular separation between u<sub>1</sub> and u<sub>2</sub> is
* not the same as the angular separation between v<sub>1</sub> and
* v<sub>2</sub>, then a corrected v'<sub>2</sub> will be used rather than
* v<sub>2</sub>, the corrected vector will be in the (v<sub>1</sub>,
* v<sub>2</sub>) plane.</p>
* v<sub>2</sub>, the corrected vector will be in the (&pm;v<sub>1</sub>,
* +v<sub>2</sub>) half-plane.</p>
* @param u1 first vector of the origin pair
* @param u2 second vector of the origin pair
* @param v1 desired image of u1 by the rotation
* @param v2 desired image of u2 by the rotation
* @exception MathArithmeticException if the norm of one of the vectors is zero,
* or if one of the pair is degenerated (i.e. the vectors of the pair are colinear)
* or if one of the pair is degenerated (i.e. the vectors of the pair are collinear)
*/
public Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2)
throws MathArithmeticException {
// build orthonormalized base from u1, u2
// this fails when vectors are null or colinear, which is forbidden to define a rotation
// this fails when vectors are null or collinear, which is forbidden to define a rotation
final Vector3D u3 = u1.crossProduct(u2).normalize();
u2 = u3.crossProduct(u1).normalize();
u1 = u1.normalize();
// build an orthonormalized base from v1, v2
// this fails when vectors are null or colinear, which is forbidden to define a rotation
// this fails when vectors are null or collinear, which is forbidden to define a rotation
final Vector3D v3 = v1.crossProduct(v2).normalize();
v2 = v3.crossProduct(v1).normalize();
v1 = v1.normalize();
@ -317,7 +331,7 @@ public class Rotation implements Serializable {
* applied to the vector u it will produce the vector v. There is an
* infinite number of such rotations, this constructor choose the
* one with the smallest associated angle (i.e. the one whose axis
* is orthogonal to the (u, v) plane). If u and v are colinear, an
* is orthogonal to the (u, v) plane). If u and v are collinear, an
* arbitrary rotation axis is chosen.</p>
* @param u origin vector
@ -372,13 +386,44 @@ public class Rotation implements Serializable {
* @param alpha1 angle of the first elementary rotation
* @param alpha2 angle of the second elementary rotation
* @param alpha3 angle of the third elementary rotation
* @deprecated as of 3.6, replaced with {@link
* #Rotation(RotationOrder, RotationConvention, double, double, double)}
*/
@Deprecated
public Rotation(RotationOrder order,
double alpha1, double alpha2, double alpha3) {
Rotation r1 = new Rotation(order.getA1(), alpha1);
Rotation r2 = new Rotation(order.getA2(), alpha2);
Rotation r3 = new Rotation(order.getA3(), alpha3);
Rotation composed = r1.applyTo(r2.applyTo(r3));
this(order, RotationConvention.VECTOR_OPERATOR, alpha1, alpha2, alpha3);
}
/** Build a rotation from three Cardan or Euler elementary rotations.
* <p>Cardan rotations are three successive rotations around the
* canonical axes X, Y and Z, each axis being used once. There are
* 6 such sets of rotations (XYZ, XZY, YXZ, YZX, ZXY and ZYX). Euler
* rotations are three successive rotations around the canonical
* axes X, Y and Z, the first and last rotations being around the
* same axis. There are 6 such sets of rotations (XYX, XZX, YXY,
* YZY, ZXZ and ZYZ), the most popular one being ZXZ.</p>
* <p>Beware that many people routinely use the term Euler angles even
* for what really are Cardan angles (this confusion is especially
* widespread in the aerospace business where Roll, Pitch and Yaw angles
* are often wrongly tagged as Euler angles).</p>
* @param order order of rotations to use
* @param convention convention to use for the semantics of the angle
* @param alpha1 angle of the first elementary rotation
* @param alpha2 angle of the second elementary rotation
* @param alpha3 angle of the third elementary rotation
* @since 3.6
*/
public Rotation(RotationOrder order, RotationConvention convention,
double alpha1, double alpha2, double alpha3) {
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));
q0 = composed.q0;
q1 = composed.q1;
q2 = composed.q2;
@ -487,18 +532,38 @@ public class Rotation implements Serializable {
/** Get the normalized axis of the rotation.
* @return normalized axis of the rotation
* @see #Rotation(Vector3D, double)
* @see #Rotation(Vector3D, double, RotationConvention)
* @deprecated as of 3.6, replaced with {@link #getAxis(RotationConvention)}
*/
@Deprecated
public Vector3D getAxis() {
double squaredSine = q1 * q1 + q2 * q2 + q3 * q3;
return getAxis(RotationConvention.VECTOR_OPERATOR);
}
/** Get the normalized axis of the rotation.
* <p>
* Note that as {@link #getAngle()} always returns an angle
* between 0 and &pi;, changing the convention changes the
* direction of the axis, not the sign of the angle.
* </p>
* @param convention convention to use for the semantics of the angle
* @return normalized axis of the rotation
* @see #Rotation(Vector3D, double, RotationConvention)
* @since 3.6
*/
public Vector3D getAxis(final RotationConvention convention) {
final double squaredSine = q1 * q1 + q2 * q2 + q3 * q3;
if (squaredSine == 0) {
return new Vector3D(1, 0, 0);
} else if (q0 < 0) {
double inverse = 1 / FastMath.sqrt(squaredSine);
return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse);
return convention == RotationConvention.VECTOR_OPERATOR ? Vector3D.PLUS_I : Vector3D.MINUS_I;
} else {
final double sgn = convention == RotationConvention.VECTOR_OPERATOR ? +1 : -1;
if (q0 < 0) {
final double inverse = sgn / FastMath.sqrt(squaredSine);
return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse);
}
final double inverse = -sgn / FastMath.sqrt(squaredSine);
return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse);
}
double inverse = -1 / FastMath.sqrt(squaredSine);
return new Vector3D(q1 * inverse, q2 * inverse, q3 * inverse);
}
/** Get the angle of the rotation.
@ -548,227 +613,491 @@ public class Rotation implements Serializable {
* @return an array of three angles, in the order specified by the set
* @exception CardanEulerSingularityException if the rotation is
* singular with respect to the angles set specified
* @deprecated as of 3.6, replaced with {@link #getAngles(RotationOrder, RotationConvention)}
*/
@Deprecated
public double[] getAngles(RotationOrder order)
throws CardanEulerSingularityException {
throws CardanEulerSingularityException {
return getAngles(order, RotationConvention.VECTOR_OPERATOR);
}
if (order == RotationOrder.XYZ) {
/** Get the Cardan or Euler angles corresponding to the instance.
// r (Vector3D.plusK) coordinates are :
// sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta)
// and we can choose to have theta in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
* <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 &pi; + a<sub>1</sub>, &pi;
* - 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 convention convention to use for the semantics of the angle
* @return an array of three angles, in the order specified by the set
* @exception CardanEulerSingularityException if the rotation is
* singular with respect to the angles set specified
* @since 3.6
*/
public double[] getAngles(RotationOrder order, RotationConvention convention)
throws CardanEulerSingularityException {
if (convention == RotationConvention.VECTOR_OPERATOR) {
if (order == RotationOrder.XYZ) {
// r (Vector3D.plusK) coordinates are :
// sin (theta), -cos (theta) sin (phi), cos (theta) cos (phi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (psi) cos (theta), -sin (psi) cos (theta), sin (theta)
// and we can choose to have theta in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(-(v1.getY()), v1.getZ()),
FastMath.asin(v2.getZ()),
FastMath.atan2(-(v2.getY()), v2.getX())
};
} else if (order == RotationOrder.XZY) {
// r (Vector3D.plusJ) coordinates are :
// -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(v1.getZ(), v1.getY()),
-FastMath.asin(v2.getY()),
FastMath.atan2(v2.getZ(), v2.getX())
};
} else if (order == RotationOrder.YXZ) {
// r (Vector3D.plusK) coordinates are :
// cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta)
// (-r) (Vector3D.plusJ) coordinates are :
// sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(v1.getX(), v1.getZ()),
-FastMath.asin(v2.getZ()),
FastMath.atan2(v2.getX(), v2.getY())
};
} else if (order == RotationOrder.YZX) {
// r (Vector3D.plusI) coordinates are :
// cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta)
// (-r) (Vector3D.plusJ) coordinates are :
// sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(-(v1.getZ()), v1.getX()),
FastMath.asin(v2.getX()),
FastMath.atan2(-(v2.getZ()), v2.getY())
};
} else if (order == RotationOrder.ZXY) {
// r (Vector3D.plusJ) coordinates are :
// -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi)
// (-r) (Vector3D.plusK) coordinates are :
// -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(-(v1.getX()), v1.getY()),
FastMath.asin(v2.getY()),
FastMath.atan2(-(v2.getX()), v2.getZ())
};
} else if (order == RotationOrder.ZYX) {
// r (Vector3D.plusI) coordinates are :
// cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta)
// (-r) (Vector3D.plusK) coordinates are :
// -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta)
// and we can choose to have theta in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(v1.getY(), v1.getX()),
-FastMath.asin(v2.getX()),
FastMath.atan2(v2.getY(), v2.getZ())
};
} else if (order == RotationOrder.XYX) {
// r (Vector3D.plusI) coordinates are :
// cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta)
// (-r) (Vector3D.plusI) coordinates are :
// cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2)
// and we can choose to have theta in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getY(), -v1.getZ()),
FastMath.acos(v2.getX()),
FastMath.atan2(v2.getY(), v2.getZ())
};
} else if (order == RotationOrder.XZX) {
// r (Vector3D.plusI) coordinates are :
// cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2)
// and we can choose to have psi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getZ(), v1.getY()),
FastMath.acos(v2.getX()),
FastMath.atan2(v2.getZ(), -v2.getY())
};
} else if (order == RotationOrder.YXY) {
// r (Vector3D.plusJ) coordinates are :
// sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi)
// (-r) (Vector3D.plusJ) coordinates are :
// sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2)
// and we can choose to have phi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getX(), v1.getZ()),
FastMath.acos(v2.getY()),
FastMath.atan2(v2.getX(), -v2.getZ())
};
} else if (order == RotationOrder.YZY) {
// r (Vector3D.plusJ) coordinates are :
// -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi)
// (-r) (Vector3D.plusJ) coordinates are :
// sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2)
// and we can choose to have psi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getZ(), -v1.getX()),
FastMath.acos(v2.getY()),
FastMath.atan2(v2.getZ(), v2.getX())
};
} else if (order == RotationOrder.ZXZ) {
// r (Vector3D.plusK) coordinates are :
// sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi)
// (-r) (Vector3D.plusK) coordinates are :
// sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi)
// and we can choose to have phi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getX(), -v1.getY()),
FastMath.acos(v2.getZ()),
FastMath.atan2(v2.getX(), v2.getY())
};
} else { // last possibility is ZYZ
// r (Vector3D.plusK) coordinates are :
// cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta)
// (-r) (Vector3D.plusK) coordinates are :
// -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta)
// and we can choose to have theta in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getY(), v1.getX()),
FastMath.acos(v2.getZ()),
FastMath.atan2(v2.getY(), -v2.getX())
};
}
} else {
if (order == RotationOrder.XYZ) {
// r (Vector3D.plusI) coordinates are :
// cos (theta) cos (psi), -cos (theta) sin (psi), sin (theta)
// (-r) (Vector3D.plusK) coordinates are :
// sin (theta), -sin (phi) cos (theta), cos (phi) cos (theta)
// and we can choose to have theta in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(-v2.getY(), v2.getZ()),
FastMath.asin(v2.getX()),
FastMath.atan2(-v1.getY(), v1.getX())
};
} else if (order == RotationOrder.XZY) {
// r (Vector3D.plusI) coordinates are :
// cos (psi) cos (theta), -sin (psi), cos (psi) sin (theta)
// (-r) (Vector3D.plusJ) coordinates are :
// -sin (psi), cos (phi) cos (psi), sin (phi) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(v2.getZ(), v2.getY()),
-FastMath.asin(v2.getX()),
FastMath.atan2(v1.getZ(), v1.getX())
};
} else if (order == RotationOrder.YXZ) {
// r (Vector3D.plusJ) coordinates are :
// cos (phi) sin (psi), cos (phi) cos (psi), -sin (phi)
// (-r) (Vector3D.plusK) coordinates are :
// sin (theta) cos (phi), -sin (phi), cos (theta) cos (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(v2.getX(), v2.getZ()),
-FastMath.asin(v2.getY()),
FastMath.atan2(v1.getX(), v1.getY())
};
} else if (order == RotationOrder.YZX) {
// r (Vector3D.plusJ) coordinates are :
// sin (psi), cos (psi) cos (phi), -cos (psi) sin (phi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (theta) cos (psi), sin (psi), -sin (theta) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(-v2.getZ(), v2.getX()),
FastMath.asin(v2.getY()),
FastMath.atan2(-v1.getZ(), v1.getY())
};
} else if (order == RotationOrder.ZXY) {
// r (Vector3D.plusK) coordinates are :
// -cos (phi) sin (theta), sin (phi), cos (phi) cos (theta)
// (-r) (Vector3D.plusJ) coordinates are :
// -sin (psi) cos (phi), cos (psi) cos (phi), sin (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(-v2.getX(), v2.getY()),
FastMath.asin(v2.getZ()),
FastMath.atan2(-v1.getX(), v1.getZ())
};
} else if (order == RotationOrder.ZYX) {
// r (Vector3D.plusK) coordinates are :
// -sin (theta), cos (theta) sin (phi), cos (theta) cos (phi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (psi) cos (theta), sin (psi) cos (theta), -sin (theta)
// and we can choose to have theta in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(v2.getY(), v2.getX()),
-FastMath.asin(v2.getZ()),
FastMath.atan2(v1.getY(), v1.getZ())
};
} else if (order == RotationOrder.XYX) {
// r (Vector3D.plusI) coordinates are :
// cos (theta), sin (phi2) sin (theta), cos (phi2) sin (theta)
// (-r) (Vector3D.plusI) coordinates are :
// cos (theta), sin (theta) sin (phi1), -sin (theta) cos (phi1)
// and we can choose to have theta in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v2.getY(), -v2.getZ()),
FastMath.acos(v2.getX()),
FastMath.atan2(v1.getY(), v1.getZ())
};
} else if (order == RotationOrder.XZX) {
// r (Vector3D.plusI) coordinates are :
// cos (psi), -cos (phi2) sin (psi), sin (phi2) sin (psi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (psi), sin (psi) cos (phi1), sin (psi) sin (phi1)
// and we can choose to have psi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v2.getZ(), v2.getY()),
FastMath.acos(v2.getX()),
FastMath.atan2(v1.getZ(), -v1.getY())
};
} else if (order == RotationOrder.YXY) {
// r (Vector3D.plusJ) coordinates are :
// sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2)
// (-r) (Vector3D.plusJ) coordinates are :
// sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi)
// and we can choose to have phi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v2.getX(), v2.getZ()),
FastMath.acos(v2.getY()),
FastMath.atan2(v1.getX(), -v1.getZ())
};
} else if (order == RotationOrder.YZY) {
// r (Vector3D.plusJ) coordinates are :
// sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2)
// (-r) (Vector3D.plusJ) coordinates are :
// -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi)
// and we can choose to have psi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v2.getZ(), -v2.getX()),
FastMath.acos(v2.getY()),
FastMath.atan2(v1.getZ(), v1.getX())
};
} else if (order == RotationOrder.ZXZ) {
// r (Vector3D.plusK) coordinates are :
// sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi)
// (-r) (Vector3D.plusK) coordinates are :
// sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi)
// and we can choose to have phi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v2.getX(), -v2.getY()),
FastMath.acos(v2.getZ()),
FastMath.atan2(v1.getX(), v1.getY())
};
} else { // last possibility is ZYZ
// r (Vector3D.plusK) coordinates are :
// -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta)
// (-r) (Vector3D.plusK) coordinates are :
// cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta)
// and we can choose to have theta in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v2.getY(), v2.getX()),
FastMath.acos(v2.getZ()),
FastMath.atan2(v1.getY(), -v1.getX())
};
}
}
return new double[] {
FastMath.atan2(-(v1.getY()), v1.getZ()),
FastMath.asin(v2.getZ()),
FastMath.atan2(-(v2.getY()), v2.getX())
};
} else if (order == RotationOrder.XZY) {
// r (Vector3D.plusJ) coordinates are :
// -sin (psi), cos (psi) cos (phi), cos (psi) sin (phi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (theta) cos (psi), -sin (psi), sin (theta) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(v1.getZ(), v1.getY()),
-FastMath.asin(v2.getY()),
FastMath.atan2(v2.getZ(), v2.getX())
};
} else if (order == RotationOrder.YXZ) {
// r (Vector3D.plusK) coordinates are :
// cos (phi) sin (theta), -sin (phi), cos (phi) cos (theta)
// (-r) (Vector3D.plusJ) coordinates are :
// sin (psi) cos (phi), cos (psi) cos (phi), -sin (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(v1.getX(), v1.getZ()),
-FastMath.asin(v2.getZ()),
FastMath.atan2(v2.getX(), v2.getY())
};
} else if (order == RotationOrder.YZX) {
// r (Vector3D.plusI) coordinates are :
// cos (psi) cos (theta), sin (psi), -cos (psi) sin (theta)
// (-r) (Vector3D.plusJ) coordinates are :
// sin (psi), cos (phi) cos (psi), -sin (phi) cos (psi)
// and we can choose to have psi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(-(v1.getZ()), v1.getX()),
FastMath.asin(v2.getX()),
FastMath.atan2(-(v2.getZ()), v2.getY())
};
} else if (order == RotationOrder.ZXY) {
// r (Vector3D.plusJ) coordinates are :
// -cos (phi) sin (psi), cos (phi) cos (psi), sin (phi)
// (-r) (Vector3D.plusK) coordinates are :
// -sin (theta) cos (phi), sin (phi), cos (theta) cos (phi)
// and we can choose to have phi in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(-(v1.getX()), v1.getY()),
FastMath.asin(v2.getY()),
FastMath.atan2(-(v2.getX()), v2.getZ())
};
} else if (order == RotationOrder.ZYX) {
// r (Vector3D.plusI) coordinates are :
// cos (theta) cos (psi), cos (theta) sin (psi), -sin (theta)
// (-r) (Vector3D.plusK) coordinates are :
// -sin (theta), sin (phi) cos (theta), cos (phi) cos (theta)
// and we can choose to have theta in the interval [-PI/2 ; +PI/2]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(true);
}
return new double[] {
FastMath.atan2(v1.getY(), v1.getX()),
-FastMath.asin(v2.getX()),
FastMath.atan2(v2.getY(), v2.getZ())
};
} else if (order == RotationOrder.XYX) {
// r (Vector3D.plusI) coordinates are :
// cos (theta), sin (phi1) sin (theta), -cos (phi1) sin (theta)
// (-r) (Vector3D.plusI) coordinates are :
// cos (theta), sin (theta) sin (phi2), sin (theta) cos (phi2)
// and we can choose to have theta in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getY(), -v1.getZ()),
FastMath.acos(v2.getX()),
FastMath.atan2(v2.getY(), v2.getZ())
};
} else if (order == RotationOrder.XZX) {
// r (Vector3D.plusI) coordinates are :
// cos (psi), cos (phi1) sin (psi), sin (phi1) sin (psi)
// (-r) (Vector3D.plusI) coordinates are :
// cos (psi), -sin (psi) cos (phi2), sin (psi) sin (phi2)
// and we can choose to have psi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_I);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_I);
if ((v2.getX() < -0.9999999999) || (v2.getX() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getZ(), v1.getY()),
FastMath.acos(v2.getX()),
FastMath.atan2(v2.getZ(), -v2.getY())
};
} else if (order == RotationOrder.YXY) {
// r (Vector3D.plusJ) coordinates are :
// sin (theta1) sin (phi), cos (phi), cos (theta1) sin (phi)
// (-r) (Vector3D.plusJ) coordinates are :
// sin (phi) sin (theta2), cos (phi), -sin (phi) cos (theta2)
// and we can choose to have phi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getX(), v1.getZ()),
FastMath.acos(v2.getY()),
FastMath.atan2(v2.getX(), -v2.getZ())
};
} else if (order == RotationOrder.YZY) {
// r (Vector3D.plusJ) coordinates are :
// -cos (theta1) sin (psi), cos (psi), sin (theta1) sin (psi)
// (-r) (Vector3D.plusJ) coordinates are :
// sin (psi) cos (theta2), cos (psi), sin (psi) sin (theta2)
// and we can choose to have psi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_J);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_J);
if ((v2.getY() < -0.9999999999) || (v2.getY() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getZ(), -v1.getX()),
FastMath.acos(v2.getY()),
FastMath.atan2(v2.getZ(), v2.getX())
};
} else if (order == RotationOrder.ZXZ) {
// r (Vector3D.plusK) coordinates are :
// sin (psi1) sin (phi), -cos (psi1) sin (phi), cos (phi)
// (-r) (Vector3D.plusK) coordinates are :
// sin (phi) sin (psi2), sin (phi) cos (psi2), cos (phi)
// and we can choose to have phi in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getX(), -v1.getY()),
FastMath.acos(v2.getZ()),
FastMath.atan2(v2.getX(), v2.getY())
};
} else { // last possibility is ZYZ
// r (Vector3D.plusK) coordinates are :
// cos (psi1) sin (theta), sin (psi1) sin (theta), cos (theta)
// (-r) (Vector3D.plusK) coordinates are :
// -sin (theta) cos (psi2), sin (theta) sin (psi2), cos (theta)
// and we can choose to have theta in the interval [0 ; PI]
Vector3D v1 = applyTo(Vector3D.PLUS_K);
Vector3D v2 = applyInverseTo(Vector3D.PLUS_K);
if ((v2.getZ() < -0.9999999999) || (v2.getZ() > 0.9999999999)) {
throw new CardanEulerSingularityException(false);
}
return new double[] {
FastMath.atan2(v1.getY(), v1.getX()),
FastMath.acos(v2.getZ()),
FastMath.atan2(v2.getY(), -v2.getX())
};
}
}

79
src/main/java/org/apache/commons/math4/geometry/euclidean/threed/RotationConvention.java Normal file
View File

@ -0,0 +1,79 @@
/*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* The ASF licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.apache.commons.math4.geometry.euclidean.threed;
/**
* This enumerates is used to differentiate the semantics of a rotation.
* @see Rotation
* @since 3.6
*/
public enum RotationConvention {
/** Constant for rotation that have the semantics of a vector operator.
* <p>
* According to this convention, the rotation moves vectors with respect
* to a fixed reference frame.
* </p>
* <p>
* This means that if we define rotation r is a 90 degrees rotation around
* the Z axis, the image of vector {@link Vector3D#PLUS_I} would be
* {@link Vector3D#PLUS_J}, the image of vector {@link Vector3D#PLUS_J}
* would be {@link Vector3D#MINUS_I}, the image of vector {@link Vector3D#PLUS_K}
* would be {@link Vector3D#PLUS_K}, and the image of vector with coordinates (1, 2, 3)
* would be vector (-2, 1, 3). This means that the vector rotates counterclockwise.
* </p>
* <p>
* This convention was the only one supported by Apache Commons Math up to version 3.5.
* </p>
* <p>
* The difference with {@link #FRAME_TRANSFORM} is only the semantics of the sign
* of the angle. It is always possible to create or use a rotation using either
* convention to really represent a rotation that would have been best created or
* used with the other convention, by changing accordingly the sign of the
* rotation angle. This is how things were done up to version 3.5.
* </p>
*/
VECTOR_OPERATOR,
/** Constant for rotation that have the semantics of a frame conversion.
* <p>
* According to this convention, the rotation considered vectors to be fixed,
* but their coordinates change as they are converted from an initial frame to
* a destination frame rotated with respect to the initial frame.
* </p>
* <p>
* This means that if we define rotation r is a 90 degrees rotation around
* the Z axis, the image of vector {@link Vector3D#PLUS_I} would be
* {@link Vector3D#MINUS_J}, the image of vector {@link Vector3D#PLUS_J}
* would be {@link Vector3D#PLUS_I}, the image of vector {@link Vector3D#PLUS_K}
* would be {@link Vector3D#PLUS_K}, and the image of vector with coordinates (1, 2, 3)
* would be vector (2, -1, 3). This means that the coordinates of the vector rotates
* clockwise, because they are expressed with respect to a destination frame that is rotated
* counterclockwise.
* </p>
* <p>
* The difference with {@link #VECTOR_OPERATOR} is only the semantics of the sign
* of the angle. It is always possible to create or use a rotation using either
* convention to really represent a rotation that would have been best created or
* used with the other convention, by changing accordingly the sign of the
* rotation angle. This is how things were done up to version 3.5.
* </p>
*/
FRAME_TRANSFORM;
}

6
src/main/java/org/apache/commons/math4/geometry/spherical/twod/SphericalPolygonsSet.java
View File

@ -27,6 +27,7 @@ import org.apache.commons.math4.geometry.enclosing.EnclosingBall;
import org.apache.commons.math4.geometry.enclosing.WelzlEncloser;
import org.apache.commons.math4.geometry.euclidean.threed.Euclidean3D;
import org.apache.commons.math4.geometry.euclidean.threed.Rotation;
import org.apache.commons.math4.geometry.euclidean.threed.RotationConvention;
import org.apache.commons.math4.geometry.euclidean.threed.SphereGenerator;
import org.apache.commons.math4.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math4.geometry.partitioning.AbstractRegion;
@ -161,10 +162,11 @@ public class SphericalPolygonsSet extends AbstractRegion<Sphere2D, Sphere1D> {
private static S2Point[] createRegularPolygonVertices(final Vector3D center, final Vector3D meridian,
final double outsideRadius, final int n) {
final S2Point[] array = new S2Point[n];
final Rotation r0 = new Rotation(Vector3D.crossProduct(center, meridian), outsideRadius);
final Rotation r0 = new Rotation(Vector3D.crossProduct(center, meridian),
outsideRadius, RotationConvention.VECTOR_OPERATOR);
array[0] = new S2Point(r0.applyTo(center));
final Rotation r = new Rotation(center, MathUtils.TWO_PI / n);
final Rotation r = new Rotation(center, MathUtils.TWO_PI / n, RotationConvention.VECTOR_OPERATOR);
for (int i = 1; i < n; ++i) {
array[i] = new S2Point(r.applyTo(array[i - 1].getVector()));
}

14
src/site/xdoc/userguide/geometry.xml
View File

@ -129,7 +129,7 @@
matrix into a set of Cardan angles can be done using the
following single line of code:
</p>
<source>double[] angles = new Rotation(matrix, 1.0e-10).getAngles(RotationOrder.XYZ);</source>
<source>double[] angles = new Rotation(matrix, 1.0e-10).getAngles(RotationOrder.XYZ, RotationConvention.FRAME_TRANSFORM);</source>
<p>
Focus is oriented on what a rotation <em>does</em> rather than on its
underlying representation. Once it has been built, and regardless of
@ -156,6 +156,18 @@
into the observing direction in inertial frame taking into account the
observatory location and the Earth rotation.
</p>
<p>
In order to support naturally both views, the enumerate RotationConvention
has been introduced in version 3.6. This enumerate can take two values:
<code>VECTOR_OPERATOR</code> or <code>FRAME_TRANSFORM</code>. This
enumerate must be passed as an argument to the few methods that depend
on an interpretation of the semantics of the angle/axis. The value
<code>VECTOR_OPERATOR</code> corresponds to rotations that are
considered to move vectors within a fixed frame. The value
<code>FRAME_TRANSFORM</code> corresponds to rotations that are
considered to represent frame rotations, so fixed vectors coordinates
change as their reference frame changes.
</p>
<p>
These examples show that a rotation means what the user wants it to
mean, so this class does not push the user towards one specific

13
src/test/java/org/apache/commons/math4/complex/QuaternionTest.java
View File

@ -22,6 +22,7 @@ import org.apache.commons.math4.complex.Quaternion;
import org.apache.commons.math4.exception.DimensionMismatchException;
import org.apache.commons.math4.exception.ZeroException;
import org.apache.commons.math4.geometry.euclidean.threed.Rotation;
import org.apache.commons.math4.geometry.euclidean.threed.RotationConvention;
import org.apache.commons.math4.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math4.util.FastMath;
import org.junit.Test;
@ -413,9 +414,15 @@ public class QuaternionTest {
final Rotation rotP = new Rotation(qP.getQ0(), qP.getQ1(), qP.getQ2(), qP.getQ3(), true);
Assert.assertEquals(rot.getAngle(), rotP.getAngle(), COMPARISON_EPS);
Assert.assertEquals(rot.getAxis().getX(), rot.getAxis().getX(), COMPARISON_EPS);
Assert.assertEquals(rot.getAxis().getY(), rot.getAxis().getY(), COMPARISON_EPS);
Assert.assertEquals(rot.getAxis().getZ(), rot.getAxis().getZ(), COMPARISON_EPS);
Assert.assertEquals(rot.getAxis(RotationConvention.VECTOR_OPERATOR).getX(),
rot.getAxis(RotationConvention.VECTOR_OPERATOR).getX(),
COMPARISON_EPS);
Assert.assertEquals(rot.getAxis(RotationConvention.VECTOR_OPERATOR).getY(),
rot.getAxis(RotationConvention.VECTOR_OPERATOR).getY(),
COMPARISON_EPS);
Assert.assertEquals(rot.getAxis(RotationConvention.VECTOR_OPERATOR).getZ(),
rot.getAxis(RotationConvention.VECTOR_OPERATOR).getZ(),
COMPARISON_EPS);
}
}

270
src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDSTest.java
View File

@ -63,7 +63,8 @@ public class FieldRotationDSTest {
}
@Test
public void testAxisAngle() throws MathIllegalArgumentException {
@Deprecated
public void testAxisAngleDeprecated() throws MathIllegalArgumentException {
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(10, 10, 10), createAngle(2 * FastMath.PI / 3));
checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 1, 0));
@ -91,6 +92,88 @@ public class FieldRotationDSTest {
}
@Test
public void testAxisAngleVectorOperator() throws MathIllegalArgumentException {
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(10, 10, 10),
createAngle(2 * FastMath.PI / 3) ,
RotationConvention.VECTOR_OPERATOR);
checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 1, 0));
checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 0, 1));
checkVector(r.applyTo(createVector(0, 0, 1)), createVector(1, 0, 0));
double s = 1 / FastMath.sqrt(3);
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector( s, s, s));
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-s, -s, -s));
checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
try {
new FieldRotation<DerivativeStructure>(createAxis(0, 0, 0),
createAngle(2 * FastMath.PI / 3),
RotationConvention.VECTOR_OPERATOR);
Assert.fail("an exception should have been thrown");
} catch (MathIllegalArgumentException e) {
}
r = new FieldRotation<DerivativeStructure>(createAxis(0, 0, 1),
createAngle(1.5 * FastMath.PI),
RotationConvention.VECTOR_OPERATOR);
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, 0, -1));
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, 0, +1));
checkAngle(r.getAngle(), 0.5 * FastMath.PI);
r = new FieldRotation<DerivativeStructure>(createAxis(0, 1, 0),
createAngle(FastMath.PI),
RotationConvention.VECTOR_OPERATOR);
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, +1, 0));
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, -1, 0));
checkAngle(r.getAngle(), FastMath.PI);
checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.VECTOR_OPERATOR), createVector(+1, 0, 0));
checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-1, 0, 0));
}
@Test
public void testAxisAngleFrameTransform() throws MathIllegalArgumentException {
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(10, 10, 10),
createAngle(2 * FastMath.PI / 3) ,
RotationConvention.FRAME_TRANSFORM);
checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1));
checkVector(r.applyTo(createVector(0, 1, 0)), createVector(1, 0, 0));
checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 1, 0));
double s = 1 / FastMath.sqrt(3);
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector( s, s, s));
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(-s, -s, -s));
checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
try {
new FieldRotation<DerivativeStructure>(createAxis(0, 0, 0),
createAngle(2 * FastMath.PI / 3),
RotationConvention.FRAME_TRANSFORM);
Assert.fail("an exception should have been thrown");
} catch (MathIllegalArgumentException e) {
}
r = new FieldRotation<DerivativeStructure>(createAxis(0, 0, 1),
createAngle(1.5 * FastMath.PI),
RotationConvention.FRAME_TRANSFORM);
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, 0, -1));
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, 0, +1));
checkAngle(r.getAngle(), 0.5 * FastMath.PI);
r = new FieldRotation<DerivativeStructure>(createAxis(0, 1, 0),
createAngle(FastMath.PI),
RotationConvention.FRAME_TRANSFORM);
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, +1, 0));
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, -1, 0));
checkAngle(r.getAngle(), FastMath.PI);
checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-1, 0, 0));
checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.VECTOR_OPERATOR), createVector(+1, 0, 0));
}
@Test
public void testRevert() {
double a = 0.001;
@ -157,7 +240,10 @@ public class FieldRotationDSTest {
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(), reverted.getAxis()).getReal(), 1.0e-15);
Assert.assertEquals(-1,
FieldVector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(),
1.0e-15);
}
@Test
@ -191,7 +277,7 @@ public class FieldRotationDSTest {
checkVector(r.applyTo(createVector(0, 1, 0)), createVector(-1, 0, 0));
r = new FieldRotation<DerivativeStructure>(u1, u2, u1.negate(), u2.negate());
FieldVector3D<DerivativeStructure> axis = r.getAxis();
FieldVector3D<DerivativeStructure> axis = r.getAxis(RotationConvention.VECTOR_OPERATOR);
if (FieldVector3D.dotProduct(axis, createVector(0, 0, 1)).getReal() > 0) {
checkVector(axis, createVector(0, 0, 1));
} else {
@ -366,7 +452,8 @@ public class FieldRotationDSTest {
}
@Test
public void testAngles()
@Deprecated
public void testAnglesDeprecated()
throws CardanEulerSingularityException {
RotationOrder[] CardanOrders = {
@ -415,58 +502,121 @@ public class FieldRotationDSTest {
}
@Test
public void testAngles()
throws CardanEulerSingularityException {
for (RotationConvention convention : RotationConvention.values()) {
RotationOrder[] CardanOrders = {
RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
};
for (int i = 0; i < CardanOrders.length; ++i) {
for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
FieldRotation<DerivativeStructure> r =
new FieldRotation<DerivativeStructure>(CardanOrders[i],
convention,
new DerivativeStructure(3, 1, 0, alpha1),
new DerivativeStructure(3, 1, 1, alpha2),
new DerivativeStructure(3, 1, 2, alpha3));
DerivativeStructure[] angles = r.getAngles(CardanOrders[i], convention);
checkAngle(angles[0], alpha1);
checkAngle(angles[1], alpha2);
checkAngle(angles[2], alpha3);
}
}
}
}
RotationOrder[] EulerOrders = {
RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
};
for (int i = 0; i < EulerOrders.length; ++i) {
for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
FieldRotation<DerivativeStructure> r =
new FieldRotation<DerivativeStructure>(EulerOrders[i],
convention,
new DerivativeStructure(3, 1, 0, alpha1),
new DerivativeStructure(3, 1, 1, alpha2),
new DerivativeStructure(3, 1, 2, alpha3));
DerivativeStructure[] angles = r.getAngles(EulerOrders[i], convention);
checkAngle(angles[0], alpha1);
checkAngle(angles[1], alpha2);
checkAngle(angles[2], alpha3);
}
}
}
}
}
}
@Test
public void testSingularities() {
RotationOrder[] CardanOrders = {
RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
};
for (RotationConvention convention : RotationConvention.values()) {
RotationOrder[] CardanOrders = {
RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
};
double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.PI / 2 };
for (int i = 0; i < CardanOrders.length; ++i) {
for (int j = 0; j < singularCardanAngle.length; ++j) {
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(CardanOrders[i],
new DerivativeStructure(3, 1, 0, 0.1),
new DerivativeStructure(3, 1, 1, singularCardanAngle[j]),
new DerivativeStructure(3, 1, 2, 0.3));
try {
r.getAngles(CardanOrders[i]);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.PI / 2 };
for (int i = 0; i < CardanOrders.length; ++i) {
for (int j = 0; j < singularCardanAngle.length; ++j) {
FieldRotation<DerivativeStructure> r =
new FieldRotation<DerivativeStructure>(CardanOrders[i],
convention,
new DerivativeStructure(3, 1, 0, 0.1),
new DerivativeStructure(3, 1, 1, singularCardanAngle[j]),
new DerivativeStructure(3, 1, 2, 0.3));
try {
r.getAngles(CardanOrders[i], convention);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
}
}
}
}
RotationOrder[] EulerOrders = {
RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
};
RotationOrder[] EulerOrders = {
RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
};
double[] singularEulerAngle = { 0, FastMath.PI };
for (int i = 0; i < EulerOrders.length; ++i) {
for (int j = 0; j < singularEulerAngle.length; ++j) {
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(EulerOrders[i],
new DerivativeStructure(3, 1, 0, 0.1),
new DerivativeStructure(3, 1, 1, singularEulerAngle[j]),
new DerivativeStructure(3, 1, 2, 0.3));
try {
r.getAngles(EulerOrders[i]);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
double[] singularEulerAngle = { 0, FastMath.PI };
for (int i = 0; i < EulerOrders.length; ++i) {
for (int j = 0; j < singularEulerAngle.length; ++j) {
FieldRotation<DerivativeStructure> r =
new FieldRotation<DerivativeStructure>(EulerOrders[i],
convention,
new DerivativeStructure(3, 1, 0, 0.1),
new DerivativeStructure(3, 1, 1, singularEulerAngle[j]),
new DerivativeStructure(3, 1, 2, 0.3));
try {
r.getAngles(EulerOrders[i], convention);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
}
}
}
}
}
@Test
public void testQuaternion() throws MathIllegalArgumentException {
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
createAngle(1.7),
RotationConvention.VECTOR_OPERATOR);
double n = 23.5;
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(r1.getQ0().multiply(n), r1.getQ1().multiply(n),
r1.getQ2().multiply(n), r1.getQ3().multiply(n),
@ -494,8 +644,12 @@ public class FieldRotationDSTest {
@Test
public void testCompose() throws MathIllegalArgumentException {
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2), createAngle(0.3));
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.applyTo(r1);
FieldRotation<DerivativeStructure> r3Double = r2.applyTo(new Rotation(r1.getQ0().getReal(),
r1.getQ1().getReal(),
@ -518,8 +672,12 @@ public class FieldRotationDSTest {
@Test
public void testComposeInverse() throws MathIllegalArgumentException {
FieldRotation<DerivativeStructure> r1 = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(-1, 3, 2), createAngle(0.3));
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.applyInverseTo(r1);
FieldRotation<DerivativeStructure> r3Double = r2.applyInverseTo(new Rotation(r1.getQ0().getReal(),
r1.getQ1().getReal(),
@ -547,7 +705,8 @@ public class FieldRotationDSTest {
for (int i = 0; i < 10; ++i) {
double[] unit = g.nextVector();
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createVector(unit[0], unit[1], unit[2]),
createAngle(random.nextDouble()));
createAngle(random.nextDouble()),
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) {
@ -581,7 +740,7 @@ public class FieldRotationDSTest {
for (int i = 0; i < 10; ++i) {
double[] unit1 = g.nextVector();
Rotation r1 = new Rotation(new Vector3D(unit1[0], unit1[1], unit1[2]),
random.nextDouble());
random.nextDouble(), RotationConvention.VECTOR_OPERATOR);
FieldRotation<DerivativeStructure> r1Prime = new FieldRotation<DerivativeStructure>(new DerivativeStructure(4, 1, 0, r1.getQ0()),
new DerivativeStructure(4, 1, 1, r1.getQ1()),
new DerivativeStructure(4, 1, 2, r1.getQ2()),
@ -589,7 +748,8 @@ public class FieldRotationDSTest {
false);
double[] unit2 = g.nextVector();
FieldRotation<DerivativeStructure> r2 = new FieldRotation<DerivativeStructure>(createVector(unit2[0], unit2[1], unit2[2]),
createAngle(random.nextDouble()));
createAngle(random.nextDouble()),
RotationConvention.VECTOR_OPERATOR);
FieldRotation<DerivativeStructure> rA = FieldRotation.applyTo(r1, r2);
FieldRotation<DerivativeStructure> rB = r1Prime.applyTo(r2);
@ -627,7 +787,9 @@ public class FieldRotationDSTest {
double theta = 1.7;
double cosTheta = FastMath.cos(theta);
double sinTheta = FastMath.sin(theta);
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(kx, ky, kz), createAngle(theta));
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(kx, ky, kz),
createAngle(theta),
RotationConvention.VECTOR_OPERATOR);
Vector3D a = new Vector3D(kx / n, ky / n, kz / n);
// Jacobian of the normalized rotation axis a with respect to the Cartesian vector k
@ -691,7 +853,9 @@ public class FieldRotationDSTest {
@Test
public void testArray() throws MathIllegalArgumentException {
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(2, -3, 5), createAngle(1.7));
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createAxis(2, -3, 5),
createAngle(1.7),
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) {
@ -719,7 +883,9 @@ public class FieldRotationDSTest {
DerivativeStructure[] in = new DerivativeStructure[3];
DerivativeStructure[] out = new DerivativeStructure[3];
DerivativeStructure[] rebuilt = new DerivativeStructure[3];
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<DerivativeStructure> r = new FieldRotation<DerivativeStructure>(createVector(2, -3, 5),
createAngle(1.7),
RotationConvention.VECTOR_OPERATOR);
for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
for (double phi = -1.55; phi < 1.55; phi += 0.2) {
FieldVector3D<DerivativeStructure> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),
@ -750,7 +916,9 @@ public class FieldRotationDSTest {
}
}
r = new FieldRotation<DerivativeStructure>(createVector(0, 0, 1), createAngle(FastMath.PI));
r = new FieldRotation<DerivativeStructure>(createVector(0, 0, 1),
createAngle(FastMath.PI),
RotationConvention.VECTOR_OPERATOR);
for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
for (double phi = -1.55; phi < 1.55; phi += 0.2) {
FieldVector3D<DerivativeStructure> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),

266
src/test/java/org/apache/commons/math4/geometry/euclidean/threed/FieldRotationDfpTest.java
View File

@ -62,7 +62,8 @@ public class FieldRotationDfpTest {
}
@Test
public void testAxisAngle() throws MathIllegalArgumentException {
@Deprecated
public void testAxisAngleDeprecated() throws MathIllegalArgumentException {
FieldRotation<Dfp> r = new FieldRotation<Dfp>(createAxis(10, 10, 10), createAngle(2 * FastMath.PI / 3));
checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 1, 0));
@ -90,6 +91,88 @@ public class FieldRotationDfpTest {
}
@Test
public void testAxisAngleVectorOperator() throws MathIllegalArgumentException {
FieldRotation<Dfp> r = new FieldRotation<Dfp>(createAxis(10, 10, 10),
createAngle(2 * FastMath.PI / 3) ,
RotationConvention.VECTOR_OPERATOR);
checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 1, 0));
checkVector(r.applyTo(createVector(0, 1, 0)), createVector(0, 0, 1));
checkVector(r.applyTo(createVector(0, 0, 1)), createVector(1, 0, 0));
double s = 1 / FastMath.sqrt(3);
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector( s, s, s));
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-s, -s, -s));
checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
try {
new FieldRotation<Dfp>(createAxis(0, 0, 0),
createAngle(2 * FastMath.PI / 3),
RotationConvention.VECTOR_OPERATOR);
Assert.fail("an exception should have been thrown");
} catch (MathIllegalArgumentException e) {
}
r = new FieldRotation<Dfp>(createAxis(0, 0, 1),
createAngle(1.5 * FastMath.PI),
RotationConvention.VECTOR_OPERATOR);
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, 0, -1));
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, 0, +1));
checkAngle(r.getAngle(), 0.5 * FastMath.PI);
r = new FieldRotation<Dfp>(createAxis(0, 1, 0),
createAngle(FastMath.PI),
RotationConvention.VECTOR_OPERATOR);
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, +1, 0));
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, -1, 0));
checkAngle(r.getAngle(), FastMath.PI);
checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.VECTOR_OPERATOR), createVector(+1, 0, 0));
checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-1, 0, 0));
}
@Test
public void testAxisAngleFrameTransform() throws MathIllegalArgumentException {
FieldRotation<Dfp> r = new FieldRotation<Dfp>(createAxis(10, 10, 10),
createAngle(2 * FastMath.PI / 3) ,
RotationConvention.FRAME_TRANSFORM);
checkVector(r.applyTo(createVector(1, 0, 0)), createVector(0, 0, 1));
checkVector(r.applyTo(createVector(0, 1, 0)), createVector(1, 0, 0));
checkVector(r.applyTo(createVector(0, 0, 1)), createVector(0, 1, 0));
double s = 1 / FastMath.sqrt(3);
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector( s, s, s));
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(-s, -s, -s));
checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
try {
new FieldRotation<Dfp>(createAxis(0, 0, 0),
createAngle(2 * FastMath.PI / 3),
RotationConvention.FRAME_TRANSFORM);
Assert.fail("an exception should have been thrown");
} catch (MathIllegalArgumentException e) {
}
r = new FieldRotation<Dfp>(createAxis(0, 0, 1),
createAngle(1.5 * FastMath.PI),
RotationConvention.FRAME_TRANSFORM);
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, 0, -1));
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, 0, +1));
checkAngle(r.getAngle(), 0.5 * FastMath.PI);
r = new FieldRotation<Dfp>(createAxis(0, 1, 0),
createAngle(FastMath.PI),
RotationConvention.FRAME_TRANSFORM);
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), createVector(0, +1, 0));
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), createVector(0, -1, 0));
checkAngle(r.getAngle(), FastMath.PI);
checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.FRAME_TRANSFORM), createVector(-1, 0, 0));
checkVector(createRotation(1, 0, 0, 0, false).getAxis(RotationConvention.VECTOR_OPERATOR), createVector(+1, 0, 0));
}
@Test
public void testRevert() {
double a = 0.001;
@ -103,7 +186,10 @@ public class FieldRotationDfpTest {
FieldRotation<Dfp> rTr = reverted.applyTo(r);
checkRotationDS(rTr, 1, 0, 0, 0);
Assert.assertEquals(r.getAngle().getReal(), reverted.getAngle().getReal(), 1.0e-15);
Assert.assertEquals(-1, FieldVector3D.dotProduct(r.getAxis(), reverted.getAxis()).getReal(), 1.0e-15);
Assert.assertEquals(-1,
FieldVector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
reverted.getAxis(RotationConvention.VECTOR_OPERATOR)).getReal(),
1.0e-15);
}
@Test
@ -137,7 +223,7 @@ public class FieldRotationDfpTest {
checkVector(r.applyTo(createVector(0, 1, 0)), createVector(-1, 0, 0));
r = new FieldRotation<Dfp>(u1, u2, u1.negate(), u2.negate());
FieldVector3D<Dfp> axis = r.getAxis();
FieldVector3D<Dfp> axis = r.getAxis(RotationConvention.VECTOR_OPERATOR);
if (FieldVector3D.dotProduct(axis, createVector(0, 0, 1)).getReal() > 0) {
checkVector(axis, createVector(0, 0, 1));
} else {
@ -312,7 +398,8 @@ public class FieldRotationDfpTest {
}
@Test
public void testAngles()
@Deprecated
public void testAnglesDeprecated()
throws CardanEulerSingularityException {
DfpField field = new DfpField(15);
@ -363,63 +450,124 @@ public class FieldRotationDfpTest {
}
@Test
public void testAngles()
throws CardanEulerSingularityException {
DfpField field = new DfpField(15);
for (RotationConvention convention : RotationConvention.values()) {
RotationOrder[] CardanOrders = {
RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
};
for (int i = 0; i < CardanOrders.length; ++i) {
for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 2.0) {
for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.8) {
for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 2.0) {
FieldRotation<Dfp> r = new FieldRotation<Dfp>(CardanOrders[i],
convention,
field.newDfp(alpha1),
field.newDfp(alpha2),
field.newDfp(alpha3));
Dfp[] angles = r.getAngles(CardanOrders[i], convention);
checkAngle(angles[0], alpha1);
checkAngle(angles[1], alpha2);
checkAngle(angles[2], alpha3);
}
}
}
}
RotationOrder[] EulerOrders = {
RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
};
for (int i = 0; i < EulerOrders.length; ++i) {
for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 2.0) {
for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.8) {
for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 2.0) {
FieldRotation<Dfp> r = new FieldRotation<Dfp>(EulerOrders[i],
convention,
field.newDfp(alpha1),
field.newDfp(alpha2),
field.newDfp(alpha3));
Dfp[] angles = r.getAngles(EulerOrders[i], convention);
checkAngle(angles[0], alpha1);
checkAngle(angles[1], alpha2);
checkAngle(angles[2], alpha3);
}
}
}
}
}
}
@Test
public void testSingularities() {
DfpField field = new DfpField(20);
RotationOrder[] CardanOrders = {
RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
};
for (RotationConvention convention : RotationConvention.values()) {
RotationOrder[] CardanOrders = {
RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
};
double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.PI / 2 };
for (int i = 0; i < CardanOrders.length; ++i) {
for (int j = 0; j < singularCardanAngle.length; ++j) {
FieldRotation<Dfp> r = new FieldRotation<Dfp>(CardanOrders[i],
field.newDfp(0.1),
field.newDfp(singularCardanAngle[j]),
field.newDfp(0.3));
try {
r.getAngles(CardanOrders[i]);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.PI / 2 };
for (int i = 0; i < CardanOrders.length; ++i) {
for (int j = 0; j < singularCardanAngle.length; ++j) {
FieldRotation<Dfp> r = new FieldRotation<Dfp>(CardanOrders[i],
convention,
field.newDfp(0.1),
field.newDfp(singularCardanAngle[j]),
field.newDfp(0.3));
try {
r.getAngles(CardanOrders[i], convention);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
}
}
}
}
RotationOrder[] EulerOrders = {
RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
};
RotationOrder[] EulerOrders = {
RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
};
double[] singularEulerAngle = { 0, FastMath.PI };
for (int i = 0; i < EulerOrders.length; ++i) {
for (int j = 0; j < singularEulerAngle.length; ++j) {
FieldRotation<Dfp> r = new FieldRotation<Dfp>(EulerOrders[i],
field.newDfp(0.1),
field.newDfp(singularEulerAngle[j]),
field.newDfp(0.3));
try {
r.getAngles(EulerOrders[i]);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
double[] singularEulerAngle = { 0, FastMath.PI };
for (int i = 0; i < EulerOrders.length; ++i) {
for (int j = 0; j < singularEulerAngle.length; ++j) {
FieldRotation<Dfp> r = new FieldRotation<Dfp>(EulerOrders[i],
convention,
field.newDfp(0.1),
field.newDfp(singularEulerAngle[j]),
field.newDfp(0.3));
try {
r.getAngles(EulerOrders[i], convention);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
}
}
}
}
}
@Test
public void testQuaternion() throws MathIllegalArgumentException {
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5),
createAngle(1.7),
RotationConvention.VECTOR_OPERATOR);
double n = 23.5;
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(r1.getQ0().multiply(n), r1.getQ1().multiply(n),
r1.getQ2().multiply(n), r1.getQ3().multiply(n),
true);
r1.getQ2().multiply(n), r1.getQ3().multiply(n),
true);
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) {
@ -439,8 +587,12 @@ public class FieldRotationDfpTest {
@Test
public void testCompose() throws MathIllegalArgumentException {
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2), createAngle(0.3));
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.applyTo(r1);
FieldRotation<Dfp> r3Double = r2.applyTo(new Rotation(r1.getQ0().getReal(),
r1.getQ1().getReal(),
@ -463,8 +615,12 @@ public class FieldRotationDfpTest {
@Test
public void testComposeInverse() throws MathIllegalArgumentException {
FieldRotation<Dfp> r1 = new FieldRotation<Dfp>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(-1, 3, 2), createAngle(0.3));
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.applyInverseTo(r1);
FieldRotation<Dfp> r3Double = r2.applyInverseTo(new Rotation(r1.getQ0().getReal(),
r1.getQ1().getReal(),
@ -492,7 +648,8 @@ public class FieldRotationDfpTest {
for (int i = 0; i < 10; ++i) {
double[] unit = g.nextVector();
FieldRotation<Dfp> r = new FieldRotation<Dfp>(createVector(unit[0], unit[1], unit[2]),
createAngle(random.nextDouble()));
createAngle(random.nextDouble()),
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) {
@ -527,7 +684,7 @@ public class FieldRotationDfpTest {
for (int i = 0; i < 10; ++i) {
double[] unit1 = g.nextVector();
Rotation r1 = new Rotation(new Vector3D(unit1[0], unit1[1], unit1[2]),
random.nextDouble());
random.nextDouble(), RotationConvention.VECTOR_OPERATOR);
FieldRotation<Dfp> r1Prime = new FieldRotation<Dfp>(field.newDfp(r1.getQ0()),
field.newDfp(r1.getQ1()),
field.newDfp(r1.getQ2()),
@ -535,7 +692,8 @@ public class FieldRotationDfpTest {
false);
double[] unit2 = g.nextVector();
FieldRotation<Dfp> r2 = new FieldRotation<Dfp>(createVector(unit2[0], unit2[1], unit2[2]),
createAngle(random.nextDouble()));
createAngle(random.nextDouble()),
RotationConvention.VECTOR_OPERATOR);
FieldRotation<Dfp> rA = FieldRotation.applyTo(r1, r2);
FieldRotation<Dfp> rB = r1Prime.applyTo(r2);
@ -564,7 +722,9 @@ public class FieldRotationDfpTest {
@Test
public void testArray() throws MathIllegalArgumentException {
FieldRotation<Dfp> r = new FieldRotation<Dfp>(createAxis(2, -3, 5), createAngle(1.7));
FieldRotation<Dfp> r = new FieldRotation<Dfp>(createAxis(2, -3, 5),
createAngle(1.7),
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) {
@ -592,7 +752,9 @@ public class FieldRotationDfpTest {
Dfp[] in = new Dfp[3];
Dfp[] out = new Dfp[3];
Dfp[] rebuilt = new Dfp[3];
FieldRotation<Dfp> r = new FieldRotation<Dfp>(createVector(2, -3, 5), createAngle(1.7));
FieldRotation<Dfp> r = new FieldRotation<Dfp>(createVector(2, -3, 5),
createAngle(1.7),
RotationConvention.VECTOR_OPERATOR);
for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
for (double phi = -1.55; phi < 1.55; phi += 0.2) {
FieldVector3D<Dfp> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),
@ -623,7 +785,7 @@ public class FieldRotationDfpTest {
}
}
r = new FieldRotation<Dfp>(createVector(0, 0, 1), createAngle(FastMath.PI));
r = new FieldRotation<Dfp>(createVector(0, 0, 1), createAngle(FastMath.PI), RotationConvention.VECTOR_OPERATOR);
for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
for (double phi = -1.55; phi < 1.55; phi += 0.2) {
FieldVector3D<Dfp> u = createVector(FastMath.cos(lambda) * FastMath.cos(phi),

6
src/test/java/org/apache/commons/math4/geometry/euclidean/threed/PlaneTest.java
View File

@ -74,17 +74,17 @@ public class PlaneTest {
Plane p = new Plane(p1, p2, p3, 1.0e-10);
Vector3D oldNormal = p.getNormal();
p = p.rotate(p2, new Rotation(p2.subtract(p1), 1.7));
p = p.rotate(p2, new Rotation(p2.subtract(p1), 1.7, RotationConvention.VECTOR_OPERATOR));
Assert.assertTrue(p.contains(p1));
Assert.assertTrue(p.contains(p2));
Assert.assertTrue(! p.contains(p3));
p = p.rotate(p2, new Rotation(oldNormal, 0.1));
p = p.rotate(p2, new Rotation(oldNormal, 0.1, RotationConvention.VECTOR_OPERATOR));
Assert.assertTrue(! p.contains(p1));
Assert.assertTrue(p.contains(p2));
Assert.assertTrue(! p.contains(p3));
p = p.rotate(p1, new Rotation(oldNormal, 0.1));
p = p.rotate(p1, new Rotation(oldNormal, 0.1, RotationConvention.VECTOR_OPERATOR));
Assert.assertTrue(! p.contains(p1));
Assert.assertTrue(! p.contains(p2));
Assert.assertTrue(! p.contains(p3));

2
src/test/java/org/apache/commons/math4/geometry/euclidean/threed/PolyhedronsSetTest.java
View File

@ -146,7 +146,7 @@ public class PolyhedronsSetTest {
Vector3D barycenter = (Vector3D) tree.getBarycenter();
Vector3D s = new Vector3D(10.2, 4.3, -6.7);
Vector3D c = new Vector3D(-0.2, 2.1, -3.2);
Rotation r = new Rotation(new Vector3D(6.2, -4.4, 2.1), 0.12);
Rotation r = new Rotation(new Vector3D(6.2, -4.4, 2.1), 0.12, RotationConvention.VECTOR_OPERATOR);
tree = tree.rotate(c, r).translate(s);

254
src/test/java/org/apache/commons/math4/geometry/euclidean/threed/RotationTest.java
View File

@ -56,7 +56,8 @@ public class RotationTest {
}
@Test
public void testAxisAngle() throws MathIllegalArgumentException {
@Deprecated
public void testAxisAngleDeprecated() throws MathIllegalArgumentException {
Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * FastMath.PI / 3);
checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J);
@ -84,6 +85,72 @@ public class RotationTest {
}
@Test
public void testAxisAngleVectorOperator() throws MathIllegalArgumentException {
Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * FastMath.PI / 3, RotationConvention.VECTOR_OPERATOR);
checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_J);
checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_K);
checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_I);
double s = 1 / FastMath.sqrt(3);
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D( s, s, s));
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(-s, -s, -s));
checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
try {
new Rotation(new Vector3D(0, 0, 0), 2 * FastMath.PI / 3, RotationConvention.VECTOR_OPERATOR);
Assert.fail("an exception should have been thrown");
} catch (MathIllegalArgumentException e) {
}
r = new Rotation(Vector3D.PLUS_K, 1.5 * FastMath.PI, RotationConvention.VECTOR_OPERATOR);
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(0, 0, -1));
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(0, 0, +1));
checkAngle(r.getAngle(), 0.5 * FastMath.PI);
r = new Rotation(Vector3D.PLUS_J, FastMath.PI, RotationConvention.VECTOR_OPERATOR);
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_J);
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_J);
checkAngle(r.getAngle(), FastMath.PI);
checkVector(Rotation.IDENTITY.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_I);
checkVector(Rotation.IDENTITY.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_I);
}
@Test
public void testAxisAngleFrameTransform() throws MathIllegalArgumentException {
Rotation r = new Rotation(new Vector3D(10, 10, 10), 2 * FastMath.PI / 3, RotationConvention.FRAME_TRANSFORM);
checkVector(r.applyTo(Vector3D.PLUS_I), Vector3D.PLUS_K);
checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.PLUS_I);
checkVector(r.applyTo(Vector3D.PLUS_K), Vector3D.PLUS_J);
double s = 1 / FastMath.sqrt(3);
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D( s, s, s));
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(-s, -s, -s));
checkAngle(r.getAngle(), 2 * FastMath.PI / 3);
try {
new Rotation(new Vector3D(0, 0, 0), 2 * FastMath.PI / 3, RotationConvention.FRAME_TRANSFORM);
Assert.fail("an exception should have been thrown");
} catch (MathIllegalArgumentException e) {
}
r = new Rotation(Vector3D.PLUS_K, 1.5 * FastMath.PI, RotationConvention.FRAME_TRANSFORM);
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), new Vector3D(0, 0, -1));
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), new Vector3D(0, 0, +1));
checkAngle(r.getAngle(), 0.5 * FastMath.PI);
r = new Rotation(Vector3D.PLUS_J, FastMath.PI, RotationConvention.FRAME_TRANSFORM);
checkVector(r.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.PLUS_J);
checkVector(r.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.MINUS_J);
checkAngle(r.getAngle(), FastMath.PI);
checkVector(Rotation.IDENTITY.getAxis(RotationConvention.FRAME_TRANSFORM), Vector3D.MINUS_I);
checkVector(Rotation.IDENTITY.getAxis(RotationConvention.VECTOR_OPERATOR), Vector3D.PLUS_I);
}
@Test
public void testRevert() {
Rotation r = new Rotation(0.001, 0.36, 0.48, 0.8, true);
@ -91,7 +158,10 @@ public class RotationTest {
checkRotation(r.applyTo(reverted), 1, 0, 0, 0);
checkRotation(reverted.applyTo(r), 1, 0, 0, 0);
Assert.assertEquals(r.getAngle(), reverted.getAngle(), 1.0e-12);
Assert.assertEquals(-1, Vector3D.dotProduct(r.getAxis(), reverted.getAxis()), 1.0e-12);
Assert.assertEquals(-1,
Vector3D.dotProduct(r.getAxis(RotationConvention.VECTOR_OPERATOR),
reverted.getAxis(RotationConvention.VECTOR_OPERATOR)),
1.0e-12);
}
@Test
@ -125,7 +195,7 @@ public class RotationTest {
checkVector(r.applyTo(Vector3D.PLUS_J), Vector3D.MINUS_I);
r = new Rotation(u1, u2, u1.negate(), u2.negate());
Vector3D axis = r.getAxis();
Vector3D axis = r.getAxis(RotationConvention.VECTOR_OPERATOR);
if (Vector3D.dotProduct(axis, Vector3D.PLUS_K) > 0) {
checkVector(axis, Vector3D.PLUS_K);
} else {
@ -300,7 +370,8 @@ public class RotationTest {
}
@Test
public void testAngles()
@Deprecated
public void testAnglesDeprecated()
throws CardanEulerSingularityException {
RotationOrder[] CardanOrders = {
@ -325,7 +396,7 @@ public class RotationTest {
RotationOrder[] EulerOrders = {
RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
};
};
for (int i = 0; i < EulerOrders.length; ++i) {
for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
@ -345,43 +416,92 @@ public class RotationTest {
}
@Test
public void testSingularities() {
public void testAngles()
throws CardanEulerSingularityException {
RotationOrder[] CardanOrders = {
RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
};
double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.PI / 2 };
for (int i = 0; i < CardanOrders.length; ++i) {
for (int j = 0; j < singularCardanAngle.length; ++j) {
Rotation r = new Rotation(CardanOrders[i], 0.1, singularCardanAngle[j], 0.3);
try {
r.getAngles(CardanOrders[i]);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
}
}
}
RotationOrder[] EulerOrders = {
RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
for (RotationConvention convention : RotationConvention.values()) {
RotationOrder[] CardanOrders = {
RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
};
double[] singularEulerAngle = { 0, FastMath.PI };
for (int i = 0; i < EulerOrders.length; ++i) {
for (int j = 0; j < singularEulerAngle.length; ++j) {
Rotation r = new Rotation(EulerOrders[i], 0.1, singularEulerAngle[j], 0.3);
try {
r.getAngles(EulerOrders[i]);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
}
for (int i = 0; i < CardanOrders.length; ++i) {
for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
for (double alpha2 = -1.55; alpha2 < 1.55; alpha2 += 0.3) {
for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
Rotation r = new Rotation(CardanOrders[i], convention, alpha1, alpha2, alpha3);
double[] angles = r.getAngles(CardanOrders[i], convention);
checkAngle(angles[0], alpha1);
checkAngle(angles[1], alpha2);
checkAngle(angles[2], alpha3);
}
}
}
}
RotationOrder[] EulerOrders = {
RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
};
for (int i = 0; i < EulerOrders.length; ++i) {
for (double alpha1 = 0.1; alpha1 < 6.2; alpha1 += 0.3) {
for (double alpha2 = 0.05; alpha2 < 3.1; alpha2 += 0.3) {
for (double alpha3 = 0.1; alpha3 < 6.2; alpha3 += 0.3) {
Rotation r = new Rotation(EulerOrders[i], convention,
alpha1, alpha2, alpha3);
double[] angles = r.getAngles(EulerOrders[i], convention);
checkAngle(angles[0], alpha1);
checkAngle(angles[1], alpha2);
checkAngle(angles[2], alpha3);
}
}
}
}
}
}
@Test
public void testSingularities() {
for (RotationConvention convention : RotationConvention.values()) {
RotationOrder[] CardanOrders = {
RotationOrder.XYZ, RotationOrder.XZY, RotationOrder.YXZ,
RotationOrder.YZX, RotationOrder.ZXY, RotationOrder.ZYX
};
double[] singularCardanAngle = { FastMath.PI / 2, -FastMath.PI / 2 };
for (int i = 0; i < CardanOrders.length; ++i) {
for (int j = 0; j < singularCardanAngle.length; ++j) {
Rotation r = new Rotation(CardanOrders[i], convention, 0.1, singularCardanAngle[j], 0.3);
try {
r.getAngles(CardanOrders[i], convention);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
}
}
}
RotationOrder[] EulerOrders = {
RotationOrder.XYX, RotationOrder.XZX, RotationOrder.YXY,
RotationOrder.YZY, RotationOrder.ZXZ, RotationOrder.ZYZ
};
double[] singularEulerAngle = { 0, FastMath.PI };
for (int i = 0; i < EulerOrders.length; ++i) {
for (int j = 0; j < singularEulerAngle.length; ++j) {
Rotation r = new Rotation(EulerOrders[i], convention, 0.1, singularEulerAngle[j], 0.3);
try {
r.getAngles(EulerOrders[i], convention);
Assert.fail("an exception should have been caught");
} catch (CardanEulerSingularityException cese) {
// expected behavior
}
}
}
}
}
}
@ -389,7 +509,7 @@ public class RotationTest {
@Test
public void testQuaternion() throws MathIllegalArgumentException {
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7);
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
double n = 23.5;
Rotation r2 = new Rotation(n * r1.getQ0(), n * r1.getQ1(),
n * r1.getQ2(), n * r1.getQ3(),
@ -411,8 +531,8 @@ public class RotationTest {
@Test
public void testCompose() throws MathIllegalArgumentException {
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7);
Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3);
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.applyTo(r1);
for (double x = -0.9; x < 0.9; x += 0.2) {
@ -429,8 +549,8 @@ public class RotationTest {
@Test
public void testComposeInverse() throws MathIllegalArgumentException {
Rotation r1 = new Rotation(new Vector3D(2, -3, 5), 1.7);
Rotation r2 = new Rotation(new Vector3D(-1, 3, 2), 0.3);
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.applyInverseTo(r1);
for (double x = -0.9; x < 0.9; x += 0.2) {
@ -447,7 +567,7 @@ public class RotationTest {
@Test
public void testArray() throws MathIllegalArgumentException {
Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7);
Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7, 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) {
@ -472,7 +592,7 @@ public class RotationTest {
@Test
public void testApplyInverseTo() throws MathIllegalArgumentException {
Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7);
Rotation r = new Rotation(new Vector3D(2, -3, 5), 1.7, RotationConvention.VECTOR_OPERATOR);
for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
for (double phi = -1.55; phi < 1.55; phi += 0.2) {
Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi),
@ -495,7 +615,7 @@ public class RotationTest {
}
}
r = new Rotation(Vector3D.PLUS_K, FastMath.PI);
r = new Rotation(Vector3D.PLUS_K, FastMath.PI, RotationConvention.VECTOR_OPERATOR);
for (double lambda = 0; lambda < 6.2; lambda += 0.2) {
for (double phi = -1.55; phi < 1.55; phi += 0.2) {
Vector3D u = new Vector3D(FastMath.cos(lambda) * FastMath.cos(phi),
@ -542,6 +662,48 @@ public class RotationTest {
}
@Test
public void testGithubPullRequest22A() {
final RotationOrder order = RotationOrder.ZYX;
final double xRotation = FastMath.toDegrees(30);
final double yRotation = FastMath.toDegrees(20);
final double zRotation = FastMath.toDegrees(10);
final Vector3D startingVector = Vector3D.PLUS_I;
Vector3D appliedIndividually = startingVector;
appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, 0, 0).applyTo(appliedIndividually);
appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, yRotation, 0).applyTo(appliedIndividually);
appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, 0, xRotation).applyTo(appliedIndividually);
final Vector3D bad = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, yRotation, xRotation).applyTo(startingVector);
Assert.assertEquals(bad.getX(), appliedIndividually.getX(), 1e-12);
Assert.assertEquals(bad.getY(), appliedIndividually.getY(), 1e-12);
Assert.assertEquals(bad.getZ(), appliedIndividually.getZ(), 1e-12);
}
@Test
public void testGithubPullRequest22B() {
final RotationOrder order = RotationOrder.ZYX;
final double xRotation = FastMath.toDegrees(30);
final double yRotation = FastMath.toDegrees(20);
final double zRotation = FastMath.toDegrees(10);
final Vector3D startingVector = Vector3D.PLUS_I;
Vector3D appliedIndividually = startingVector;
appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, zRotation, 0, 0).applyTo(appliedIndividually);
appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, yRotation, 0).applyTo(appliedIndividually);
appliedIndividually = new Rotation(order, RotationConvention.FRAME_TRANSFORM, 0, 0, xRotation).applyTo(appliedIndividually);
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 Vector3D good = composite.applyTo(startingVector);
Assert.assertEquals(good.getX(), appliedIndividually.getX(), 1e-12);
Assert.assertEquals(good.getY(), appliedIndividually.getY(), 1e-12);
Assert.assertEquals(good.getZ(), appliedIndividually.getZ(), 1e-12);
}
private void checkVector(Vector3D v1, Vector3D v2) {
Assert.assertTrue(v1.subtract(v2).getNorm() < 1.0e-10);
}

4
src/test/java/org/apache/commons/math4/geometry/spherical/twod/CircleTest.java
View File

@ -17,6 +17,7 @@
package org.apache.commons.math4.geometry.spherical.twod;
import org.apache.commons.math4.geometry.euclidean.threed.Rotation;
import org.apache.commons.math4.geometry.euclidean.threed.RotationConvention;
import org.apache.commons.math4.geometry.euclidean.threed.Vector3D;
import org.apache.commons.math4.geometry.partitioning.Transform;
import org.apache.commons.math4.geometry.spherical.oned.Arc;
@ -162,7 +163,8 @@ public class CircleTest {
for (int i = 0; i < 100; ++i) {
Rotation r = new Rotation(new Vector3D(sphRandom.nextVector()),
FastMath.PI * random.nextDouble());
FastMath.PI * random.nextDouble(),
RotationConvention.VECTOR_OPERATOR);
Transform<Sphere2D, Sphere1D> t = Circle.getTransform(r);
S2Point p = new S2Point(new Vector3D(sphRandom.nextVector()));