1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud (at) inria.fr> 5 // 6 // This Source Code Form is subject to the terms of the Mozilla 7 // Public License v. 2.0. If a copy of the MPL was not distributed 8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 10 #ifndef EIGEN_ANGLEAXIS_H 11 #define EIGEN_ANGLEAXIS_H 12 13 namespace Eigen { 14 15 /** \geometry_module \ingroup Geometry_Module 16 * 17 * \class AngleAxis 18 * 19 * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis 20 * 21 * \param _Scalar the scalar type, i.e., the type of the coefficients. 22 * 23 * \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized. 24 * 25 * The following two typedefs are provided for convenience: 26 * \li \c AngleAxisf for \c float 27 * \li \c AngleAxisd for \c double 28 * 29 * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily 30 * mimic Euler-angles. Here is an example: 31 * \include AngleAxis_mimic_euler.cpp 32 * Output: \verbinclude AngleAxis_mimic_euler.out 33 * 34 * \note This class is not aimed to be used to store a rotation transformation, 35 * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix) 36 * and transformation objects. 37 * 38 * \sa class Quaternion, class Transform, MatrixBase::UnitX() 39 */ 40 41 namespace internal { 42 template<typename _Scalar> struct traits<AngleAxis<_Scalar> > 43 { 44 typedef _Scalar Scalar; 45 }; 46 } 47 48 template<typename _Scalar> 49 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3> 50 { 51 typedef RotationBase<AngleAxis<_Scalar>,3> Base; 52 53 public: 54 55 using Base::operator*; 56 57 enum { Dim = 3 }; 58 /** the scalar type of the coefficients */ 59 typedef _Scalar Scalar; 60 typedef Matrix<Scalar,3,3> Matrix3; 61 typedef Matrix<Scalar,3,1> Vector3; 62 typedef Quaternion<Scalar> QuaternionType; 63 64 protected: 65 66 Vector3 m_axis; 67 Scalar m_angle; 68 69 public: 70 71 /** Default constructor without initialization. */ 72 EIGEN_DEVICE_FUNC AngleAxis() {} 73 /** Constructs and initialize the angle-axis rotation from an \a angle in radian 74 * and an \a axis which \b must \b be \b normalized. 75 * 76 * \warning If the \a axis vector is not normalized, then the angle-axis object 77 * represents an invalid rotation. */ 78 template<typename Derived> 79 EIGEN_DEVICE_FUNC 80 inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {} 81 /** Constructs and initialize the angle-axis rotation from a quaternion \a q. 82 * This function implicitly normalizes the quaternion \a q. 83 */ 84 template<typename QuatDerived> 85 EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; } 86 /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */ 87 template<typename Derived> 88 EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; } 89 90 /** \returns the value of the rotation angle in radian */ 91 EIGEN_DEVICE_FUNC Scalar angle() const { return m_angle; } 92 /** \returns a read-write reference to the stored angle in radian */ 93 EIGEN_DEVICE_FUNC Scalar& angle() { return m_angle; } 94 95 /** \returns the rotation axis */ 96 EIGEN_DEVICE_FUNC const Vector3& axis() const { return m_axis; } 97 /** \returns a read-write reference to the stored rotation axis. 98 * 99 * \warning The rotation axis must remain a \b unit vector. 100 */ 101 EIGEN_DEVICE_FUNC Vector3& axis() { return m_axis; } 102 103 /** Concatenates two rotations */ 104 EIGEN_DEVICE_FUNC inline QuaternionType operator* (const AngleAxis& other) const 105 { return QuaternionType(*this) * QuaternionType(other); } 106 107 /** Concatenates two rotations */ 108 EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& other) const 109 { return QuaternionType(*this) * other; } 110 111 /** Concatenates two rotations */ 112 friend EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) 113 { return a * QuaternionType(b); } 114 115 /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */ 116 EIGEN_DEVICE_FUNC AngleAxis inverse() const 117 { return AngleAxis(-m_angle, m_axis); } 118 119 template<class QuatDerived> 120 EIGEN_DEVICE_FUNC AngleAxis& operator=(const QuaternionBase<QuatDerived>& q); 121 template<typename Derived> 122 EIGEN_DEVICE_FUNC AngleAxis& operator=(const MatrixBase<Derived>& m); 123 124 template<typename Derived> 125 EIGEN_DEVICE_FUNC AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); 126 EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const; 127 128 /** \returns \c *this with scalar type casted to \a NewScalarType 129 * 130 * Note that if \a NewScalarType is equal to the current scalar type of \c *this 131 * then this function smartly returns a const reference to \c *this. 132 */ 133 template<typename NewScalarType> 134 EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const 135 { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); } 136 137 /** Copy constructor with scalar type conversion */ 138 template<typename OtherScalarType> 139 EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) 140 { 141 m_axis = other.axis().template cast<Scalar>(); 142 m_angle = Scalar(other.angle()); 143 } 144 145 EIGEN_DEVICE_FUNC static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); } 146 147 /** \returns \c true if \c *this is approximately equal to \a other, within the precision 148 * determined by \a prec. 149 * 150 * \sa MatrixBase::isApprox() */ 151 EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const 152 { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); } 153 }; 154 155 /** \ingroup Geometry_Module 156 * single precision angle-axis type */ 157 typedef AngleAxis<float> AngleAxisf; 158 /** \ingroup Geometry_Module 159 * double precision angle-axis type */ 160 typedef AngleAxis<double> AngleAxisd; 161 162 /** Set \c *this from a \b unit quaternion. 163 * 164 * The resulting axis is normalized, and the computed angle is in the [0,pi] range. 165 * 166 * This function implicitly normalizes the quaternion \a q. 167 */ 168 template<typename Scalar> 169 template<typename QuatDerived> 170 EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q) 171 { 172 EIGEN_USING_STD_MATH(atan2) 173 EIGEN_USING_STD_MATH(abs) 174 Scalar n = q.vec().norm(); 175 if(n<NumTraits<Scalar>::epsilon()) 176 n = q.vec().stableNorm(); 177 178 if (n != Scalar(0)) 179 { 180 m_angle = Scalar(2)*atan2(n, abs(q.w())); 181 if(q.w() < 0) 182 n = -n; 183 m_axis = q.vec() / n; 184 } 185 else 186 { 187 m_angle = Scalar(0); 188 m_axis << Scalar(1), Scalar(0), Scalar(0); 189 } 190 return *this; 191 } 192 193 /** Set \c *this from a 3x3 rotation matrix \a mat. 194 */ 195 template<typename Scalar> 196 template<typename Derived> 197 EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) 198 { 199 // Since a direct conversion would not be really faster, 200 // let's use the robust Quaternion implementation: 201 return *this = QuaternionType(mat); 202 } 203 204 /** 205 * \brief Sets \c *this from a 3x3 rotation matrix. 206 **/ 207 template<typename Scalar> 208 template<typename Derived> 209 EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) 210 { 211 return *this = QuaternionType(mat); 212 } 213 214 /** Constructs and \returns an equivalent 3x3 rotation matrix. 215 */ 216 template<typename Scalar> 217 typename AngleAxis<Scalar>::Matrix3 218 EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const 219 { 220 EIGEN_USING_STD_MATH(sin) 221 EIGEN_USING_STD_MATH(cos) 222 Matrix3 res; 223 Vector3 sin_axis = sin(m_angle) * m_axis; 224 Scalar c = cos(m_angle); 225 Vector3 cos1_axis = (Scalar(1)-c) * m_axis; 226 227 Scalar tmp; 228 tmp = cos1_axis.x() * m_axis.y(); 229 res.coeffRef(0,1) = tmp - sin_axis.z(); 230 res.coeffRef(1,0) = tmp + sin_axis.z(); 231 232 tmp = cos1_axis.x() * m_axis.z(); 233 res.coeffRef(0,2) = tmp + sin_axis.y(); 234 res.coeffRef(2,0) = tmp - sin_axis.y(); 235 236 tmp = cos1_axis.y() * m_axis.z(); 237 res.coeffRef(1,2) = tmp - sin_axis.x(); 238 res.coeffRef(2,1) = tmp + sin_axis.x(); 239 240 res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c; 241 242 return res; 243 } 244 245 } // end namespace Eigen 246 247 #endif // EIGEN_ANGLEAXIS_H 248
