Eigen  3.3.3
Rotation2D.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_ROTATION2D_H
00011 #define EIGEN_ROTATION2D_H
00012 
00013 namespace Eigen { 
00014 
00032 namespace internal {
00033 
00034 template<typename _Scalar> struct traits<Rotation2D<_Scalar> >
00035 {
00036   typedef _Scalar Scalar;
00037 };
00038 } // end namespace internal
00039 
00040 template<typename _Scalar>
00041 class Rotation2D : public RotationBase<Rotation2D<_Scalar>,2>
00042 {
00043   typedef RotationBase<Rotation2D<_Scalar>,2> Base;
00044 
00045 public:
00046 
00047   using Base::operator*;
00048 
00049   enum { Dim = 2 };
00051   typedef _Scalar Scalar;
00052   typedef Matrix<Scalar,2,1> Vector2;
00053   typedef Matrix<Scalar,2,2> Matrix2;
00054 
00055 protected:
00056 
00057   Scalar m_angle;
00058 
00059 public:
00060 
00062   EIGEN_DEVICE_FUNC explicit inline Rotation2D(const Scalar& a) : m_angle(a) {}
00063   
00065   EIGEN_DEVICE_FUNC Rotation2D() {}
00066 
00071   template<typename Derived>
00072   EIGEN_DEVICE_FUNC explicit Rotation2D(const MatrixBase<Derived>& m)
00073   {
00074     fromRotationMatrix(m.derived());
00075   }
00076 
00078   EIGEN_DEVICE_FUNC inline Scalar angle() const { return m_angle; }
00079 
00081   EIGEN_DEVICE_FUNC inline Scalar& angle() { return m_angle; }
00082   
00084   EIGEN_DEVICE_FUNC inline Scalar smallestPositiveAngle() const {
00085     Scalar tmp = numext::fmod(m_angle,Scalar(2*EIGEN_PI));
00086     return tmp<Scalar(0) ? tmp + Scalar(2*EIGEN_PI) : tmp;
00087   }
00088   
00090   EIGEN_DEVICE_FUNC inline Scalar smallestAngle() const {
00091     Scalar tmp = numext::fmod(m_angle,Scalar(2*EIGEN_PI));
00092     if(tmp>Scalar(EIGEN_PI))       tmp -= Scalar(2*EIGEN_PI);
00093     else if(tmp<-Scalar(EIGEN_PI)) tmp += Scalar(2*EIGEN_PI);
00094     return tmp;
00095   }
00096 
00098   EIGEN_DEVICE_FUNC inline Rotation2D inverse() const { return Rotation2D(-m_angle); }
00099 
00101   EIGEN_DEVICE_FUNC inline Rotation2D operator*(const Rotation2D& other) const
00102   { return Rotation2D(m_angle + other.m_angle); }
00103 
00105   EIGEN_DEVICE_FUNC inline Rotation2D& operator*=(const Rotation2D& other)
00106   { m_angle += other.m_angle; return *this; }
00107 
00109   EIGEN_DEVICE_FUNC Vector2 operator* (const Vector2& vec) const
00110   { return toRotationMatrix() * vec; }
00111   
00112   template<typename Derived>
00113   EIGEN_DEVICE_FUNC Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
00114   EIGEN_DEVICE_FUNC Matrix2 toRotationMatrix() const;
00115 
00123   template<typename Derived>
00124   EIGEN_DEVICE_FUNC Rotation2D& operator=(const  MatrixBase<Derived>& m)
00125   { return fromRotationMatrix(m.derived()); }
00126 
00130   EIGEN_DEVICE_FUNC inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const
00131   {
00132     Scalar dist = Rotation2D(other.m_angle-m_angle).smallestAngle();
00133     return Rotation2D(m_angle + dist*t);
00134   }
00135 
00141   template<typename NewScalarType>
00142   EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type cast() const
00143   { return typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type(*this); }
00144 
00146   template<typename OtherScalarType>
00147   EIGEN_DEVICE_FUNC inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other)
00148   {
00149     m_angle = Scalar(other.angle());
00150   }
00151 
00152   EIGEN_DEVICE_FUNC static inline Rotation2D Identity() { return Rotation2D(0); }
00153 
00158   EIGEN_DEVICE_FUNC bool isApprox(const Rotation2D& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
00159   { return internal::isApprox(m_angle,other.m_angle, prec); }
00160   
00161 };
00162 
00165 typedef Rotation2D<float> Rotation2Df;
00168 typedef Rotation2D<double> Rotation2Dd;
00169 
00174 template<typename Scalar>
00175 template<typename Derived>
00176 EIGEN_DEVICE_FUNC Rotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
00177 {
00178   EIGEN_USING_STD_MATH(atan2)
00179   EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE)
00180   m_angle = atan2(mat.coeff(1,0), mat.coeff(0,0));
00181   return *this;
00182 }
00183 
00186 template<typename Scalar>
00187 typename Rotation2D<Scalar>::Matrix2
00188 EIGEN_DEVICE_FUNC Rotation2D<Scalar>::toRotationMatrix(void) const
00189 {
00190   EIGEN_USING_STD_MATH(sin)
00191   EIGEN_USING_STD_MATH(cos)
00192   Scalar sinA = sin(m_angle);
00193   Scalar cosA = cos(m_angle);
00194   return (Matrix2() << cosA, -sinA, sinA, cosA).finished();
00195 }
00196 
00197 } // end namespace Eigen
00198 
00199 #endif // EIGEN_ROTATION2D_H
 All Classes Functions Variables Typedefs Enumerations Enumerator Friends