1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 2c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// for linear algebra. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr> 5c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 6c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This Source Code Form is subject to the terms of the Mozilla 7c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Public License v. 2.0. If a copy of the MPL was not distributed 8c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 10c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#ifndef EIGEN_ROTATION2D_H 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#define EIGEN_ROTATION2D_H 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace Eigen { 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \geometry_module \ingroup Geometry_Module 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \class Rotation2D 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Represents a rotation/orientation in a 2 dimensional space. 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param _Scalar the scalar type, i.e., the type of the coefficients 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * This class is equivalent to a single scalar representing a counter clock wise rotation 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * as a single angle in radian. It provides some additional features such as the automatic 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * conversion from/to a 2x2 rotation matrix. Moreover this class aims to provide a similar 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * interface to Quaternion in order to facilitate the writing of generic algorithms 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * dealing with rotations. 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa class Quaternion, class Transform 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace internal { 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar> struct traits<Rotation2D<_Scalar> > 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace internal 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar> 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathclass Rotation2D : public RotationBase<Rotation2D<_Scalar>,2> 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef RotationBase<Rotation2D<_Scalar>,2> Base; 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathpublic: 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using Base::operator*; 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { Dim = 2 }; 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** the scalar type of the coefficients */ 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,2,1> Vector2; 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,2,2> Matrix2; 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathprotected: 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar m_angle; 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathpublic: 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ 627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez inline Rotation2D(const Scalar& a) : m_angle(a) {} 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the rotation angle */ 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar angle() const { return m_angle; } 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a read-write reference to the rotation angle */ 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar& angle() { return m_angle; } 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the inverse rotation */ 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Rotation2D inverse() const { return -m_angle; } 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Concatenates two rotations */ 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Rotation2D operator*(const Rotation2D& other) const 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return m_angle + other.m_angle; } 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Concatenates two rotations */ 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Rotation2D& operator*=(const Rotation2D& other) 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { m_angle += other.m_angle; return *this; } 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Applies the rotation to a 2D vector */ 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector2 operator* (const Vector2& vec) const 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return toRotationMatrix() * vec; } 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Derived> 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m); 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix2 toRotationMatrix(void) const; 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the spherical interpolation between \c *this and \a other using 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * parameter \a t. It is in fact equivalent to a linear interpolation. 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return m_angle * (1-t) + other.angle() * t; } 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns \c *this with scalar type casted to \a NewScalarType 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Note that if \a NewScalarType is equal to the current scalar type of \c *this 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * then this function smartly returns a const reference to \c *this. 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename NewScalarType> 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type cast() const 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type(*this); } 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Copy constructor with scalar type conversion */ 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename OtherScalarType> 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other) 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m_angle = Scalar(other.angle()); 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath static inline Rotation2D Identity() { return Rotation2D(0); } 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns \c true if \c *this is approximately equal to \a other, within the precision 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * determined by \a prec. 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa MatrixBase::isApprox() */ 1177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez bool isApprox(const Rotation2D& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return internal::isApprox(m_angle,other.m_angle, prec); } 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * single precision 2D rotation type */ 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef Rotation2D<float> Rotation2Df; 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * double precision 2D rotation type */ 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef Rotation2D<double> Rotation2Dd; 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Set \c *this from a 2x2 rotation matrix \a mat. 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * In other words, this function extract the rotation angle 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * from the rotation matrix. 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar> 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Derived> 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathRotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 1367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::atan2; 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE) 1387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_angle = atan2(mat.coeff(1,0), mat.coeff(0,0)); 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return *this; 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Constructs and \returns an equivalent 2x2 rotation matrix. 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar> 145c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypename Rotation2D<Scalar>::Matrix2 146c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathRotation2D<Scalar>::toRotationMatrix(void) const 147c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 1487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::sin; 1497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez using std::cos; 1507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar sinA = sin(m_angle); 1517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Scalar cosA = cos(m_angle); 152c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); 153c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 154c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 155c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 156c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 157c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath#endif // EIGEN_ROTATION2D_H 158