A quaternion class. More...
#include <ignition/math/Quaternion.hh>
Public Member Functions | |
Quaternion () | |
Default Constructor. More... | |
Quaternion (const Matrix3< T > &_mat) | |
Construct from rotation matrix. More... | |
Quaternion (const Quaternion< T > &_qt) | |
Copy constructor. More... | |
Quaternion (const T &_roll, const T &_pitch, const T &_yaw) | |
Constructor from Euler angles in radians. More... | |
Quaternion (const T &_w, const T &_x, const T &_y, const T &_z) | |
Constructor. More... | |
Quaternion (const Vector3< T > &_axis, const T &_angle) | |
Constructor from axis angle. More... | |
Quaternion (const Vector3< T > &_rpy) | |
Constructor. More... | |
~Quaternion () | |
Destructor. More... | |
void | Axis (const Vector3< T > &_axis, T _a) |
Set the quaternion from an axis and angle. More... | |
void | Axis (T _ax, T _ay, T _az, T _aa) |
Set the quaternion from an axis and angle. More... | |
void | Correct () |
Correct any nan values in this quaternion. More... | |
T | Dot (const Quaternion< T > &_q) const |
Dot product. More... | |
bool | Equal (const Quaternion< T > &_qt, const T &_tol) const |
Equality test with tolerance. More... | |
Vector3< T > | Euler () const |
Return the rotation in Euler angles. More... | |
void | Euler (const Vector3< T > &_vec) |
Set the quaternion from Euler angles. The order of operations is roll, pitch, yaw around a fixed body frame axis (the original frame of the object before rotation is applied). Roll is a rotation about x, pitch is about y, yaw is about z. More... | |
void | Euler (T _roll, T _pitch, T _yaw) |
Set the quaternion from Euler angles. More... | |
Quaternion< T > | Exp () const |
Return the exponent. More... | |
void | From2Axes (const Vector3< T > &_v1, const Vector3< T > &_v2) |
Set this quaternion to represent rotation from vector _v1 to vector _v2, so that _v2.Normalize() == this * _v1.Normalize() holds. More... | |
Quaternion< T > | Integrate (const Vector3< T > &_angularVelocity, const T _deltaT) const |
Integrate quaternion for constant angular velocity vector along specified interval _deltaT . Implementation based on: http://physicsforgames.blogspot.com/2010/02/quaternions.html. More... | |
Quaternion< T > | Inverse () const |
Get the inverse of this quaternion. More... | |
void | Invert () |
Invert the quaternion. More... | |
bool | IsFinite () const |
See if a quaternion is finite (e.g., not nan) More... | |
Quaternion< T > | Log () const |
Return the logarithm. More... | |
void | Matrix (const Matrix3< T > &_mat) |
Set from a rotation matrix. More... | |
void | Normalize () |
Normalize the quaternion. More... | |
Quaternion< T > | Normalized () const |
Gets a normalized version of this quaternion. More... | |
bool | operator!= (const Quaternion< T > &_qt) const |
Not equal to operator. More... | |
Quaternion< T > | operator* (const Quaternion< T > &_q) const |
Multiplication operator. More... | |
Quaternion< T > | operator* (const T &_f) const |
Multiplication operator by a scalar. More... | |
Vector3< T > | operator* (const Vector3< T > &_v) const |
Vector3 multiplication operator. More... | |
Quaternion< T > | operator*= (const Quaternion< T > &_qt) |
Multiplication operator. More... | |
Quaternion< T > | operator+ (const Quaternion< T > &_qt) const |
Addition operator. More... | |
Quaternion< T > | operator+= (const Quaternion< T > &_qt) |
Addition operator. More... | |
Quaternion< T > | operator- () const |
Unary minus operator. More... | |
Quaternion< T > | operator- (const Quaternion< T > &_qt) const |
Subtraction operator. More... | |
Quaternion< T > | operator-= (const Quaternion< T > &_qt) |
Subtraction operator. More... | |
Quaternion< T > & | operator= (const Quaternion< T > &_qt) |
Assignment operator. More... | |
bool | operator== (const Quaternion< T > &_qt) const |
Equal to operator. More... | |
T | Pitch () const |
Get the Euler pitch angle in radians. More... | |
T | Roll () const |
Get the Euler roll angle in radians. More... | |
Vector3< T > | RotateVector (const Vector3< T > &_vec) const |
Rotate a vector using the quaternion. More... | |
Vector3< T > | RotateVectorReverse (const Vector3< T > &_vec) const |
Do the reverse rotation of a vector by this quaternion. More... | |
void | Round (int _precision) |
Round all values to _precision decimal places. More... | |
void | Scale (T _scale) |
Scale a Quaternion<T> More... | |
void | Set (T _w, T _x, T _y, T _z) |
Set this quaternion from 4 floating numbers. More... | |
void | ToAxis (Vector3< T > &_axis, T &_angle) const |
Return rotation as axis and angle. More... | |
T & | W () |
Get a mutable w component. More... | |
const T & | W () const |
Get the w component. More... | |
void | W (T _v) |
Set the w component. More... | |
T & | X () |
Get a mutable x component. More... | |
const T & | X () const |
Get the x component. More... | |
void | X (T _v) |
Set the x component. More... | |
Vector3< T > | XAxis () const |
Return the X axis. More... | |
T & | Y () |
Get a mutable y component. More... | |
const T & | Y () const |
Get the y component. More... | |
void | Y (T _v) |
Set the y component. More... | |
T | Yaw () const |
Get the Euler yaw angle in radians. More... | |
Vector3< T > | YAxis () const |
Return the Y axis. More... | |
T & | Z () |
Get a mutable z component. More... | |
const T & | Z () const |
Get the z component. More... | |
void | Z (T _v) |
Set the z component. More... | |
Vector3< T > | ZAxis () const |
Return the Z axis. More... | |
Static Public Member Functions | |
static Quaternion< T > | EulerToQuaternion (const Vector3< T > &_vec) |
Convert euler angles to quatern. More... | |
static Quaternion< T > | EulerToQuaternion (T _x, T _y, T _z) |
Convert euler angles to quatern. More... | |
static Quaternion< T > | Slerp (T _fT, const Quaternion< T > &_rkP, const Quaternion< T > &_rkQ, bool _shortestPath=false) |
Spherical linear interpolation between 2 quaternions, given the ends and an interpolation parameter between 0 and 1. More... | |
static Quaternion< T > | Squad (T _fT, const Quaternion< T > &_rkP, const Quaternion< T > &_rkA, const Quaternion< T > &_rkB, const Quaternion< T > &_rkQ, bool _shortestPath=false) |
Spherical quadratic interpolation given the ends and an interpolation parameter between 0 and 1. More... | |
Static Public Attributes | |
static const Quaternion | Identity |
math::Quaternion(1, 0, 0, 0) More... | |
static const Quaternion | Zero |
math::Quaternion(0, 0, 0, 0) More... | |
Detailed Description
A quaternion class.
Constructor & Destructor Documentation
◆ Quaternion() [1/7]
|
inline |
Default Constructor.
◆ Quaternion() [2/7]
|
inline |
Constructor.
- Parameters
-
[in] _w W param [in] _x X param [in] _y Y param [in] _z Z param
◆ Quaternion() [3/7]
|
inline |
Constructor from Euler angles in radians.
- Parameters
-
[in] _roll roll [in] _pitch pitch [in] _yaw yaw
◆ Quaternion() [4/7]
|
inline |
Constructor from axis angle.
- Parameters
-
[in] _axis the rotation axis [in] _angle the rotation angle in radians
◆ Quaternion() [5/7]
|
inlineexplicit |
Constructor.
- Parameters
-
[in] _rpy euler angles, in radians
◆ Quaternion() [6/7]
|
inlineexplicit |
Construct from rotation matrix.
- Parameters
-
[in] _mat rotation matrix (must be orthogonal, the function doesn't check it)
◆ Quaternion() [7/7]
|
inline |
Copy constructor.
- Parameters
-
[in] _qt Quaternion<T> to copy
◆ ~Quaternion()
|
inline |
Destructor.
Member Function Documentation
◆ Axis() [1/2]
|
inline |
Set the quaternion from an axis and angle.
- Parameters
-
[in] _axis Axis [in] _a Angle in radians
◆ Axis() [2/2]
|
inline |
Set the quaternion from an axis and angle.
- Parameters
-
[in] _ax X axis [in] _ay Y axis [in] _az Z axis [in] _aa Angle in radians
Referenced by Quaternion< Precision >::Axis(), Quaternion< Precision >::Quaternion(), and Quaternion< Precision >::Scale().
◆ Correct()
|
inline |
Correct any nan values in this quaternion.
Referenced by Pose3< T >::Correct().
◆ Dot()
|
inline |
Dot product.
- Parameters
-
[in] _q the other quaternion
- Returns
- the product
Referenced by Quaternion< Precision >::Slerp().
◆ Equal()
|
inline |
Equality test with tolerance.
- Parameters
-
[in] _qt Quaternion<T> for comparison [in] _tol equality tolerance
- Returns
- true if the elements of the quaternions are equal within the tolerance specified by _tol, false otherwise
Referenced by Quaternion< Precision >::operator==().
◆ Euler() [1/3]
|
inline |
Return the rotation in Euler angles.
- Returns
- This quaternion as an Euler vector
Referenced by Quaternion< Precision >::Euler(), Quaternion< Precision >::Pitch(), Quaternion< Precision >::Quaternion(), Quaternion< Precision >::Roll(), and Quaternion< Precision >::Yaw().
◆ Euler() [2/3]
|
inline |
Set the quaternion from Euler angles. The order of operations is roll, pitch, yaw around a fixed body frame axis (the original frame of the object before rotation is applied). Roll is a rotation about x, pitch is about y, yaw is about z.
- Parameters
-
[in] _vec Euler angle
Referenced by Quaternion< Precision >::EulerToQuaternion(), and Pose3< T >::Set().
◆ Euler() [3/3]
|
inline |
Set the quaternion from Euler angles.
- Parameters
-
[in] _roll Roll angle (radians). [in] _pitch Pitch angle (radians). [in] _yaw Yaw angle (radians).
◆ EulerToQuaternion() [1/2]
|
inlinestatic |
Convert euler angles to quatern.
- Parameters
-
[in] _vec The vector of angles to convert.
- Returns
- The converted quaternion.
Referenced by Quaternion< Precision >::EulerToQuaternion().
◆ EulerToQuaternion() [2/2]
|
inlinestatic |
Convert euler angles to quatern.
- Parameters
-
[in] _x rotation along x [in] _y rotation along y [in] _z rotation along z
- Returns
- The converted quaternion.
◆ Exp()
|
inline |
Return the exponent.
- Returns
- the exp
◆ From2Axes()
Set this quaternion to represent rotation from vector _v1 to vector _v2, so that _v2.Normalize() == this * _v1.Normalize() holds.
- Parameters
-
[in] _v1 The first vector [in] _v2 The second vector
Implementation inspired by http://stackoverflow.com/a/11741520/1076564
◆ Integrate()
|
inline |
Integrate quaternion for constant angular velocity vector along specified interval _deltaT
. Implementation based on: http://physicsforgames.blogspot.com/2010/02/quaternions.html.
- Parameters
-
[in] _angularVelocity Angular velocity vector, specified in same reference frame as base of this quaternion. [in] _deltaT Time interval in seconds to integrate over.
- Returns
- Quaternion at integrated configuration.
◆ Inverse()
|
inline |
Get the inverse of this quaternion.
- Returns
- Inverse quaternion
Referenced by Pose3< T >::CoordPoseSolve(), Pose3< T >::CoordPositionAdd(), Pose3< T >::CoordPositionSub(), Pose3< T >::CoordRotationSub(), Pose3< T >::Inverse(), MassMatrix3< T >::PrincipalAxesOffset(), Quaternion< Precision >::RotateVector(), Quaternion< Precision >::RotateVectorReverse(), and Inertial< T >::SetMassMatrixRotation().
◆ Invert()
|
inline |
Invert the quaternion.
◆ IsFinite()
|
inline |
See if a quaternion is finite (e.g., not nan)
- Returns
- True if quaternion is finite
Referenced by Pose3< T >::IsFinite().
◆ Log()
|
inline |
Return the logarithm.
- Returns
- the log
◆ Matrix()
|
inline |
Set from a rotation matrix.
- Parameters
-
[in] _mat rotation matrix (must be orthogonal, the function doesn't check it)
Implementation inspired by http://www.euclideanspace.com/maths/geometry/rotations/ conversions/matrixToQuaternion/
Referenced by Quaternion< Precision >::Quaternion().
◆ Normalize()
|
inline |
Normalize the quaternion.
Referenced by Quaternion< Precision >::Axis(), Pose3< T >::CoordRotationSub(), Quaternion< Precision >::Euler(), Quaternion< Precision >::From2Axes(), Quaternion< Precision >::Invert(), Matrix3< T >::Matrix3(), Matrix4< T >::Matrix4(), Quaternion< Precision >::Normalized(), and Quaternion< Precision >::Slerp().
◆ Normalized()
|
inline |
Gets a normalized version of this quaternion.
- Returns
- a normalized quaternion
◆ operator!=()
|
inline |
◆ operator*() [1/3]
|
inline |
Multiplication operator.
- Parameters
-
[in] _q Quaternion<T> for multiplication
- Returns
- This quaternion multiplied by the parameter
◆ operator*() [2/3]
|
inline |
Multiplication operator by a scalar.
- Parameters
-
[in] _f factor
- Returns
- quaternion multiplied by the scalar
◆ operator*() [3/3]
Vector3 multiplication operator.
- Parameters
-
[in] _v vector to multiply
- Returns
- The result of the vector multiplication
◆ operator*=()
|
inline |
Multiplication operator.
- Parameters
-
[in] _qt Quaternion<T> for multiplication
- Returns
- This quaternion multiplied by the parameter
◆ operator+()
|
inline |
Addition operator.
- Parameters
-
[in] _qt quaternion for addition
- Returns
- this quaternion + _qt
◆ operator+=()
|
inline |
Addition operator.
- Parameters
-
[in] _qt quaternion for addition
- Returns
- this quaternion + qt
◆ operator-() [1/2]
|
inline |
Unary minus operator.
- Returns
- negates each component of the quaternion
◆ operator-() [2/2]
|
inline |
Subtraction operator.
- Parameters
-
[in] _qt quaternion to subtract
- Returns
- this quaternion - _qt
◆ operator-=()
|
inline |
◆ operator=()
|
inline |
Assignment operator.
- Parameters
-
[in] _qt Quaternion<T> to copy
◆ operator==()
|
inline |
◆ Pitch()
|
inline |
◆ Roll()
|
inline |
◆ RotateVector()
Rotate a vector using the quaternion.
- Parameters
-
[in] _vec vector to rotate
- Returns
- the rotated vector
◆ RotateVectorReverse()
Do the reverse rotation of a vector by this quaternion.
- Parameters
-
[in] _vec the vector
- Returns
- the reversed vector
◆ Round()
|
inline |
Round all values to _precision decimal places.
- Parameters
-
[in] _precision the precision
Referenced by Pose3< T >::Round().
◆ Scale()
|
inline |
Scale a Quaternion<T>
- Parameters
-
[in] _scale Amount to scale this rotation
◆ Set()
|
inline |
Set this quaternion from 4 floating numbers.
- Parameters
-
[in] _w w [in] _x x [in] _y y [in] _z z
◆ Slerp()
|
inlinestatic |
Spherical linear interpolation between 2 quaternions, given the ends and an interpolation parameter between 0 and 1.
- Parameters
-
[in] _fT the interpolation parameter [in] _rkP the beginning quaternion [in] _rkQ the end quaternion [in] _shortestPath when true, the rotation may be inverted to get to minimize rotation
- Returns
- The result of the linear interpolation
Referenced by OnePoleQuaternion::Process(), and Quaternion< Precision >::Squad().
◆ Squad()
|
inlinestatic |
Spherical quadratic interpolation given the ends and an interpolation parameter between 0 and 1.
- Parameters
-
[in] _fT the interpolation parameter [in] _rkP the beginning quaternion [in] _rkA first intermediate quaternion [in] _rkB second intermediate quaternion [in] _rkQ the end quaternion [in] _shortestPath when true, the rotation may be inverted to get to minimize rotation
- Returns
- The result of the quadratic interpolation
◆ ToAxis()
|
inline |
Return rotation as axis and angle.
- Parameters
-
[out] _axis rotation axis [out] _angle ccw angle in radians
Referenced by Quaternion< Precision >::Scale().
◆ W() [1/3]
|
inline |
Get a mutable w component.
- Returns
- The w quaternion component.
◆ W() [2/3]
|
inline |
Get the w component.
- Returns
- The w quaternion component.
Referenced by ignition::math::eigen3::convert(), Quaternion< Precision >::Integrate(), Matrix3< T >::Matrix3(), Matrix4< T >::Matrix4(), Pose3< T >::RotatePositionAboutOrigin(), and Matrix4< T >::Rotation().
◆ W() [3/3]
|
inline |
Set the w component.
- Parameters
-
[in] _v The new value for the w quaternion component.
◆ X() [1/3]
|
inline |
Get a mutable x component.
- Returns
- The x quaternion component.
◆ X() [2/3]
|
inline |
Get the x component.
- Returns
- The x quaternion component.
Referenced by ignition::math::eigen3::convert(), Pose3< T >::CoordPoseSolve(), Pose3< T >::CoordPositionAdd(), Pose3< T >::CoordPositionSub(), Quaternion< Precision >::Integrate(), Matrix3< T >::Matrix3(), Matrix4< T >::Matrix4(), Pose3< T >::RotatePositionAboutOrigin(), and Matrix4< T >::Rotation().
◆ X() [3/3]
|
inline |
Set the x component.
- Parameters
-
[in] _v The new value for the x quaternion component.
◆ XAxis()
|
inline |
Return the X axis.
- Returns
- the X axis of the vector
◆ Y() [1/3]
|
inline |
Get a mutable y component.
- Returns
- The y quaternion component.
◆ Y() [2/3]
|
inline |
Get the y component.
- Returns
- The y quaternion component.
Referenced by ignition::math::eigen3::convert(), Pose3< T >::CoordPoseSolve(), Pose3< T >::CoordPositionAdd(), Pose3< T >::CoordPositionSub(), Quaternion< Precision >::Integrate(), Matrix3< T >::Matrix3(), Matrix4< T >::Matrix4(), Pose3< T >::RotatePositionAboutOrigin(), and Matrix4< T >::Rotation().
◆ Y() [3/3]
|
inline |
Set the y component.
- Parameters
-
[in] _v The new value for the y quaternion component.
◆ Yaw()
|
inline |
◆ YAxis()
|
inline |
Return the Y axis.
- Returns
- the Y axis of the vector
◆ Z() [1/3]
|
inline |
Get a mutable z component.
- Returns
- The z quaternion component.
◆ Z() [2/3]
|
inline |
Get the z component.
- Returns
- The z quaternion component.
Referenced by ignition::math::eigen3::convert(), Pose3< T >::CoordPoseSolve(), Pose3< T >::CoordPositionAdd(), Pose3< T >::CoordPositionSub(), Quaternion< Precision >::Integrate(), Matrix3< T >::Matrix3(), Matrix4< T >::Matrix4(), Pose3< T >::RotatePositionAboutOrigin(), and Matrix4< T >::Rotation().
◆ Z() [3/3]
|
inline |
Set the z component.
- Parameters
-
[in] _v The new value for the z quaternion component.
◆ ZAxis()
|
inline |
Return the Z axis.
- Returns
- the Z axis of the vector
Member Data Documentation
◆ Identity
|
static |
math::Quaternion(1, 0, 0, 0)
◆ Zero
|
static |
math::Quaternion(0, 0, 0, 0)
The documentation for this class was generated from the following files: