![]() |
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_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