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