1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. Eigen itself is part of the KDE project. 3 // 4 // Copyright (C) 2008 Gael Guennebaud <g.gael (at) free.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 // no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway 11 12 namespace Eigen { 13 14 /** \geometry_module \ingroup Geometry_Module 15 * 16 * \class AngleAxis 17 * 18 * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis 19 * 20 * \param _Scalar the scalar type, i.e., the type of the coefficients. 21 * 22 * The following two typedefs are provided for convenience: 23 * \li \c AngleAxisf for \c float 24 * \li \c AngleAxisd for \c double 25 * 26 * \addexample AngleAxisForEuler \label How to define a rotation from Euler-angles 27 * 28 * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily 29 * mimic Euler-angles. Here is an example: 30 * \include AngleAxis_mimic_euler.cpp 31 * Output: \verbinclude AngleAxis_mimic_euler.out 32 * 33 * \note This class is not aimed to be used to store a rotation transformation, 34 * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix) 35 * and transformation objects. 36 * 37 * \sa class Quaternion, class Transform, MatrixBase::UnitX() 38 */ 39 40 template<typename _Scalar> struct ei_traits<AngleAxis<_Scalar> > 41 { 42 typedef _Scalar Scalar; 43 }; 44 45 template<typename _Scalar> 46 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3> 47 { 48 typedef RotationBase<AngleAxis<_Scalar>,3> Base; 49 50 public: 51 52 using Base::operator*; 53 54 enum { Dim = 3 }; 55 /** the scalar type of the coefficients */ 56 typedef _Scalar Scalar; 57 typedef Matrix<Scalar,3,3> Matrix3; 58 typedef Matrix<Scalar,3,1> Vector3; 59 typedef Quaternion<Scalar> QuaternionType; 60 61 protected: 62 63 Vector3 m_axis; 64 Scalar m_angle; 65 66 public: 67 68 /** Default constructor without initialization. */ 69 AngleAxis() {} 70 /** Constructs and initialize the angle-axis rotation from an \a angle in radian 71 * and an \a axis which must be normalized. */ 72 template<typename Derived> 73 inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {} 74 /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */ 75 inline AngleAxis(const QuaternionType& q) { *this = q; } 76 /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */ 77 template<typename Derived> 78 inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; } 79 80 Scalar angle() const { return m_angle; } 81 Scalar& angle() { return m_angle; } 82 83 const Vector3& axis() const { return m_axis; } 84 Vector3& axis() { return m_axis; } 85 86 /** Concatenates two rotations */ 87 inline QuaternionType operator* (const AngleAxis& other) const 88 { return QuaternionType(*this) * QuaternionType(other); } 89 90 /** Concatenates two rotations */ 91 inline QuaternionType operator* (const QuaternionType& other) const 92 { return QuaternionType(*this) * other; } 93 94 /** Concatenates two rotations */ 95 friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) 96 { return a * QuaternionType(b); } 97 98 /** Concatenates two rotations */ 99 inline Matrix3 operator* (const Matrix3& other) const 100 { return toRotationMatrix() * other; } 101 102 /** Concatenates two rotations */ 103 inline friend Matrix3 operator* (const Matrix3& a, const AngleAxis& b) 104 { return a * b.toRotationMatrix(); } 105 106 /** Applies rotation to vector */ 107 inline Vector3 operator* (const Vector3& other) const 108 { return toRotationMatrix() * other; } 109 110 /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */ 111 AngleAxis inverse() const 112 { return AngleAxis(-m_angle, m_axis); } 113 114 AngleAxis& operator=(const QuaternionType& q); 115 template<typename Derived> 116 AngleAxis& operator=(const MatrixBase<Derived>& m); 117 118 template<typename Derived> 119 AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); 120 Matrix3 toRotationMatrix(void) const; 121 122 /** \returns \c *this with scalar type casted to \a NewScalarType 123 * 124 * Note that if \a NewScalarType is equal to the current scalar type of \c *this 125 * then this function smartly returns a const reference to \c *this. 126 */ 127 template<typename NewScalarType> 128 inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const 129 { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); } 130 131 /** Copy constructor with scalar type conversion */ 132 template<typename OtherScalarType> 133 inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) 134 { 135 m_axis = other.axis().template cast<Scalar>(); 136 m_angle = Scalar(other.angle()); 137 } 138 139 /** \returns \c true if \c *this is approximately equal to \a other, within the precision 140 * determined by \a prec. 141 * 142 * \sa MatrixBase::isApprox() */ 143 bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const 144 { return m_axis.isApprox(other.m_axis, prec) && ei_isApprox(m_angle,other.m_angle, prec); } 145 }; 146 147 /** \ingroup Geometry_Module 148 * single precision angle-axis type */ 149 typedef AngleAxis<float> AngleAxisf; 150 /** \ingroup Geometry_Module 151 * double precision angle-axis type */ 152 typedef AngleAxis<double> AngleAxisd; 153 154 /** Set \c *this from a quaternion. 155 * The axis is normalized. 156 */ 157 template<typename Scalar> 158 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q) 159 { 160 Scalar n2 = q.vec().squaredNorm(); 161 if (n2 < precision<Scalar>()*precision<Scalar>()) 162 { 163 m_angle = 0; 164 m_axis << 1, 0, 0; 165 } 166 else 167 { 168 m_angle = 2*std::acos(q.w()); 169 m_axis = q.vec() / ei_sqrt(n2); 170 } 171 return *this; 172 } 173 174 /** Set \c *this from a 3x3 rotation matrix \a mat. 175 */ 176 template<typename Scalar> 177 template<typename Derived> 178 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) 179 { 180 // Since a direct conversion would not be really faster, 181 // let's use the robust Quaternion implementation: 182 return *this = QuaternionType(mat); 183 } 184 185 /** Constructs and \returns an equivalent 3x3 rotation matrix. 186 */ 187 template<typename Scalar> 188 typename AngleAxis<Scalar>::Matrix3 189 AngleAxis<Scalar>::toRotationMatrix(void) const 190 { 191 Matrix3 res; 192 Vector3 sin_axis = ei_sin(m_angle) * m_axis; 193 Scalar c = ei_cos(m_angle); 194 Vector3 cos1_axis = (Scalar(1)-c) * m_axis; 195 196 Scalar tmp; 197 tmp = cos1_axis.x() * m_axis.y(); 198 res.coeffRef(0,1) = tmp - sin_axis.z(); 199 res.coeffRef(1,0) = tmp + sin_axis.z(); 200 201 tmp = cos1_axis.x() * m_axis.z(); 202 res.coeffRef(0,2) = tmp + sin_axis.y(); 203 res.coeffRef(2,0) = tmp - sin_axis.y(); 204 205 tmp = cos1_axis.y() * m_axis.z(); 206 res.coeffRef(1,2) = tmp - sin_axis.x(); 207 res.coeffRef(2,1) = tmp + sin_axis.x(); 208 209 res.diagonal() = (cos1_axis.cwise() * m_axis).cwise() + c; 210 211 return res; 212 } 213 214 } // end namespace Eigen 215
