AngleAxis.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.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 // no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway
00011 
00012 namespace Eigen { 
00013 
00040 template<typename _Scalar> struct ei_traits<AngleAxis<_Scalar> >
00041 {
00042   typedef _Scalar Scalar;
00043 };
00044 
00045 template<typename _Scalar>
00046 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
00047 {
00048   typedef RotationBase<AngleAxis<_Scalar>,3> Base;
00049 
00050 public:
00051 
00052   using Base::operator*;
00053 
00054   enum { Dim = 3 };
00056   typedef _Scalar Scalar;
00057   typedef Matrix<Scalar,3,3> Matrix3;
00058   typedef Matrix<Scalar,3,1> Vector3;
00059   typedef Quaternion<Scalar> QuaternionType;
00060 
00061 protected:
00062 
00063   Vector3 m_axis;
00064   Scalar m_angle;
00065 
00066 public:
00067 
00069   AngleAxis() {}
00072   template<typename Derived>
00073   inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
00075   inline AngleAxis(const QuaternionType& q) { *this = q; }
00077   template<typename Derived>
00078   inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
00079 
00080   Scalar angle() const { return m_angle; }
00081   Scalar& angle() { return m_angle; }
00082 
00083   const Vector3& axis() const { return m_axis; }
00084   Vector3& axis() { return m_axis; }
00085 
00087   inline QuaternionType operator* (const AngleAxis& other) const
00088   { return QuaternionType(*this) * QuaternionType(other); }
00089 
00091   inline QuaternionType operator* (const QuaternionType& other) const
00092   { return QuaternionType(*this) * other; }
00093 
00095   friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
00096   { return a * QuaternionType(b); }
00097 
00099   inline Matrix3 operator* (const Matrix3& other) const
00100   { return toRotationMatrix() * other; }
00101 
00103   inline friend Matrix3 operator* (const Matrix3& a, const AngleAxis& b)
00104   { return a * b.toRotationMatrix(); }
00105 
00107   inline Vector3 operator* (const Vector3& other) const
00108   { return toRotationMatrix() * other; }
00109 
00111   AngleAxis inverse() const
00112   { return AngleAxis(-m_angle, m_axis); }
00113 
00114   AngleAxis& operator=(const QuaternionType& q);
00115   template<typename Derived>
00116   AngleAxis& operator=(const MatrixBase<Derived>& m);
00117 
00118   template<typename Derived>
00119   AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
00120   Matrix3 toRotationMatrix(void) const;
00121 
00127   template<typename NewScalarType>
00128   inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
00129   { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
00130 
00132   template<typename OtherScalarType>
00133   inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
00134   {
00135     m_axis = other.axis().template cast<Scalar>();
00136     m_angle = Scalar(other.angle());
00137   }
00138 
00143   bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
00144   { return m_axis.isApprox(other.m_axis, prec) && ei_isApprox(m_angle,other.m_angle, prec); }
00145 };
00146 
00149 typedef AngleAxis<float> AngleAxisf;
00152 typedef AngleAxis<double> AngleAxisd;
00153 
00157 template<typename Scalar>
00158 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
00159 {
00160   Scalar n2 = q.vec().squaredNorm();
00161   if (n2 < precision<Scalar>()*precision<Scalar>())
00162   {
00163     m_angle = 0;
00164     m_axis << 1, 0, 0;
00165   }
00166   else
00167   {
00168     m_angle = 2*std::acos(q.w());
00169     m_axis = q.vec() / ei_sqrt(n2);
00170   }
00171   return *this;
00172 }
00173 
00176 template<typename Scalar>
00177 template<typename Derived>
00178 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
00179 {
00180   // Since a direct conversion would not be really faster,
00181   // let's use the robust Quaternion implementation:
00182   return *this = QuaternionType(mat);
00183 }
00184 
00187 template<typename Scalar>
00188 typename AngleAxis<Scalar>::Matrix3
00189 AngleAxis<Scalar>::toRotationMatrix(void) const
00190 {
00191   Matrix3 res;
00192   Vector3 sin_axis  = ei_sin(m_angle) * m_axis;
00193   Scalar c = ei_cos(m_angle);
00194   Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
00195 
00196   Scalar tmp;
00197   tmp = cos1_axis.x() * m_axis.y();
00198   res.coeffRef(0,1) = tmp - sin_axis.z();
00199   res.coeffRef(1,0) = tmp + sin_axis.z();
00200 
00201   tmp = cos1_axis.x() * m_axis.z();
00202   res.coeffRef(0,2) = tmp + sin_axis.y();
00203   res.coeffRef(2,0) = tmp - sin_axis.y();
00204 
00205   tmp = cos1_axis.y() * m_axis.z();
00206   res.coeffRef(1,2) = tmp - sin_axis.x();
00207   res.coeffRef(2,1) = tmp + sin_axis.x();
00208 
00209   res.diagonal() = (cos1_axis.cwise() * m_axis).cwise() + c;
00210 
00211   return res;
00212 }
00213 
00214 } // end namespace Eigen


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Thu Aug 27 2015 11:57:48