![]() |
Eigen
3.3.3
|
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 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 }; 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 00072 EIGEN_DEVICE_FUNC AngleAxis() {} 00078 template<typename Derived> 00079 EIGEN_DEVICE_FUNC 00080 inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {} 00084 template<typename QuatDerived> 00085 EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; } 00087 template<typename Derived> 00088 EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; } 00089 00091 EIGEN_DEVICE_FUNC Scalar angle() const { return m_angle; } 00093 EIGEN_DEVICE_FUNC Scalar& angle() { return m_angle; } 00094 00096 EIGEN_DEVICE_FUNC const Vector3& axis() const { return m_axis; } 00101 EIGEN_DEVICE_FUNC Vector3& axis() { return m_axis; } 00102 00104 EIGEN_DEVICE_FUNC inline QuaternionType operator* (const AngleAxis& other) const 00105 { return QuaternionType(*this) * QuaternionType(other); } 00106 00108 EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& other) const 00109 { return QuaternionType(*this) * other; } 00110 00112 friend EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) 00113 { return a * QuaternionType(b); } 00114 00116 EIGEN_DEVICE_FUNC AngleAxis inverse() const 00117 { return AngleAxis(-m_angle, m_axis); } 00118 00119 template<class QuatDerived> 00120 EIGEN_DEVICE_FUNC AngleAxis& operator=(const QuaternionBase<QuatDerived>& q); 00121 template<typename Derived> 00122 EIGEN_DEVICE_FUNC AngleAxis& operator=(const MatrixBase<Derived>& m); 00123 00124 template<typename Derived> 00125 EIGEN_DEVICE_FUNC AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); 00126 EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const; 00127 00133 template<typename NewScalarType> 00134 EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const 00135 { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); } 00136 00138 template<typename OtherScalarType> 00139 EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) 00140 { 00141 m_axis = other.axis().template cast<Scalar>(); 00142 m_angle = Scalar(other.angle()); 00143 } 00144 00145 EIGEN_DEVICE_FUNC static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); } 00146 00151 EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const 00152 { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); } 00153 }; 00154 00157 typedef AngleAxis<float> AngleAxisf; 00160 typedef AngleAxis<double> AngleAxisd; 00161 00168 template<typename Scalar> 00169 template<typename QuatDerived> 00170 EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q) 00171 { 00172 EIGEN_USING_STD_MATH(atan2) 00173 EIGEN_USING_STD_MATH(abs) 00174 Scalar n = q.vec().norm(); 00175 if(n<NumTraits<Scalar>::epsilon()) 00176 n = q.vec().stableNorm(); 00177 00178 if (n != Scalar(0)) 00179 { 00180 m_angle = Scalar(2)*atan2(n, abs(q.w())); 00181 if(q.w() < 0) 00182 n = -n; 00183 m_axis = q.vec() / n; 00184 } 00185 else 00186 { 00187 m_angle = Scalar(0); 00188 m_axis << Scalar(1), Scalar(0), Scalar(0); 00189 } 00190 return *this; 00191 } 00192 00195 template<typename Scalar> 00196 template<typename Derived> 00197 EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) 00198 { 00199 // Since a direct conversion would not be really faster, 00200 // let's use the robust Quaternion implementation: 00201 return *this = QuaternionType(mat); 00202 } 00203 00207 template<typename Scalar> 00208 template<typename Derived> 00209 EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) 00210 { 00211 return *this = QuaternionType(mat); 00212 } 00213 00216 template<typename Scalar> 00217 typename AngleAxis<Scalar>::Matrix3 00218 EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const 00219 { 00220 EIGEN_USING_STD_MATH(sin) 00221 EIGEN_USING_STD_MATH(cos) 00222 Matrix3 res; 00223 Vector3 sin_axis = sin(m_angle) * m_axis; 00224 Scalar c = cos(m_angle); 00225 Vector3 cos1_axis = (Scalar(1)-c) * m_axis; 00226 00227 Scalar tmp; 00228 tmp = cos1_axis.x() * m_axis.y(); 00229 res.coeffRef(0,1) = tmp - sin_axis.z(); 00230 res.coeffRef(1,0) = tmp + sin_axis.z(); 00231 00232 tmp = cos1_axis.x() * m_axis.z(); 00233 res.coeffRef(0,2) = tmp + sin_axis.y(); 00234 res.coeffRef(2,0) = tmp - sin_axis.y(); 00235 00236 tmp = cos1_axis.y() * m_axis.z(); 00237 res.coeffRef(1,2) = tmp - sin_axis.x(); 00238 res.coeffRef(2,1) = tmp + sin_axis.x(); 00239 00240 res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c; 00241 00242 return res; 00243 } 00244 00245 } // end namespace Eigen 00246 00247 #endif // EIGEN_ANGLEAXIS_H