mt
 All Classes Files Functions Enumerations Groups Pages
Public Member Functions | List of all members
mt::Rotation Class Reference

Singularity-free spatial rotations class. More...

#include <rotation.h>

Inheritance diagram for mt::Rotation:
mt::Quaternion

Public Member Functions

 Rotation ()
 Default constructor. Creates the [0.0, 0.0, 0.0, 1.0] quaternion.
 Rotation (const Scalar &x, const Scalar &y, const Scalar &z, const Scalar &w)
 Rotation (const Scalar *r)
 Rotation (const Unit3 &axis, const Scalar &angle)
 Constructor for axis-angle input.
 Rotation (const Scalar &yaw, const Scalar &pitch, const Scalar &roll)
 Rotation (const Matrix3x3 &mat)
 Constructor for rotation matrix.
 Rotation (const Unit3 &u, const Unit3 &v, const Unit3 &r, const Unit3 &s)
 Constructor for two pairs of independent unit vectors.
 Rotation (const Unit3 &u, const Unit3 &v)
 Rotation (const Quaternion &q)
 Constructor for Quaternion input.
Rotationoperator= (const Quaternion &q)
Vector3 operator() (const Vector3 &v) const
 Applies rotation to input vector.
bool operator== (const Rotation &r) const
bool operator!= (const Rotation &r) const
Scalar angleCos (const Rotation &r) const
 Cosine of the angle between unit quatenions.
Scalar angle (const Rotation &r) const
Rotation inverse () const
 Quaternion inverse.
void getAxisAngle (Unit3 &axis, Scalar &angle) const
void getYpr (Scalar &yaw, Scalar &pitch, Scalar &roll) const
Matrix3x3 getMatrix () const
void setValue (const Scalar &x, const Scalar &y, const Scalar &z, const Scalar &w)
 Sets unit quaternion values.
void setValue (const Scalar *r)
 Sets unit quaternion values from pointer input.
void setAxisAngle (const Unit3 &axis, const Scalar &angle)
void setYpr (const Scalar &yaw, const Scalar &pitch, const Scalar &roll)
void setMatrix (const Matrix3x3 &mat)
void setTwoPairs (const Unit3 &u, const Unit3 &v, const Unit3 &r, const Unit3 &s)
void setOnePair (const Unit3 &u, const Unit3 &v)
- Public Member Functions inherited from mt::Quaternion
 Quaternion ()
 Default constructor. Creates the [0.0, 0.0, 0.0, 1.0] quaternion.
 Quaternion (const Scalar &x, const Scalar &y, const Scalar &z, const Scalar &w)
 Constructor for four input values, where $ xi + yj + zk + w $.
 Quaternion (const Scalar *q)
 Constructor for pointer input.
Scalar & operator[] (size_t n)
 Unchecked element access.
const Scalar & operator[] (size_t n) const
 Unchecked element access.
Quaternionoperator+= (const Quaternion &q)
Quaternionoperator-= (const Quaternion &q)
Quaternionoperator*= (const Quaternion &q)
 Quaternion product (not memberwise product).
Quaternionoperator*= (const Scalar &s)
 Scalar-quaternion product.
Quaternionoperator/= (const Scalar &s)
 Scalar-quaternion division.
bool operator== (const Quaternion &q) const
bool operator!= (const Quaternion &q) const
Scalar dot (const Quaternion &q) const
 Dot product.
Scalar length2 () const
 Squared quaternion length.
Scalar length () const
 Quaternion length.
Quaternionnormalize ()
 Normalize quaternion to unit length.
Scalar distance2 (const Quaternion &q) const
 Squared distance between quaternions.
Scalar distance (const Quaternion &q) const
 Distance between quaternions.
Scalar angleCos (const Quaternion &q) const
 Cosine of the angle between quaternions.
Scalar angle (const Quaternion &q) const
 Angle between quaternions in the range [0, pi] expressed in radians.
Quaternion conjugate () const
 Quaternion conjugate.
Quaternion slerp (const Quaternion &q, const Scalar &t) const
 Spherical linear interpolation/extrapolation with input quaternion.
Scalar & at (size_t n)
 Checked element access.
const Scalar & at (size_t n) const
 Checked element access.

Additional Inherited Members

- Protected Attributes inherited from mt::Quaternion
Scalar m_co [4]

Detailed Description

Singularity-free spatial rotations class.

The Rotation class permits defining 3D rotations from quaternion, rotation matrix, axis-angle, and yaw, pitch, and roll representations. Also, any of the above representations can be extracted from an existing Rotation instance.

Rotations can be applied to a vector using the () operator, and can be combined by means of the product operator.

The Rotation class is implemented using unit quaternions, so it provides the usual operators and functions used for expressing and manipulating quaternions, including spherical linear interpolation/extrapolation (SLERP).

The main difference from Quaternion, its base class, is that the quaternion length is always guaranteed to be unity.

Let $ \mathbf{q} $ be a unit quaternion, then the following notations are analogous: $ \mathbf{q} = [ x, y, z, w ] = xi + yj + zk + w $

with $ \Vert \mathbf{q} \Vert = 1 $.

This is an example of how to use the Rotation class:

using namespace mt;
// Constructors
Rotation R1(0.0, 0.0, 0.0, 1.0); // Quaternion input
const Unit3 axis(1.0, 0.0, 0.0);
const Scalar angle = HALF_PI;
Rotation R2(axis, angle); // Axis-angle input
Scalar yaw = -PI / Scalar(3.0); // Angle around fixed z
Scalar pitch = 0.0; // Angle around fixed y
Scalar roll = -TWO_PI; // Angle around fixed x
Rotation R3(yaw, pitch, roll); // Yaw, pitch, and roll constructor
// Accesors
const Matrix3x3 M(R1.getMatrix()); // M is the identity 3x3 matrix
R3.setAxisAngle(axis, angle); // R3 and R2 are now equivalent
// Applying a rotation
R3 *= R2; // R3 = rotation of PI about the x axis
Vector3 position(0.0, 0.0, 1.0);
position = R3(position); // position = [0.0, 0.0, -1.0]
// Interpolating rotations
R1 = slerp(R1, R2, 0.5); // R1 = rotation of PI/4 about the x axis

Constructor & Destructor Documentation

mt::Rotation::Rotation ( const Scalar &  x,
const Scalar &  y,
const Scalar &  z,
const Scalar &  w 
)
inline

Constructor for four input values.

The input values need not define a quaternion of unit length.

mt::Rotation::Rotation ( const Scalar *  r)
inlineexplicit

Constructor for pointer input.

The input values need not define a quaternion of unit length.

mt::Rotation::Rotation ( const Scalar &  yaw,
const Scalar &  pitch,
const Scalar &  roll 
)
inline

Constructor for yaw, pitch, and roll angle input. Rotations angles are measured about the fixed reference frame.

Parameters
yawRotation around the z axis
pitchRotation around the y axis
rollRotation around the x axis
mt::Rotation::Rotation ( const Unit3 u,
const Unit3 v 
)
inline

Constructor for two unit vectors (obtains rotation that transforms u into v ).

Member Function Documentation

Scalar mt::Rotation::angle ( const Rotation r) const
inline

Angle between unit quatenions in the range $ [0, \pi] $ expressed in radians.

void mt::Rotation::getAxisAngle ( Unit3 axis,
Scalar &  angle 
) const
inline
Gets axis-angle representation of rotation, where:

\[ \alpha = 2 acos(q_w) \]

and

\begin{eqnarray*} u_x & = & q_x / s \\ u_y & = & q_y / s \\ u_z & = & q_z / s \end{eqnarray*}

where $ s = \sqrt{1 - q_w^2} $.

Of the two possible rotation angles, the one belonging to $ [0, \pi] $ is chosen.

Matrix3x3 mt::Rotation::getMatrix ( ) const
inline

Gets matrix representation of rotation according to:

\[ \left[ \begin{array}{c c c} 1 - 2(q_y^2 + q_z^2) & 2(q_x q_y - q_z q_w) & 2(q_x q_z + q_y q_w) \\ 2(q_x q_y + q_z q_w) & 1 - 2(q_x^2 + q_z^2) & 2(q_y q_z - q_x q_w) \\ 2(q_x q_z - q_y q_w) & 2(q_y q_z + q_x q_w) & 1 - 2(q_x^2 + q_y^2) \\ \end{array} \right] \]

void mt::Rotation::getYpr ( Scalar &  yaw,
Scalar &  pitch,
Scalar &  roll 
) const
inline
Gets \a yaw, \a pitch, and \a roll angles representation of rotation

according to:

\begin{eqnarray*} pitch & = & asin(2(q_y q_w - q_x q_z)) \\ yaw & = & atan2 \left( \frac{2(q_x q_y + q_z q_w)}{q_x^2 - q_y^2 - q_z^2 + q_w^2} \right) \\ roll & = & atan2 \left( \frac{2(q_x q_w + q_y q_z)}{-q_x^2 - q_y^2 + q_z^2 + q_w^2} \right) \\ \end{eqnarray*}

Angular values belong to the interval $ [0, 2\pi) $. Since the above equation for the pitch angle has two solutions in the mentioned interval, the solution closest to the initial value of pitch is chosen.

Singularities (i.e., gimbal lock) arise when the pitch angle equals $ \pi / 2 $ or $ 3 \pi / 2 $. In such cases the initial value of the roll angle is used, and the yaw angle is calculated as follows:

If $ pitch = \pi / 2 $,

\[ yaw = roll -atan2(\frac{-r_{23}}{r_{13}}) \]

If $ pitch = 3 \pi / 2 $,

\[ yaw = -roll + atan2(\frac{-r_{23}}{-r_{13}}) \]

where

\begin{eqnarray*} r_{13} & = & s (q_x q_z + q_y q_w) \\ r_{23} & = & s (q_y q_z + q_x q_w) \\ s & = & 2 / \Vert \mathbf{q} \Vert ^ 2 \end{eqnarray*}

Rotations angles are measured about the fixed reference frame.

Parameters
yawRotation around the z axis
pitchRotation around the y axis
rollRotation around the x axis
Rotation & mt::Rotation::operator= ( const Quaternion q)
inline

Assignment operator.

Parameters
qThe Rotation to assign to this object.
Returns
A reference to this object.
bool mt::Rotation::operator== ( const Rotation r) const
inline

Equality operator. The comparison criterion is that the angle between the two quaternions must be equal to zero.

void mt::Rotation::setAxisAngle ( const Unit3 axis,
const Scalar &  angle 
)
inline
Sets quaternion from axis-angle input according to:

\begin{eqnarray*} q_x & = & u_x sin(\alpha / 2) \\ q_y & = & u_y sin(\alpha / 2) \\ q_z & = & u_z sin(\alpha / 2) \\ q_w & = & cos(\alpha / 2) \end{eqnarray*}

where $ \hat{\mathbf{u}} $ represents the rotation axis and $ \alpha $ represents the rotation angle.

void mt::Rotation::setMatrix ( const Matrix3x3 mat)
inline
Sets quaternion from rotation matrix.

Given a rotation matrix

\[ R = \left[ \begin{array}{c c c} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \\ \end{array} \right] \]

the associated quaternion $ \mathbf{q} $ is given by one of the following relations:

  • If $ (r_{22} \geq -r_{33}) \wedge (r_{11} \geq -r_{22}) \wedge (r_{11} \geq -r_{33}) $

    \[ \mathbf{q} = \left[ \begin{array}{c} (r_{32} - r_{23}) / s \\ (r_{13} - r_{31}) / s \\ (r_{21} - r_{12}) / s \\ 1 / (4s) \end{array} \right] \]

    with $ s = 2 \sqrt{1 + r_{11} + r_{22} + r_{33}} $
  • If $ (r_{22} < -r_{33}) \wedge (r_{11} \geq r_{22}) \wedge (r_{11} \geq r_{33}) $

    \[ \mathbf{q} = \left[ \begin{array}{c} 1 / (4s) \\ (r_{21} + r_{12}) / s \\ (r_{13} + r_{31}) / s \\ (r_{32} - r_{23}) / s \end{array} \right] \]

    with $ s = 2 \sqrt{1 + r_{11} - r_{22} - r_{33}} $
  • If $ (r_{22} \geq r_{33}) \wedge (r_{11} < r_{22}) \wedge (r_{11} < -r_{33}) $

    \[ \mathbf{q} = \left[ \begin{array}{c} (r_{21} + r_{12}) / s \\ 1 / (4s) \\ (r_{32} + r_{23}) / s \\ (r_{13} - r_{31}) / s \end{array} \right] \]

    with $ s = 2 \sqrt{1 - r_{11} + r_{22} - r_{33}} $
  • If $ (r_{22} < r_{33}) \wedge (r_{11} < -r_{22}) \wedge (r_{11} < r_{33}) $

    \[ \mathbf{q} = \left[ \begin{array}{c} (r_{13} + r_{31}) / s \\ (r_{32} + r_{23}) / s \\ 1 / (4s) \\ (r_{21} - r_{12}) / s \end{array} \right] \]

    with $ s = 2 \sqrt{1 - r_{11} - r_{22} + r_{33}} $
void mt::Rotation::setOnePair ( const Unit3 u,
const Unit3 v 
)
inline

Sets quaternion from one pair of unit vectors. The resulting rotation transforms vector u into vector v. This is equivalent to setting the rotation through an axis-angle pair, where the axis is a vector perpendicular to both u and v, and the angle is the angle between u and v.

void mt::Rotation::setTwoPairs ( const Unit3 u,
const Unit3 v,
const Unit3 r,
const Unit3 s 
)
inline
Sets quaternion from two pairs of unit vectors.

Given two pairs of independent unit vectors $ \{ \hat\mathbf{u}, \hat\mathbf{v} \} $ and $ \{ \hat\mathbf{r}, \hat\mathbf{s} \} $, the rotation $ \mathrm{R} (\hat\mathbf{u}, \hat\mathbf{v} \rightarrow \hat\mathbf{r}, \hat\mathbf{s})$ places $ \hat\mathbf{u} $ parallel to $ \hat\mathbf{r} $ and $ \hat\mathbf{v} $ in the plane defined by $ \hat\mathbf{r} $ and $ \hat\mathbf{s} $. If $ \hat\mathbf{u} \cdot \hat\mathbf{v} = \hat\mathbf{r} \cdot \hat\mathbf{s} $, then $ \hat\mathbf{v} $ will additionally be parallel to $ \hat\mathbf{s} $.

\[ \mathrm{R} (\hat\mathbf{u}, \hat\mathbf{v} \rightarrow \hat\mathbf{r}, \hat\mathbf{s}) = \left[ \begin{array}{c c c} \hat\mathbf{r} & \hat\mathbf{s}_r & \hat\mathbf{t} \end{array} \right] \left[ \begin{array}{c c c} \hat\mathbf{u} & \hat\mathbf{v}_u & \hat\mathbf{w} \end{array} \right]^T \]

where $ \{ \hat\mathbf{u}, \hat\mathbf{v}_u, \hat\mathbf{w} \} $ and $ \{ \hat\mathbf{r}, \hat\mathbf{s}_r, \hat\mathbf{t} \} $ are the orthonormal basis associated to $ \{ \hat\mathbf{u}, \hat\mathbf{v} \} $ and $ \{ \hat\mathbf{r}, \hat\mathbf{s} \} $, respectively.

void mt::Rotation::setYpr ( const Scalar &  yaw,
const Scalar &  pitch,
const Scalar &  roll 
)
inline
Sets quaternion from \a yaw, \a pitch, and \a roll input according to:

\begin{eqnarray*} q_x & = & cos(yaw / 2) cos(pitch / 2) sin(roll / 2) - sin(yaw / 2) sin(pitch / 2) cos(roll / 2) \\ q_y & = & cos(yaw / 2) sin(pitch / 2) cos(roll / 2) + sin(yaw / 2) cos(pitch / 2) sin(roll / 2) \\ q_z & = & sin(yaw / 2) cos(pitch / 2) cos(roll / 2) - cos(yaw / 2) sin(pitch / 2) sin(roll / 2) \\ q_w & = & cos(yaw / 2) cos(pitch / 2) cos(roll / 2) + sin(yaw / 2) sin(pitch / 2) sin(roll / 2) \end{eqnarray*}

The rotation matrix associated to the yaw, pitch and roll convention is:

\[ R = R_y R_p R_r \left[ \begin{array}{c c c} c_y c_p & c_y s_p s_r - s_y c_r & c_y s_p c_r + s_y s_r \\ s_y c_p & s_y s_p s_r + c_y c_r & s_y s_p c_r - c_y s_r \\ -s_p & c_p s_r & c_p c_r \end{array} \right] \]

with $ s_\alpha = sin(\alpha) $ and $ c_\alpha = cos(\alpha) $.

The yaw, pitch, and roll angles represent rotations about the fixed z, y, and x axes, respectively.

Other names given to this rotation convention are X-Y-Z fixed angles and 3-2-1 Euler Angle Sequence


The documentation for this class was generated from the following file: