$treeview $search $mathjax
Eigen
3.2.5
$projectbrief
|
$projectbrief
|
$searchbox |
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 AngleAxis() {} 00078 template<typename Derived> 00079 inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {} 00081 template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; } 00083 template<typename Derived> 00084 inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; } 00085 00086 Scalar angle() const { return m_angle; } 00087 Scalar& angle() { return m_angle; } 00088 00089 const Vector3& axis() const { return m_axis; } 00090 Vector3& axis() { return m_axis; } 00091 00093 inline QuaternionType operator* (const AngleAxis& other) const 00094 { return QuaternionType(*this) * QuaternionType(other); } 00095 00097 inline QuaternionType operator* (const QuaternionType& other) const 00098 { return QuaternionType(*this) * other; } 00099 00101 friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b) 00102 { return a * QuaternionType(b); } 00103 00105 AngleAxis inverse() const 00106 { return AngleAxis(-m_angle, m_axis); } 00107 00108 template<class QuatDerived> 00109 AngleAxis& operator=(const QuaternionBase<QuatDerived>& q); 00110 template<typename Derived> 00111 AngleAxis& operator=(const MatrixBase<Derived>& m); 00112 00113 template<typename Derived> 00114 AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m); 00115 Matrix3 toRotationMatrix(void) const; 00116 00122 template<typename NewScalarType> 00123 inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const 00124 { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); } 00125 00127 template<typename OtherScalarType> 00128 inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) 00129 { 00130 m_axis = other.axis().template cast<Scalar>(); 00131 m_angle = Scalar(other.angle()); 00132 } 00133 00134 static inline const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); } 00135 00140 bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const 00141 { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); } 00142 }; 00143 00146 typedef AngleAxis<float> AngleAxisf; 00149 typedef AngleAxis<double> AngleAxisd; 00150 00157 template<typename Scalar> 00158 template<typename QuatDerived> 00159 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q) 00160 { 00161 using std::acos; 00162 using std::min; 00163 using std::max; 00164 using std::sqrt; 00165 Scalar n2 = q.vec().squaredNorm(); 00166 if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision()) 00167 { 00168 m_angle = 0; 00169 m_axis << 1, 0, 0; 00170 } 00171 else 00172 { 00173 m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w()),Scalar(1))); 00174 m_axis = q.vec() / sqrt(n2); 00175 } 00176 return *this; 00177 } 00178 00181 template<typename Scalar> 00182 template<typename Derived> 00183 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) 00184 { 00185 // Since a direct conversion would not be really faster, 00186 // let's use the robust Quaternion implementation: 00187 return *this = QuaternionType(mat); 00188 } 00189 00193 template<typename Scalar> 00194 template<typename Derived> 00195 AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) 00196 { 00197 return *this = QuaternionType(mat); 00198 } 00199 00202 template<typename Scalar> 00203 typename AngleAxis<Scalar>::Matrix3 00204 AngleAxis<Scalar>::toRotationMatrix(void) const 00205 { 00206 using std::sin; 00207 using std::cos; 00208 Matrix3 res; 00209 Vector3 sin_axis = sin(m_angle) * m_axis; 00210 Scalar c = cos(m_angle); 00211 Vector3 cos1_axis = (Scalar(1)-c) * m_axis; 00212 00213 Scalar tmp; 00214 tmp = cos1_axis.x() * m_axis.y(); 00215 res.coeffRef(0,1) = tmp - sin_axis.z(); 00216 res.coeffRef(1,0) = tmp + sin_axis.z(); 00217 00218 tmp = cos1_axis.x() * m_axis.z(); 00219 res.coeffRef(0,2) = tmp + sin_axis.y(); 00220 res.coeffRef(2,0) = tmp - sin_axis.y(); 00221 00222 tmp = cos1_axis.y() * m_axis.z(); 00223 res.coeffRef(1,2) = tmp - sin_axis.x(); 00224 res.coeffRef(2,1) = tmp + sin_axis.x(); 00225 00226 res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c; 00227 00228 return res; 00229 } 00230 00231 } // end namespace Eigen 00232 00233 #endif // EIGEN_ANGLEAXIS_H