org.apache.commons.math3.geometry.euclidean.threed
Class Rotation

java.lang.Object
  org.apache.commons.math3.geometry.euclidean.threed.Rotation

All Implemented Interfaces:

Serializable

public class Rotation
extends Object
implements Serializable

This class implements rotations in a three-dimensional space.

Rotations can be represented by several different mathematical entities (matrices, axis and angle, Cardan or Euler angles, quaternions). This class presents an higher level abstraction, more user-oriented and hiding this implementation details. Well, for the curious, we use normalized rotation quaternions for the internal representation. The user can build a rotation from any of these representations, and any of these representations can be retrieved from a Rotation instance (see the various constructors and getters). In addition, a rotation can also be built implicitly from a set of vectors and their image.

This implies that this class can be used to convert from one representation to another one. For example, converting a rotation matrix into a set of Cardan angles can be done using the following single line of code:

 double[] angles = new Rotation(matrix, 1.0e-10).getAngles(RotationOrder.XYZ);
 

A rotation is an operator which basically rotates three dimensional vectors into other three dimensional vectors using applyTo(Vector3D).

For example, the image of vector A using the rotation r could be obtained by : B = r.applyTo(A).

Since a rotation is basically a vectorial operator, several rotations can be composed together and the composite operation r = r1 o r2 (which means that for each vector u, r(u) = r1(r2(u))) is also a rotation. Hence we can consider that in addition to vectors, a rotation can be applied to other rotations as well (or to itself). We can apply r1 to r2 and the result we get is r = r1 o r2. This rotation could be applied to vector A like this : B = r.applyTo(A).

The rotation can be used to change the basis of a vector using applyInverseTo(Vector3D). For example, r12 represents the orientation of frame R2 with respect to frame R1: The image of a vector A expressed in frame R2 is : B = r12.applyInverseTo(A).

A rotation can be considered as a change of basis from R1 to R2 as follow : r_r2 = r12.applyTo(r_R1.revert()).

Rotations are guaranteed to be immutable objects.

Since:

1.2

Version:

$Id: Rotation.java 14281 2015-10-27 16:09:21Z chabaud $

See Also:

Vector3D, RotationOrder, Serialized Form

Field Summary

static Rotation

IDENTITY Identity rotation.

Constructor Summary

Rotation(boolean needsNormalization, double[] q)
Build a rotation from the quaternion coordinates.

Rotation(boolean needsNormalization, double q0, double q1, double q2, double q3)
Build a rotation from the quaternion coordinates.

Rotation(boolean needsNormalization, Quaternion quaternion)
Build a rotation from the quaternion.

Rotation(double[][] m, double threshold)
Build a rotation from a 3X3 matrix.

Rotation(RotationOrder order, double alpha1, double alpha2, double alpha3)
Build a rotation from three Cardan or Euler elementary rotations.

Rotation(Vector3D axis, double angle)
Build a rotation from an axis and an angle.

Rotation(Vector3D u, Vector3D v)
Build one of the rotations that transform one vector into another one.

Rotation(Vector3D u1, Vector3D u2, Vector3D v1, Vector3D v2)
Build the rotation that transforms a pair of vector into another pair.

Method Summary

void	applyInverseTo(double[] in, double[] out) Apply the inverse of the rotation to a vector stored in an array.
Rotation	applyInverseTo(Rotation r) Apply the inverse of the instance to another rotation.
Vector3D	applyInverseTo(Vector3D u) Apply the inverse of the rotation to a vector.
void	applyTo(double[] in, double[] out) Apply the rotation to a vector stored in an array.
Rotation	applyTo(Rotation r) Apply the instance to another rotation.
Vector3D	applyTo(Vector3D u) Apply the rotation to a vector.
static double	distance(Rotation r1, Rotation r2) Compute the distance between two rotations.
double	getAngle() Get the angle of the rotation.
double[]	getAngles(RotationOrder order) Get the Cardan or Euler angles corresponding to the instance.
Vector3D	getAxis() Get the normalized axis of the rotation.
double[][]	getMatrix() Get the 3X3 rotation matrix corresponding to the instance.
double[]	getQi() Get the normalized quaternion in double[] type : {q0, q1, q2, q3}.
Quaternion	getQuaternion() Get the normalized quaternion.
boolean	isEqualTo(Rotation rotation) Compare two rotations with respect to the distance between them (see distance(Rotation, Rotation)
boolean	isEqualTo(Rotation rotation, double angleThreshold, double axisThreshold) Compare two rotations with respect to their axis and angle
static Rotation	lerp(Rotation r0, Rotation r1, double h) Returns linear interpolated rotation.
Rotation	revert() Revert a rotation.
static Rotation	slerp(Rotation r0, Rotation r1, double h) Returns spherical linear interpolated rotation.
String	toString() Get a string representation for the rotation.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, wait, wait, wait

Field Detail

IDENTITY

public static final Rotation IDENTITY

Identity rotation.

Constructor Detail

Rotation

public Rotation(boolean needsNormalization,
                double q0,
                double q1,
                double q2,
                double q3)

Build a rotation from the quaternion coordinates.

A rotation can be built from a normalized quaternion, i.e. a quaternion for which q₀² + q₁² + q₂² + q₃² = 1. If the quaternion is not normalized, the constructor can normalize it in a preprocessing step.

Note that some conventions put the scalar part of the quaternion as the 4^th component and the vector part as the first three components. This is not our convention. We put the scalar part as the first component. As a consequence, the normalized quaternion coordinates are defined as :

q₀ = cos(θ/2)

q₁ = x * sin(θ/2)

q₂ = y * sin(θ/2)

q₃ = z * sin(θ/2) }

θ beeing the oriented rotation angle around the axis (x, y, z).

Parameters:

needsNormalization - if true, the coordinates are considered not to be normalized, a normalization preprocessing step is performed before using them

q0 - scalar part of the quaternion

q1 - first coordinate of the vectorial part of the quaternion

q2 - second coordinate of the vectorial part of the quaternion

q3 - third coordinate of the vectorial part of the quaternion

Throws:

MathIllegalArgumentException - thrown if norm of the quaternion is zero

Since:

3.0

Rotation

public Rotation(boolean needsNormalization,
                Quaternion quaternion)
         throws MathIllegalArgumentException

Build a rotation from the quaternion.

A rotation can be built from a normalized quaternion, i.e. a quaternion for which q₀² + q₁² + q₂² + q₃² = 1. If the quaternion is not normalized, the constructor can normalize it in a preprocessing step.

The normalized coordinates of this quaternion are defined as :

q₀ = cos(θ/2)

q₁ = x * sin(θ/2)

q₂ = y * sin(θ/2)

q₃ = z * sin(θ/2) }

θ beeing the oriented rotation angle around the axis (x, y, z).

Parameters:

needsNormalization - if true, the coordinates are considered not to be normalized, a normalization preprocessing step is performed before using them

quaternion - the quaternion of rotation

Throws:

MathIllegalArgumentException - thrown if norm of the quaternion is zero

Since:

3.0

See Also:

Quaternion

Rotation

public Rotation(boolean needsNormalization,
                double[] q)
         throws MathIllegalArgumentException

Build a rotation from the quaternion coordinates.

A rotation can be built from a normalized quaternion, i.e. a quaternion for which q₀² + q₁² + q₂² + q₃² = 1. If the quaternion is not normalized, the constructor can normalize it in a preprocessing step.

Note that some conventions put the scalar part of the quaternion as the 4^th component and the vector part as the first three components. This is not our convention. We put the scalar part as the first component. As a consequence, the normalized coordinates {q₀, q₁, q₂, q₃} are defined as :

q₀ = cos(θ/2)

q₁ = x * sin(θ/2)

q₂ = y * sin(θ/2)

q₃ = z * sin(θ/2) }

θ beeing the oriented rotation angle around the axis (x, y, z).

Parameters:

needsNormalization - if true, the coordinates are considered not to be normalized, a normalization preprocessing step is performed before using them

q - the quaternion of rotation

Throws:

MathIllegalArgumentException - thrown if norm of the quaternion is zero

Since:

3.0

Rotation

public Rotation(Vector3D axis,
                double angle)
         throws MathIllegalArgumentException

Build a rotation from an axis and an angle.

We use the convention that angles are oriented according to the effect of the rotation on vectors around the axis. That means that if (i, j, k) is a direct frame and if we first provide +k as the axis and π/2 as the angle to this constructor, and then apply the instance to +i, we will get +j.

Another way to represent our convention is to say that a rotation of angle θ about the unit vector (x, y, z) is the same as the rotation build from quaternion components { cos(θ/2), x * sin(θ/2), y * sin(θ/2), z * sin(θ/2) }. No minus sign on the angle!

Parameters:

axis - axis around which to rotate

angle - rotation angle.

Throws:

MathIllegalArgumentException - if the axis norm is zero

Rotation

public Rotation(double[][] m,
                double threshold)
         throws NotARotationMatrixException

Build a rotation from a 3X3 matrix.

Rotation matrices are orthogonal matrices, i.e. unit matrices (which are matrices for which m.m^T = I) with real coefficients. The module of the determinant of unit matrices is 1, among the orthogonal 3X3 matrices, only the ones having a positive determinant (+1) are rotation matrices.

When a rotation is defined by a matrix with truncated values (typically when it is extracted from a technical sheet where only four to five significant digits are available), the matrix is not orthogonal anymore. This constructor handles this case transparently by using a copy of the given matrix and applying a correction to the copy in order to perfect its orthogonality. If the Frobenius norm of the correction needed is above the given threshold, then the matrix is considered to be too far from a true rotation matrix and an exception is thrown.

Parameters:

m - rotation matrix

threshold - convergence threshold for the iterative orthogonality correction (convergence is reached when the difference between two steps of the Frobenius norm of the correction is below this threshold)

Throws:

NotARotationMatrixException - if the matrix is not a 3X3 matrix, or if it cannot be transformed into an orthogonal matrix with the given threshold, or if the determinant of the resulting orthogonal matrix is negative

Rotation

public Rotation(Vector3D u1,
                Vector3D u2,
                Vector3D v1,
                Vector3D v2)
         throws MathArithmeticException

Build the rotation that transforms a pair of vector into another pair.

Except for possible scale factors, if the instance were applied to the pair (u₁, u₂) it will produce the pair (v₁, v₂).

If the angular separation between u₁ and u₂ is not the same as the angular separation between v₁ and v₂, then a corrected v'₂ will be used rather than v₂, the corrected vector will be in the (v₁, v₂) plane.

Parameters:

u1 - first vector of the origin pair

u2 - second vector of the origin pair

v1 - desired image of u1 by the rotation

v2 - desired image of u2 by the rotation

Throws:

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)

Rotation

public Rotation(Vector3D u,
                Vector3D v)
         throws MathArithmeticException

Build one of the rotations that transform one vector into another one.

Except for a possible scale factor, if the instance were 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 arbitrary rotation axis is chosen.

Parameters:

u - origin vector

v - desired image of u by the rotation

Throws:

MathArithmeticException - if the norm of one of the vectors is zero

Rotation

public Rotation(RotationOrder order,
                double alpha1,
                double alpha2,
                double alpha3)

Build a rotation from three Cardan or Euler elementary rotations.

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.

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).

Parameters:

order - order of rotations to use

alpha1 - angle of the first elementary rotation

alpha2 - angle of the second elementary rotation

alpha3 - angle of the third elementary rotation

Method Detail

revert

public Rotation revert()

Revert a rotation. Build a rotation which reverse the effect of another rotation. This means that if r(u) = v, then r.revert(v) = u. The instance is not changed.

Returns:

a new rotation whose effect is the reverse of the effect of the instance

getAxis

public Vector3D getAxis()

Get the normalized axis of the rotation.

Returns:

normalized axis of the rotation

See Also:

Rotation(Vector3D, double)

getQuaternion

public Quaternion getQuaternion()

Get the normalized quaternion.

The coordinates of this quaternion are defined as :

q₀ = cos(θ/2)

q₁ = x * sin(θ/2)

q₂ = y * sin(θ/2)

q₃ = z * sin(θ/2) }

θ beeing the oriented rotation angle around the axis (x, y, z).

Returns:

the quaternion.

Since:

3.0

getQi

public double[] getQi()

Get the normalized quaternion in double[] type : {q₀, q₁, q₂, q₃}.

Those coordinates are defined as :

q₀ = cos(θ/2)

q₁ = x * sin(θ/2)

q₂ = y * sin(θ/2)

q₃ = z * sin(θ/2) }

θ beeing the oriented rotation angle around the axis (x, y, z).

Returns:

the quaternion.

Since:

3.0

getAngle

public double getAngle()

Get the angle of the rotation. inverse trigo function are protected because for a normalize quaternion it is not possible to have asin(x > 1) when q0 is different from 0 else acos is called with a value between -0.1 and 0.1.

Returns:

angle of the rotation (between 0 and π)

See Also:

Rotation(Vector3D, double)

getAngles

public double[] getAngles(RotationOrder order)

Get the Cardan or Euler angles corresponding to the instance.

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₁, a₂ and a₃ is the same as the rotation defined by the angles π + a₁, π - a₂ and π + a₃. This method implements the following arbitrary choices:

• for Cardan angles, the chosen set is the one for which the second angle is between -π/2 and π/2 (i.e its cosine is positive),
• for Euler angles, the chosen set is the one for which the second angle is between 0 and π (i.e its sine is positive).

Cardan and Euler angle have a very disappointing drawback: all of them have singularities. For Cardan angles, this is often called gimbal lock. There is nothing to do to prevent this, it is an intrinsic problem with Cardan and Euler representation (but not a problem with the rotation itself, which is perfectly well defined). For Cardan angles, singularities occur when the second angle is close to -π/2 or +π/2, for Euler angle singularities occur when the second angle is close to 0 or π, this implies that the identity rotation is always singular for Euler angles!

Parameters:

order - rotation order to use

Returns:

an array of three angles, in the order specified by the set

getMatrix

public double[][] getMatrix()

Get the 3X3 rotation matrix corresponding to the instance.

Returns:

the rotation matrix corresponding to the instance

applyTo

public Vector3D applyTo(Vector3D u)

Apply the rotation to a vector. The image v' of a vector v is v' = Q.v.Q'.

Parameters:

u - vector to apply the rotation to

Returns:

a new vector which is the image of u by the rotation

applyTo

public void applyTo(double[] in,
                    double[] out)

Apply the rotation to a vector stored in an array. The image v' of a vector v is v' = Q.v.Q'.

Parameters:

in - an array with three items which stores vector to rotate

out - an array with three items to put result to (it can be the same array as in)

applyInverseTo

public Vector3D applyInverseTo(Vector3D u)

Apply the inverse of the rotation to a vector. The image v' of a vector v applying the inverse of the rotation is v' = Q'.v.Q.

Parameters:

u - vector to apply the inverse of the rotation to

Returns:

a new vector which such that u is its image by the rotation

applyInverseTo

public void applyInverseTo(double[] in,
                           double[] out)

Apply the inverse of the rotation to a vector stored in an array. The image v' of a vector v applying the inverse of the rotation is v' = Q'.v.Q.

Parameters:

in - an array with three items which stores vector to rotate

out - an array with three items to put result to (it can be the same array as in)

applyTo

public Rotation applyTo(Rotation r)

Apply the instance to another rotation.

Applying the instance to a rotation is creating a composed rotation in the following order :

R3 = R2.applyTo(R1) is equivalent to R3 = R2 o R1

Example :

The vector u being transformed into v by R1 : v = R1(u)

The vector v being transformed into w by R2 : w = R2(v)

w = R2(v) = R2(R1(u)) = R2 o R1(u) = R3(u)

Parameters:

r - rotation to apply the rotation to

Returns:

a new rotation which is the composition of r by the instance

applyInverseTo

public Rotation applyInverseTo(Rotation r)

Apply the inverse of the instance to another rotation.

Applying the inverse of the instance to a rotation is creating a composed rotation in the following order :

R3 = R2.applyInverseTo(R1) is equivalent to R3 = R2^-1 o R1

Example :

The vector u being transformed into v by R1 : v = R1(u)

The vector v being transformed into w by R2^-1 : w = R2^-1(v)

w = R2^-1(v) = R2^-1(R1(u)) = R2^-1 o R1(u) = R3(u)

Parameters:

r - rotation to apply the rotation to

Returns:

a new rotation which is the composition of r by the inverse of the instance

distance

public static double distance(Rotation r1,
                              Rotation r2)

Compute the distance between two rotations.

The distance is intended here as a way to check if two rotations are almost similar (i.e. they transform vectors the same way) or very different. It is mathematically defined as the angle of the rotation r that prepended to one of the rotations gives the other one:

        r1(r) = r2
 

This distance is an angle between 0 and π. Its value is the smallest possible upper bound of the angle in radians between r₁(v) and r₂(v) for all possible vectors v. This upper bound is reached for some v. The distance is equal to 0 if and only if the two rotations are identical.

Comparing two rotations should always be done using this value rather than for example comparing the components of the quaternions. It is much more stable, and has a geometric meaning. Also comparing quaternions components is error prone since for example quaternions (0.36, 0.48, -0.48, -0.64) and (-0.36, -0.48, 0.48, 0.64) represent exactly the same rotation despite their components are different (they are exact opposites).

Parameters:

r1 - first rotation

r2 - second rotation

Returns:

distance between r1 and r2

isEqualTo

public boolean isEqualTo(Rotation rotation,
                         double angleThreshold,
                         double axisThreshold)

Compare two rotations with respect to their axis and angle

Parameters:

rotation - : the rotation with which one want s to compare this rotation

angleThreshold - : threshold below which one the angle between the rotations is neglectable

axisThreshold - : threshold below which one the angle between the axis of the rotations is neglectable

Returns:

true if the rotation can be considered similar according to the given thresholds, false otherwise

Since:

1.0

isEqualTo

public boolean isEqualTo(Rotation rotation)

Compare two rotations with respect to the distance between them (see distance(Rotation, Rotation)

Parameters:

rotation - : the rotation with which one want s to compare this rotation

Returns:

true if the rotation can be considered similar, false otherwise

Since:

1.0

lerp

public static Rotation lerp(Rotation r0,
                            Rotation r1,
                            double h)

Returns linear interpolated rotation. h parameter must be in [0;1] range.

Parameters:

r0 - rotation at interpolation parameter h=0

r1 - rotation at interpolation parameter h=1

h - interpolation parameter, must be in [0;1] range

Returns:

linear interpolated rotation at interpolation parameter h

Since:

1.0

slerp

public static Rotation slerp(Rotation r0,
                             Rotation r1,
                             double h)

Returns spherical linear interpolated rotation. h interpolation parameter must be in [0;1] range.

Parameters:

r0 - rotation at interpolation parameter h=0

r1 - rotation at interpolation parameter h=1

h - interpolation parameter, must be in [0;1] range

Returns:

spherical linear interpolated rotation at interpolation parameter t

Since:

1.0

toString

public String toString()

Get a string representation for the rotation.

Overrides:

toString in class Object

Returns:

a string representation for this rotation