Jaesung Oh / Eigen

Eigne Matrix Class Library

Dependents:   MPC_current_control HydraulicControlBoard_SW AHRS Test_ekf ... more

src/Geometry/Rotation2D.h@0:13a5d365ba16, 2016-10-13 (annotated)

Committer:
ykuroda
Date:
Thu Oct 13 04:07:23 2016 +0000
Revision:
0:13a5d365ba16
First commint, Eigne Matrix Class Library

Who changed what in which revision?

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