1c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// This file is part of Eigen, a lightweight C++ template library 27faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// for linear algebra. 3c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// 4c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath// Copyright (C) 2008 Gael Guennebaud <g.gael@free.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// no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway 11c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 12c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathnamespace Eigen { 13c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 14c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \geometry_module \ingroup Geometry_Module 15c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 16c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \class Rotation2D 17c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 18c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \brief Represents a rotation/orientation in a 2 dimensional space. 19c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 20c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \param _Scalar the scalar type, i.e., the type of the coefficients 21c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 22c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * This class is equivalent to a single scalar representing a counter clock wise rotation 23c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * as a single angle in radian. It provides some additional features such as the automatic 24c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * conversion from/to a 2x2 rotation matrix. Moreover this class aims to provide a similar 25c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * interface to Quaternion in order to facilitate the writing of generic algorithms 26c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * dealing with rotations. 27c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 28c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa class Quaternion, class Transform 29c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 30c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar> struct ei_traits<Rotation2D<_Scalar> > 31c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 32c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 33c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 34c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 35c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename _Scalar> 36c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathclass Rotation2D : public RotationBase<Rotation2D<_Scalar>,2> 37c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 38c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef RotationBase<Rotation2D<_Scalar>,2> Base; 39c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 40c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathpublic: 41c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 42c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath using Base::operator*; 43c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 44c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath enum { Dim = 2 }; 45c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** the scalar type of the coefficients */ 46c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef _Scalar Scalar; 47c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,2,1> Vector2; 48c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath typedef Matrix<Scalar,2,2> Matrix2; 49c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 50c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathprotected: 51c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 52c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar m_angle; 53c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 54c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathpublic: 55c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 56c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Construct a 2D counter clock wise rotation from the angle \a a in radian. */ 57c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Rotation2D(Scalar a) : m_angle(a) {} 58c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 59c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the rotation angle */ 60c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar angle() const { return m_angle; } 61c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 62c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns a read-write reference to the rotation angle */ 63c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Scalar& angle() { return m_angle; } 64c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 65c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the inverse rotation */ 66c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Rotation2D inverse() const { return -m_angle; } 67c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 68c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Concatenates two rotations */ 69c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Rotation2D operator*(const Rotation2D& other) const 70c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return m_angle + other.m_angle; } 71c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 72c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Concatenates two rotations */ 73c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Rotation2D& operator*=(const Rotation2D& other) 74c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return m_angle += other.m_angle; return *this; } 75c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 76c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Applies the rotation to a 2D vector */ 77c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Vector2 operator* (const Vector2& vec) const 78c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return toRotationMatrix() * vec; } 79c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 80c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename Derived> 81c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m); 82c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Matrix2 toRotationMatrix(void) const; 83c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 84c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns the spherical interpolation between \c *this and \a other using 85c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * parameter \a t. It is in fact equivalent to a linear interpolation. 86c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 87c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline Rotation2D slerp(Scalar t, const Rotation2D& other) const 88c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return m_angle * (1-t) + other.angle() * t; } 89c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 90c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns \c *this with scalar type casted to \a NewScalarType 91c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 92c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * Note that if \a NewScalarType is equal to the current scalar type of \c *this 93c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * then this function smartly returns a const reference to \c *this. 94c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 95c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename NewScalarType> 96c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type cast() const 97c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type(*this); } 98c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 99c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** Copy constructor with scalar type conversion */ 100c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath template<typename OtherScalarType> 101c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other) 102c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { 103c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m_angle = Scalar(other.angle()); 104c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath } 105c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 106c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath /** \returns \c true if \c *this is approximately equal to \a other, within the precision 107c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * determined by \a prec. 108c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * 109c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * \sa MatrixBase::isApprox() */ 110c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath bool isApprox(const Rotation2D& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const 111c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath { return ei_isApprox(m_angle,other.m_angle, prec); } 112c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath}; 113c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 114c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 115c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * single precision 2D rotation type */ 116c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef Rotation2D<float> Rotation2Df; 117c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** \ingroup Geometry_Module 118c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * double precision 2D rotation type */ 119c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypedef Rotation2D<double> Rotation2Dd; 120c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 121c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Set \c *this from a 2x2 rotation matrix \a mat. 122c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * In other words, this function extract the rotation angle 123c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath * from the rotation matrix. 124c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 125c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar> 126c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Derived> 127c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathRotation2D<Scalar>& Rotation2D<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) 128c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 129c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE) 130c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath m_angle = ei_atan2(mat.coeff(1,0), mat.coeff(0,0)); 131c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return *this; 132c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 133c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 134c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath/** Constructs and \returns an equivalent 2x2 rotation matrix. 135c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath */ 136c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtemplate<typename Scalar> 137c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamathtypename Rotation2D<Scalar>::Matrix2 138c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan KamathRotation2D<Scalar>::toRotationMatrix(void) const 139c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath{ 140c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar sinA = ei_sin(m_angle); 141c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath Scalar cosA = ei_cos(m_angle); 142c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath return (Matrix2() << cosA, -sinA, sinA, cosA).finished(); 143c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} 144c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath 145c981c48f5bc9aefeffc0bcb0cc3934c2fae179ddNarayan Kamath} // end namespace Eigen 146