Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
AngleAxis.h
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> 00005 // 00006 // This Source Code Form is subject to the terms of the Mozilla 00007 // Public License v. 2.0. If a copy of the MPL was not distributed 00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00009 00010 #ifndef EIGEN_ANGLEAXIS_H 00011 #define EIGEN_ANGLEAXIS_H 00012 00013 namespace Eigen { 00014 00015 /** \geometry_module \ingroup Geometry_Module 00016 * 00017 * \class AngleAxis 00018 * 00019 * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis 00020 * 00021 * \param _Scalar the scalar type, i.e., the type of the coefficients. 00022 * 00023 * \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized. 00024 * 00025 * The following two typedefs are provided for convenience: 00026 * \li \c AngleAxisf for \c float 00027 * \li \c AngleAxisd for \c double 00028 * 00029 * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily 00030 * mimic Euler-angles. Here is an example: 00031 * \include AngleAxis_mimic_euler.cpp 00032 * Output: \verbinclude AngleAxis_mimic_euler.out 00033 * 00034 * \note This class is not aimed to be used to store a rotation transformation, 00035 * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix) 00036 * and transformation objects. 00037 * 00038 * \sa class Quaternion, class Transform, MatrixBase::UnitX() 00039 */ 00040 00041 namespace internal { 00042 template<typename _Scalar> struct traits<AngleAxis<_Scalar> > 00043 { 00044 typedef _Scalar Scalar; 00045 }; 00046 } 00047 00048 template<typename _Scalar> 00049 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3> 00050 { 00051 typedef RotationBase<AngleAxis<_Scalar>,3> Base ; 00052 00053 public: 00054 00055 using Base::operator*; 00056 00057 enum { Dim = 3 }; 00058 /** the scalar type of the coefficients */ 00059 typedef _Scalar Scalar; 00060 typedef Matrix<Scalar,3,3> Matrix3; 00061 typedef Matrix<Scalar,3,1> Vector3 ; 00062 typedef Quaternion<Scalar> QuaternionType ; 00063 00064 protected: 00065 00066 Vector3 m_axis; 00067 Scalar m_angle; 00068 00069 public: 00070 00071 /** Default constructor without initialization. */ 00072 AngleAxis() {} 00073 /** Constructs and initialize the angle-axis rotation from an \a angle in radian 00074 * and an \a axis which \b must \b be \b normalized. 00075 * 00076 * \warning If the \a axis vector is not normalized, then the angle-axis object 00077 * represents an invalid rotation. */ 00078 template<typename Derived> 00079 inline AngleAxis(const Scalar& angle , const MatrixBase<Derived>& axis ) : m_axis(axis), m_angle(angle) {} 00080 /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */ 00081 template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived> & q) { *this = q; } 00082 /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */ 00083 template<typename Derived> 00084 inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; } 00085 00086 /** \returns the value of the rotation angle in radian */ 00087 Scalar angle () const { return m_angle; } 00088 /** \returns a read-write reference to the stored angle in radian */ 00089 Scalar& angle () { return m_angle; } 00090 00091 /** \returns the rotation axis */ 00092 const Vector3 & axis () const { return m_axis; } 00093 /** \returns a read-write reference to the stored rotation axis. 00094 * 00095 * \warning The rotation axis must remain a \b unit vector. 00096 */ 00097 Vector3 & axis () { return m_axis; } 00098 00099 /** Concatenates two rotations */ 00100 inline QuaternionType operator* (const AngleAxis & other) const 00101 { return QuaternionType (*this) * QuaternionType (other); } 00102 00103 /** Concatenates two rotations */ 00104 inline QuaternionType operator* (const QuaternionType & other) const 00105 { return QuaternionType (*this) * other; } 00106 00107 /** Concatenates two rotations */ 00108 friend inline QuaternionType operator* (const QuaternionType & a, const AngleAxis & b) 00109 { return a * QuaternionType (b); } 00110 00111 /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */ 00112 AngleAxis inverse () const 00113 { return AngleAxis(-m_angle, m_axis); } 00114 00115 template<class QuatDerived> 00116 AngleAxis & operator=(const QuaternionBase<QuatDerived> & q); 00117 template<typename Derived> 00118 AngleAxis & operator=(const MatrixBase<Derived>& m); 00119 00120 template<typename Derived> 00121 AngleAxis & fromRotationMatrix(const MatrixBase<Derived>& m); 00122 Matrix3 toRotationMatrix(void) const; 00123 00124 /** \returns \c *this with scalar type casted to \a NewScalarType 00125 * 00126 * Note that if \a NewScalarType is equal to the current scalar type of \c *this 00127 * then this function smartly returns a const reference to \c *this. 00128 */ 00129 template<typename NewScalarType> 00130 inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast () const 00131 { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); } 00132 00133 /** Copy constructor with scalar type conversion */ 00134 template<typename OtherScalarType> 00135 inline explicit AngleAxis(const AngleAxis<OtherScalarType> & other) 00136 { 00137 m_axis = other.axis ().template cast<Scalar>(); 00138 m_angle = Scalar(other.angle ()); 00139 } 00140 00141 static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); } 00142 00143 /** \returns \c true if \c *this is approximately equal to \a other, within the precision 00144 * determined by \a prec. 00145 * 00146 * \sa MatrixBase::isApprox() */ 00147 bool isApprox (const AngleAxis & other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const 00148 { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox (m_angle,other.m_angle, prec); } 00149 }; 00150 00151 /** \ingroup Geometry_Module 00152 * single precision angle-axis type */ 00153 typedef AngleAxis<float> AngleAxisf; 00154 /** \ingroup Geometry_Module 00155 * double precision angle-axis type */ 00156 typedef AngleAxis<double> AngleAxisd; 00157 00158 /** Set \c *this from a \b unit quaternion. 00159 * The axis is normalized. 00160 * 00161 * \warning As any other method dealing with quaternion, if the input quaternion 00162 * is not normalized then the result is undefined. 00163 */ 00164 template<typename Scalar> 00165 template<typename QuatDerived> 00166 AngleAxis<Scalar> & AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived> & q) 00167 { 00168 using std::acos; 00169 using std::min; 00170 using std::max; 00171 using std::sqrt; 00172 Scalar n2 = q.vec ().squaredNorm(); 00173 if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision()) 00174 { 00175 m_angle = Scalar(0); 00176 m_axis << Scalar(1), Scalar(0), Scalar(0); 00177 } 00178 else 00179 { 00180 m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w ()),Scalar(1))); 00181 m_axis = q.vec () / sqrt(n2); 00182 } 00183 return *this; 00184 } 00185 00186 /** Set \c *this from a 3x3 rotation matrix \a mat. 00187 */ 00188 template<typename Scalar> 00189 template<typename Derived> 00190 AngleAxis<Scalar> & AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) 00191 { 00192 // Since a direct conversion would not be really faster, 00193 // let's use the robust Quaternion implementation: 00194 return *this = QuaternionType (mat); 00195 } 00196 00197 /** 00198 * \brief Sets \c *this from a 3x3 rotation matrix. 00199 **/ 00200 template<typename Scalar> 00201 template<typename Derived> 00202 AngleAxis<Scalar> & AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) 00203 { 00204 return *this = QuaternionType (mat); 00205 } 00206 00207 /** Constructs and \returns an equivalent 3x3 rotation matrix. 00208 */ 00209 template<typename Scalar> 00210 typename AngleAxis<Scalar>::Matrix3 00211 AngleAxis<Scalar>::toRotationMatrix(void) const 00212 { 00213 using std::sin; 00214 using std::cos; 00215 Matrix3 res; 00216 Vector3 sin_axis = sin(m_angle) * m_axis; 00217 Scalar c = cos(m_angle); 00218 Vector3 cos1_axis = (Scalar(1)-c) * m_axis; 00219 00220 Scalar tmp; 00221 tmp = cos1_axis.x() * m_axis.y(); 00222 res.coeffRef(0,1) = tmp - sin_axis.z(); 00223 res.coeffRef(1,0) = tmp + sin_axis.z(); 00224 00225 tmp = cos1_axis.x() * m_axis.z(); 00226 res.coeffRef(0,2) = tmp + sin_axis.y(); 00227 res.coeffRef(2,0) = tmp - sin_axis.y(); 00228 00229 tmp = cos1_axis.y() * m_axis.z(); 00230 res.coeffRef(1,2) = tmp - sin_axis.x(); 00231 res.coeffRef(2,1) = tmp + sin_axis.x(); 00232 00233 res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c; 00234 00235 return res; 00236 } 00237 00238 } // end namespace Eigen 00239 00240 #endif // EIGEN_ANGLEAXIS_H
Generated on Tue Jul 12 2022 17:46:49 by 1.7.2