596 lines
21 KiB
C++
596 lines
21 KiB
C++
////////////////////////////////////////////////////////////////////
|
|
// Rotation.h
|
|
//
|
|
// Copyright 2007 cDc@seacave
|
|
// Distributed under the Boost Software License, Version 1.0
|
|
// (See http://www.boost.org/LICENSE_1_0.txt)
|
|
|
|
#ifndef __SEACAVE_ROTATION_H__
|
|
#define __SEACAVE_ROTATION_H__
|
|
|
|
|
|
// I N C L U D E S /////////////////////////////////////////////////
|
|
|
|
|
|
// D E F I N E S ///////////////////////////////////////////////////
|
|
|
|
#ifndef _RELEASE
|
|
#define _CPC_DEBUG
|
|
#endif
|
|
|
|
#define CPC_ERROR(msg) { SLOG(msg); ASSERT(false); }
|
|
#if TD_VERBOSE != TD_VERBOSE_OFF
|
|
#define CPC_SLOG(msg) if (VERBOSITY_LEVEL > 0) SLOG(msg)
|
|
#define CPC_LOG(t, msg) CPC_SLOG(msg)
|
|
#else
|
|
#define CPC_SLOG(msg)
|
|
#define CPC_LOG(t, msg)
|
|
#endif
|
|
|
|
|
|
namespace SEACAVE {
|
|
|
|
// S T R U C T S ///////////////////////////////////////////////////
|
|
|
|
// generic matrix struct
|
|
#define QUATERNION_EPSILON 1E-10 // DBL_EPSILON, too small (dherzog)
|
|
|
|
// forward declaration to avoid mutual inclusion
|
|
template <typename TYPE> class TRMatrixBase;
|
|
|
|
/** @class TQuaternion
|
|
* @test tested with TestRotationConversion.cpp
|
|
@ingroup g_geometry
|
|
@brief class for rotation with axis and angle
|
|
|
|
Quaternions can be used for rotation around an axis through the
|
|
origin by spcecifing the normal direction vector of the axis and
|
|
an angle.
|
|
|
|
The rotation for positive angles is clockwise,
|
|
when looking in direction of the axis.
|
|
|
|
Contains qx=[0], qy=[1], qz=[2], qw=[3].
|
|
|
|
Not every quaternion describes a rotation.
|
|
Only quaternions of the form:
|
|
|
|
qx = x * sin(phi/2) , which is the imaginary part i
|
|
|
|
qy = y * sin(phi/2) , which is the imaginary part j
|
|
|
|
qz = z * sin(phi/2) , which is the imaginary part k
|
|
|
|
qw = cos(phi/2), , which is the real part
|
|
|
|
where (x, y, z) is the normalized direction vector of the axis
|
|
and phi is the angle of rtoation.
|
|
|
|
Some useful quaternions:
|
|
x y z w Description
|
|
0 0 0 1 Identity quaternion,
|
|
no rotation
|
|
1 0 0 0 180' turn around X axis
|
|
0 1 0 0 180' turn around Y axis
|
|
0 0 1 0 180' turn around Z axis
|
|
sqrt(0.5) 0 0 sqrt(0.5) 90' rotation around X axis
|
|
0 sqrt(0.5) 0 sqrt(0.5) 90' rotation around Y axis
|
|
0 0 sqrt(0.5) sqrt(0.5) 90' rotation around Z axis
|
|
-sqrt(0.5) 0 0 sqrt(0.5) -90' rotation around X axis
|
|
0 -sqrt(0.5) 0 sqrt(0.5) -90' rotation around Y axis
|
|
0 0 -sqrt(0.5) sqrt(0.5) -90' rotation around Z axis
|
|
|
|
@author grest 06/2003
|
|
*/
|
|
template <typename TYPE>
|
|
class TQuaternion : public TMatrix<TYPE,4,1>
|
|
{
|
|
public:
|
|
typedef TMatrix<TYPE,4,1> Base;
|
|
typedef TMatrix<TYPE,4,4> Mat;
|
|
typedef TMatrix<TYPE,3,1> Vec;
|
|
typedef TMatrix<TYPE,3,3> Rot;
|
|
|
|
using Base::val;
|
|
|
|
public:
|
|
static const TQuaternion IDENTITY;
|
|
|
|
public:
|
|
inline ~TQuaternion();
|
|
|
|
inline TQuaternion();
|
|
|
|
inline TQuaternion(const Base& q);
|
|
|
|
inline TQuaternion(TYPE i, TYPE j, TYPE k, TYPE r);
|
|
|
|
inline TQuaternion(const Rot& R);
|
|
|
|
inline void SetIdentity();
|
|
|
|
/**
|
|
* Sets all parts of quaternion in mathematical order,
|
|
* real part first, then 1.,2. and 3. imaginary part
|
|
*/
|
|
inline void SetQuaternion(TYPE real, TYPE i, TYPE j, TYPE k);
|
|
|
|
/** returns the Inverse rotation TQuaternion
|
|
*/
|
|
inline TQuaternion<TYPE> Inverse() const;
|
|
|
|
/** inverts this, by changing the rotation axis by *=-1
|
|
*/
|
|
inline void Invert();
|
|
|
|
/** makes TQuaternion-representation unique, ensuring vector lies
|
|
in upper (by real part) hemisphere. (inplace)
|
|
*/
|
|
inline void MakeUnique();
|
|
|
|
/** quaternion multiplication: this= this * quat
|
|
*/
|
|
inline void Mult(const TQuaternion<TYPE> &quat);
|
|
|
|
/** quaternion multiplication: res = this * arg
|
|
*/
|
|
inline void Mult(const TQuaternion<TYPE> &arg,
|
|
TQuaternion<TYPE> &res) const;
|
|
|
|
/** quaternion multiplication: this = quat * this
|
|
*/
|
|
inline void MultLeft(const TQuaternion<TYPE> &quat);
|
|
|
|
/** rotates the given Vector with the quaternion ( q v q* )
|
|
the resulting vector is given in res
|
|
@returns 0 in case of no error
|
|
@author Daniel Grest, June 2003
|
|
*/
|
|
inline int MultVec(const Vec &vec, Vec &res) const;
|
|
|
|
/* rets result from MultVec()
|
|
@author herzog 2005-07-15 */
|
|
inline Vec MultVec(const Vec &vec) const;
|
|
|
|
/** Computes this^(scale), which scales the angle from axis/angle-
|
|
representation with 'scale'. This is the same like inter-/extra-
|
|
polating between (0,0,0,1)-quaternion and this!
|
|
*/
|
|
TQuaternion<TYPE> Power(const TYPE & scale) const;
|
|
|
|
/** Linear interpolation between this and given quaternion.
|
|
@note Quaternions are assumed to be unit quaternions! */
|
|
TQuaternion<TYPE> InterpolateLinear(const TQuaternion<TYPE> &to, const TYPE & t) const;
|
|
|
|
/** Spherical interpolation between this and given quaternion.
|
|
@note Quaternions are assumed to be unit quaternions! */
|
|
TQuaternion<TYPE> Interpolate(const TQuaternion<TYPE> &to, const TYPE & t) const;
|
|
|
|
/** sets the quaternion with given rotation axis and angle (in rad)
|
|
@returns 0 in case of no error
|
|
@author Daniel Grest, June 2003
|
|
*/
|
|
inline void SetValueAsAxisRad(const Vec &axis, TYPE angle);
|
|
|
|
/** sets the quaternion with given rotation axis and angle (in rad)
|
|
@returns 0 in case of no error
|
|
@author Daniel Grest, June 2003
|
|
*/
|
|
|
|
inline void SetValueAsAxisRad(TYPE axisX, TYPE axisY, TYPE axisZ, TYPE angle);
|
|
|
|
/** Returns matrix which expresses (left) quaternion multiplication.
|
|
* A quaternions stored in an object can be understood as a 4-vector
|
|
* q =(ix iy iz r)^T.
|
|
* Left multiplying a quaternion q2 with a quaternion q1 can be
|
|
* expressed in terms of matrix multiplication as
|
|
* qRes = q1*q2 = M(q1)*q2.
|
|
* This method returns the matrix M(*this).
|
|
*/
|
|
Mat GetQuaternionMultMatrixLeft() const;
|
|
|
|
/** Returns matrix which expresses right quaternion multiplication.
|
|
* Right multiplying a quaternion q1 with quaternion q2 can be
|
|
* expressed as qRes = q1*q2 = N(q2)*q1.
|
|
* This method returns the matrix N(*this).
|
|
*/
|
|
Mat GetQuaternionMultMatrixRight() const;
|
|
|
|
/** Returns rotation axis. See GetRotationAxisAngle(). */
|
|
Vec GetRotationAxis() const;
|
|
|
|
/** Returns rotation angle notation in radians.
|
|
Note that the returned angle resides in the interval [0,PI)! */
|
|
TYPE GetRotationAngle() const;
|
|
|
|
/** Returns rotation in axis and angle notation (angle in radians).
|
|
Note that the returned angle resides in the interval [0,PI) and
|
|
the axis points into the according direction!
|
|
@author woelk 12 2002 */
|
|
int GetAxisAngle(Vec& axis, TYPE& angle) const;
|
|
|
|
/** Sets quaternion as concatenated rotations around
|
|
x,y,z-axis; this = q_x * q_y * q_z (BIAS-RMatrix conform)
|
|
@author herzog 2005-07-19 */
|
|
int SetXYZ(TYPE radX, TYPE radY, TYPE radZ);
|
|
|
|
/** Sets quaternion as concatenated rotations around
|
|
x,y,z-axis; this = q_z * q_y * q_x (BIAS-RMatrix conform)
|
|
@author herzog 2005-07-19 */
|
|
int SetZYX(TYPE radX, TYPE radY, TYPE radZ);
|
|
|
|
|
|
/** Scales quaternion to unit length, i.e. norm equals 1. */
|
|
inline void Normalize() {
|
|
const TYPE n = norm(*this);
|
|
if (ISZERO(n))
|
|
*this = IDENTITY;
|
|
else
|
|
*this *= TYPE(1) / n;
|
|
}
|
|
inline bool IsNormalized() const {
|
|
return ISEQUAL(norm(*this), TYPE(1));
|
|
}
|
|
|
|
/** assignment operator
|
|
@author grest 06 2003 */
|
|
TQuaternion<TYPE>& operator=(const TQuaternion<TYPE>& vec);
|
|
|
|
/** Enforces rigid coupling constraint for this and the other given unit
|
|
quaternion (i.e. equal rotation angle resp. scalar part of both).
|
|
Computes direct simple interpolation of both unit quaternion where
|
|
the scalar parts of the resulting unit quaternion are identical.
|
|
This solution can be used as initial guess for numerical refinement.
|
|
@author esquivel 02/2012 */
|
|
void EnforceRigidCouplingConstraint(TQuaternion<TYPE> &other);
|
|
|
|
/** Enforces rigid coupling constraint for all quaternion in given vector.
|
|
@author esquivel 02/2012 */
|
|
static void EnforceRigidCouplingConstraint(std::vector< TQuaternion<TYPE> > &quats);
|
|
}; // class
|
|
template <typename TYPE> const TQuaternion<TYPE> TQuaternion<TYPE>::IDENTITY(0,0,0,1);
|
|
/*----------------------------------------------------------------*/
|
|
typedef TQuaternion<float> QuaternionF;
|
|
typedef TQuaternion<double> QuaternionD;
|
|
/*----------------------------------------------------------------*/
|
|
|
|
|
|
// generic rotation matrix struct
|
|
#define ROTATION_MATRIX_EPSILON DBL_EPSILON
|
|
|
|
/** @class TRMatrixBase
|
|
@ingroup g_geometry
|
|
@brief 3D rotation matrix
|
|
@author frahm, woelk **/
|
|
template <typename TYPE>
|
|
class TRMatrixBase : public TMatrix<TYPE,3,3>
|
|
{
|
|
public:
|
|
typedef TMatrix<TYPE,3,3> Base;
|
|
typedef Base Mat;
|
|
typedef TMatrix<TYPE,3,1> Vec;
|
|
typedef TQuaternion<TYPE> Quat;
|
|
typedef typename Base::Base BaseBase;
|
|
|
|
using Base::val;
|
|
|
|
public:
|
|
/** @brief Constructor setting matrix to identity */
|
|
inline TRMatrixBase();
|
|
inline TRMatrixBase(const BaseBase& rhs) : Base(rhs) {}
|
|
template <typename T> inline TRMatrixBase(const cv::Matx<T,3,3>& rhs) : Base(rhs) {}
|
|
|
|
/** @brief Copy constructor from 3x3 matrix
|
|
@attention Orthonormality of matrix is enforced automatically! */
|
|
inline TRMatrixBase(const Mat& mat);
|
|
|
|
/** @brief Initialization from parametrized rotation (axis-angle) */
|
|
inline TRMatrixBase(const Vec& rot);
|
|
|
|
/** @brief Initialization from rotation axis w and angle phi (in rad) using Rodrigues' formula */
|
|
inline TRMatrixBase(const Vec& w, const TYPE phi);
|
|
|
|
/** @brief Initialization from quaternion */
|
|
inline TRMatrixBase(const Quat& q);
|
|
|
|
/** @brief Initialization with the rotation from roll/pitch/yaw (in rad) */
|
|
inline TRMatrixBase(TYPE roll, TYPE pitch, TYPE yaw);
|
|
|
|
/** @brief Initialization with the rotation from direction dir0 to direction dir1 */
|
|
inline TRMatrixBase(const Vec& dir0, const Vec& dir1);
|
|
|
|
template <typename T> inline TRMatrixBase& operator = (const cv::Matx<T,3,3>& rhs) { BaseBase::operator = (rhs); return *this; }
|
|
inline TRMatrixBase& operator = (const cv::Mat& rhs) { BaseBase::operator = (rhs); return *this; }
|
|
#ifdef _USE_EIGEN
|
|
inline TRMatrixBase& operator = (const typename Base::EMat& rhs) { Base::operator = (rhs); return *this; }
|
|
#endif
|
|
|
|
/** @brief Set Euler angles (in rad) in order XYZ
|
|
|
|
Set angles either in order 1. z, 2. y, 3. x and fixed axes
|
|
or in order 1. x, 2. y, 3. z and moving axes.
|
|
Angles are measured as in mathematics (counter-clockwise), i.e.
|
|
Set(x, 0, 0) results in the following matrix:
|
|
|
|
| 1 0 0 |
|
|
| 0 cos(x) -sin(x) |
|
|
| 0 sin(x) cos(x) |
|
|
|
|
(*this) = Rx*Ry*Rz =
|
|
|
|
| c(y)c(z) -c(y)s(z) s(y) |
|
|
| c(z)s(x)s(y)+c(x)s(z) c(x)c(z)-s(x)s(y)s(z) -c(y)s(x) |
|
|
| -c(x)c(z)s(y)+s(x)s(z) c(z)s(x)+c(x)s(y)s(z) c(x)c(y) |
|
|
|
|
with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ
|
|
|
|
@author frahm, woelk */
|
|
void SetXYZ(TYPE PhiX, TYPE PhiY, TYPE PhiZ);
|
|
|
|
|
|
/** @brief Set Euler angles (in rad) in order XYZ from vector */
|
|
inline void SetXYZ(const Vec& r)
|
|
{ SetXYZ(r[0], r[1], r[2]); }
|
|
|
|
/** @brief Set Euler angles (in rad) in order ZYX
|
|
|
|
Set angles either in order 1. z, 2. y, 3. x and moving axes
|
|
or in order 1. x, 2. y, 3. z and fixed axes.
|
|
Angles are measured as in mathematics (counter-clockwise), i.e.
|
|
Set(x, 0, 0) results in the following matrix:
|
|
|
|
| 1 0 0 |
|
|
| 0 cos(x) -sin(x) |
|
|
| 0 sin(x) cos(x) |
|
|
|
|
(*this) = Rz*Ry*Rx =
|
|
|
|
| c(y)c(z) s(x)s(y)c(z)-c(x)s(z) c(x)s(y)c(z)+s(x)s(z) |
|
|
| c(y)s(z) s(x)s(y)s(z)+c(x)c(z) c(x)s(y)s(z)-s(x)s(z) |
|
|
| -s(y) s(x)c(y) c(x)c(y) |
|
|
|
|
with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ
|
|
|
|
@author frahm, woelk */
|
|
void SetZYX(TYPE PhiX, TYPE PhiY, TYPE PhiZ);
|
|
|
|
/** @brief Set Euler angles (in rad) in order ZYX from vector */
|
|
inline void SetZYX(const Vec& r)
|
|
{ SetZYX(r[0], r[1], r[2]); }
|
|
|
|
/** @brief Set Euler angles (in rad) in order ZXY
|
|
|
|
Set angles either in order 1. z, 2. x, 3. y and moving axes
|
|
or in order 1. y, 2. x, 3. z and fixed axes.
|
|
Angles are measured as in mathematics (counter-clockwise), i.e.
|
|
Set(x, 0, 0) results in the following matrix:
|
|
|
|
| 1 0 0 |
|
|
| 0 cos(x) -sin(x) |
|
|
| 0 sin(x) cos(x) |
|
|
|
|
(*this) = Rz*Rx*Ry =
|
|
|
|
| c(y)c(z)-s(x)s(y)s(z), c(x)s(z), s(y)c(z)+s(x)c(y)s(z) |
|
|
| s(z)c(y)+s(x)s(y)c(z), c(x)c(z), s(y)*s(z)+s(x)-c(y)c(z) |
|
|
| -c(x)s(y) , s(x) , c(x)c(y) |
|
|
|
|
with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ
|
|
|
|
@author haase */
|
|
void SetZXY(TYPE PhiX, TYPE PhiY, TYPE PhiZ);
|
|
|
|
/** @brief Set Euler angles (in rad) in order ZXY from vector */
|
|
inline void SetZXY(const Vec& r)
|
|
{ SetZXY(r[0], r[1], r[2]); }
|
|
|
|
/** @brief Set Euler angles (in rad) in order YXZ
|
|
|
|
Set angles either in order 1. y, 2. x, 3. z and moving axes
|
|
or in order 1. z, 2. x, 3. y and fixed axes.
|
|
Angles are measured as in mathematics (counter-clockwise), i.e.
|
|
Set(x, 0, 0) results in the following matrix:
|
|
|
|
| 1 0 0 |
|
|
| 0 cos(x) sin(x) |
|
|
| 0 -sin(x) cos(x) |
|
|
|
|
(*this) = Rz*Rx*Ry =
|
|
|
|
| c(y)c(z)+s(x)s(y)s(z), c(x)s(z), -s(y)c(z)+s(x)c(y)s(z) |
|
|
| -s(z)c(y)+s(x)s(y)c(z), c(x)c(z), -s(y)*-s(z)+s(x)c(y)c(z) |
|
|
| c(x)s(y) , -s(x) , c(x)c(y) |
|
|
|
|
with s(g) = sin(g), c(g) = cos(g), x = PhiX, y = PhiY and z = PhiZ
|
|
|
|
@author haase */
|
|
void SetYXZ(TYPE PhiZ, TYPE PhiX, TYPE PhiY);
|
|
|
|
/** @brief Set Euler angles (in rad) in order YXZ from vector */
|
|
inline void SetYXZ(const Vec& r)
|
|
{ SetYXZ(r[0], r[1], r[2]); }
|
|
|
|
/** @brief Set from rotation axis w and angle phi (in rad)
|
|
@param w Axis vector w will be normalized to length 1, so we need
|
|
|w|>1e-6 if phi != 0, otherwise an exception is thrown
|
|
@param phi Rotation angle is given in radians
|
|
@author evers, woelk */
|
|
void Set(const Vec& w, TYPE phi);
|
|
|
|
/** set this matrix from 3 vectors each representing a column*/
|
|
void SetFromColumnVectors(const Vec& v0,
|
|
const Vec& v1,
|
|
const Vec& v2);
|
|
|
|
/** set this matrix from 3 vectors, each representing a row */
|
|
void SetFromRowVectors(const Vec& v0,
|
|
const Vec& v1,
|
|
const Vec& v2);
|
|
|
|
/** @brief Set from rotation axis * angle (modified Rodrigues vector)
|
|
@author evers */
|
|
void SetFromAxisAngle(const Vec& w);
|
|
|
|
/* @brief Set rotation matrix from an orthogonal basis given in world
|
|
coordinate system (WCS)
|
|
|
|
As R' is expressing the transformation from WCS to ICS (image
|
|
coordinate system), R is ICS to WCS and hereby represents the ICS
|
|
base vectors expressed in WCS. These are the *rows* of R because
|
|
the transformation is the scalar product.
|
|
|
|
Assume xxx,yyy,zzz are right-hand orthogonal (RHS), e.g. the orthogonal
|
|
image coordinate system (ICS) base vectors expressed in world
|
|
coordinates (WCS).
|
|
(Inverse doesn't make sense because base would be identity then).
|
|
Normal vectors of length 1 are computed.
|
|
|
|
@todo Warning if (xxx,yyy,zzz) are not an orthogonal basis!
|
|
|
|
@author jw 09/2003, added exception */
|
|
void SetFromOrthogonalBasis(const Vec &xxx, const Vec &yyy, const Vec &zzz);
|
|
|
|
/** @brief Set rotation matrix from two vectors from an orthonormal basis
|
|
@param xh represents the first base vector and is left unchanged
|
|
@param vy should be orthogonal to xh, it is orthogonalized otherwise
|
|
|
|
You can think of this routine as computing R from an image plane
|
|
given by two (usually orthogonal) base vectors.
|
|
If the given base vectors are not orthogonal, xh is kept and yv is
|
|
orthogonalized appropriately.
|
|
|
|
@author jw */
|
|
void SetFromHV(const Vec& xh, const Vec& vy);
|
|
|
|
/** @brief Create rotation matrix that rotates from dir0 to dir1
|
|
@param dir0 represents the first (reference) direction vector
|
|
@param dir1 represents the second (target) direction vector
|
|
@author cDc */
|
|
TRMatrixBase& SetFromDir2Dir(const Vec& dir0, const Vec& dir1);
|
|
|
|
/** @brief Calculates quaternion representation for this rotation matrix
|
|
@attention Scalar part of quaternion will always be non-negative
|
|
for sake of uniqueness of the resulting quaternion!
|
|
@author woelk 12/2003
|
|
@author esquivel 03/2011 (changed algorithm, bugfix) */
|
|
void GetQuaternion(Quat& q) const;
|
|
inline Quat GetQuaternion() const { Quat q; GetQuaternion(q); return q; }
|
|
|
|
/** @brief Set rotation matrix from a quaternion
|
|
@author grest 06/2003 */
|
|
void SetFromQuaternion(const Quat& q);
|
|
|
|
/** @brief Set rotation matrix from direction and up vector
|
|
@note This is openGL conform, similar to gluLookAt(), i.e. if
|
|
direction is (0,0,-1) and up is (0,1,0) the resulting matrix is identity.
|
|
@author grest 12/2005 */
|
|
void SetFromDirUpGL(const Vec& viewDir, const Vec& viewUp);
|
|
|
|
/** @brief Set rotation matrix from direction and up vector */
|
|
void SetFromDirUp(const Vec& viewDir, const Vec& viewUp);
|
|
|
|
/** @brief Set rotation matrix from an eye point and a look-at target point and the up vector */
|
|
void LookAt(const Vec& from, const Vec& to, const Vec& up);
|
|
|
|
// get parametrized rotation (axis-angle) from the rotation matrix
|
|
inline void SetRotationAxisAngle(const Vec& rot);
|
|
|
|
// modify the rotation matrix by the given parametrized delta rotation (axis-angle)
|
|
inline void Apply(const Vec& delta);
|
|
|
|
// set rotation matrix to the given parametrized rotation (axis-angle)
|
|
inline Vec GetRotationAxisAngle() const;
|
|
|
|
/** @brief Calculates angle and rotation axis representation for
|
|
this rotation matrix
|
|
@param angle Rotation angle is returned in radians
|
|
@author woelk 12/2003 */
|
|
void GetRotationAxisAngle(Vec& axis, TYPE& angle) const;
|
|
|
|
/** @brief Interface for axis component of GetRotationAxisAngle() */
|
|
Vec GetRotationAxis() const;
|
|
|
|
/** @brief Interface for angle component of GetRotationAxisAngle() */
|
|
TYPE GetRotationAngle() const;
|
|
|
|
/** @brief Get Euler angles for this rotation matrix in order XYZ
|
|
@attention Representation is not unique and has singularities at
|
|
+-pi/2. Assume for example (*this) = Rx * Ry * Rz, then we have
|
|
either Euler angles in order 1. x, 2. y, 3. z and moving axes
|
|
or in order 1. z, 2. y, 3. x and fixed axes.
|
|
@author woelk 01/2003 */
|
|
int GetRotationAnglesXYZ(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const;
|
|
|
|
/** @brief Get Euler angles for this rotation matrix in order XYZ
|
|
@see GetRotationAnglesXYZ(TYPE&, TYPE&, TYPE&) */
|
|
inline int GetRotationAnglesXYZ(Vec& r) const
|
|
{ return GetRotationAnglesXYZ(r[0], r[1], r[2]); }
|
|
|
|
/** @brief Get Euler angles for this rotation matrix in order YZX
|
|
@attention Representation is not unique and has singularities at
|
|
+-pi/2. Assume for example (*this) = Rz * Ry * Rx, then we have
|
|
either Euler angles in order 1. x, 2. y, 3. z and fixed axes
|
|
or in order 1. z, 2. y, 3. x and moving axes.
|
|
@author woelk 01 2003 */
|
|
int GetRotationAnglesZYX(TYPE& PhiX, TYPE& PhiY, TYPE& PhiZ) const;
|
|
|
|
/** @brief Get Euler angles for this rotation matrix in order ZYX
|
|
@see GetRotationAnglesZYX(TYPE&, TYPE&, TYPE&) */
|
|
inline int GetRotationAnglesZYX(Vec& r) const
|
|
{ return GetRotationAnglesZYX(r[0], r[1], r[2]); }
|
|
|
|
/** @brief Get Euler angles for this rotation matrix in order ZXY
|
|
@attention Representation is not unique and has singularities at
|
|
+-pi/2. Rotation order ZXY refers to rotation around point/vector!
|
|
@author haase 2007 */
|
|
int GetRotationAnglesZXY(TYPE& PhiZ, TYPE& PhiX, TYPE& PhiY) const;
|
|
|
|
/** @brief Get Euler angles for this rotation matrix in order ZXY
|
|
@see GetRotationAnglesZXY(TYPE&, TYPE&, TYPE&) */
|
|
inline int GetRotationAnglesZXY(Vec& r) const
|
|
{ return GetRotationAnglesZXY(r[2], r[0], r[1]); }
|
|
|
|
/** @brief Get Euler angles for this rotation matrix in order YXZ
|
|
@attention Representation is not unique and has singularities at
|
|
+-pi/2. Rotation order YXZ refers to rotation with moving axes!
|
|
@author haase 2007 */
|
|
int GetRotationAnglesYXZ(TYPE& PhiY, TYPE& PhiX, TYPE& PhiZ) const;
|
|
|
|
/** @brief Get Euler angles for this rotation matrix in order YXZ
|
|
@see GetRotationAnglesYXZ(TYPE&, TYPE&, TYPE&) */
|
|
inline int GetRotationAnglesYXZ(Vec& r) const
|
|
{ return GetRotationAnglesYXZ(r[1], r[0], r[2]); }
|
|
|
|
/** @brief Check that the matrix is a valid rotation matrix (orthogonal) */
|
|
inline bool IsValid() const {
|
|
// the trace should be three and the determinant should be one
|
|
#if 1
|
|
return (ISEQUAL((float)cv::determinant(*this), 1.f) && ISEQUAL((float)cv::trace((*this)*(*this).t()), 3.f));
|
|
#else
|
|
return (ISEQUAL((TYPE)cv::determinant(*this), TYPE(1)) && ISEQUAL((TYPE)cv::trace((*this)*(*this).t()), TYPE(3)));
|
|
#endif
|
|
}
|
|
|
|
/** @brief Check if this is a rotation matrix, i.e. if the determinant
|
|
is +1 and the columns are orthonormal
|
|
@param eps Numerical limit for constraint evaluation
|
|
@param verbose Show reason in case of failure */
|
|
bool Check(const TYPE eps = std::numeric_limits<TYPE>::epsilon(),
|
|
int verbose = 0) const;
|
|
|
|
/** @brief Enforce orthogonality constraint on rotation matrix and
|
|
sets determinant to +1 */
|
|
void EnforceOrthogonality();
|
|
}; // class
|
|
/*----------------------------------------------------------------*/
|
|
typedef TRMatrixBase<float> RMatrixBaseF;
|
|
typedef TRMatrixBase<double> RMatrixBaseD;
|
|
/*----------------------------------------------------------------*/
|
|
|
|
} // namespace SEACAVE
|
|
|
|
#include "Rotation.inl"
|
|
|
|
#endif // __SEACAVE_ROTATION_H__
|
|
|