Eigen  3.4.90 (git rev 67eeba6e720c5745abc77ae6c92ce0a44aa7b7ae)
Rotation2D.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
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 #ifndef EIGEN_ROTATION2D_H
11 #define EIGEN_ROTATION2D_H
12 
13 #include "./InternalHeaderCheck.h"
14 
15 namespace Eigen {
16 
34 namespace internal {
35 
36 template<typename Scalar_> struct traits<Rotation2D<Scalar_> >
37 {
38  typedef Scalar_ Scalar;
39 };
40 } // end namespace internal
41 
42 template<typename Scalar_>
43 class Rotation2D : public RotationBase<Rotation2D<Scalar_>,2>
44 {
46 
47 public:
48 
49  using Base::operator*;
50 
51  enum { Dim = 2 };
53  typedef Scalar_ Scalar;
56 
57 protected:
58 
59  Scalar m_angle;
60 
61 public:
62 
64  EIGEN_DEVICE_FUNC explicit inline Rotation2D(const Scalar& a) : m_angle(a) {}
65 
67  EIGEN_DEVICE_FUNC Rotation2D() {}
68 
73  template<typename Derived>
74  EIGEN_DEVICE_FUNC explicit Rotation2D(const MatrixBase<Derived>& m)
75  {
76  fromRotationMatrix(m.derived());
77  }
78 
80  EIGEN_DEVICE_FUNC inline Scalar angle() const { return m_angle; }
81 
83  EIGEN_DEVICE_FUNC inline Scalar& angle() { return m_angle; }
84 
86  EIGEN_DEVICE_FUNC inline Scalar smallestPositiveAngle() const {
87  Scalar tmp = numext::fmod(m_angle,Scalar(2*EIGEN_PI));
88  return tmp<Scalar(0) ? tmp + Scalar(2*EIGEN_PI) : tmp;
89  }
90 
92  EIGEN_DEVICE_FUNC inline Scalar smallestAngle() const {
93  Scalar tmp = numext::fmod(m_angle,Scalar(2*EIGEN_PI));
94  if(tmp>Scalar(EIGEN_PI)) tmp -= Scalar(2*EIGEN_PI);
95  else if(tmp<-Scalar(EIGEN_PI)) tmp += Scalar(2*EIGEN_PI);
96  return tmp;
97  }
98 
100  EIGEN_DEVICE_FUNC inline Rotation2D inverse() const { return Rotation2D(-m_angle); }
101 
103  EIGEN_DEVICE_FUNC inline Rotation2D operator*(const Rotation2D& other) const
104  { return Rotation2D(m_angle + other.m_angle); }
105 
107  EIGEN_DEVICE_FUNC inline Rotation2D& operator*=(const Rotation2D& other)
108  { m_angle += other.m_angle; return *this; }
109 
111  EIGEN_DEVICE_FUNC Vector2 operator* (const Vector2& vec) const
112  { return toRotationMatrix() * vec; }
113 
114  template<typename Derived>
115  EIGEN_DEVICE_FUNC Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
116  EIGEN_DEVICE_FUNC Matrix2 toRotationMatrix() const;
117 
125  template<typename Derived>
126  EIGEN_DEVICE_FUNC Rotation2D& operator=(const MatrixBase<Derived>& m)
127  { return fromRotationMatrix(m.derived()); }
128 
132  EIGEN_DEVICE_FUNC inline Rotation2D slerp(const Scalar& t, const Rotation2D& other) const
133  {
134  Scalar dist = Rotation2D(other.m_angle-m_angle).smallestAngle();
135  return Rotation2D(m_angle + dist*t);
136  }
137 
143  template<typename NewScalarType>
144  EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type cast() const
145  { return typename internal::cast_return_type<Rotation2D,Rotation2D<NewScalarType> >::type(*this); }
146 
148  template<typename OtherScalarType>
149  EIGEN_DEVICE_FUNC inline explicit Rotation2D(const Rotation2D<OtherScalarType>& other)
150  {
151  m_angle = Scalar(other.angle());
152  }
153 
154  EIGEN_DEVICE_FUNC static inline Rotation2D Identity() { return Rotation2D(0); }
155 
160  EIGEN_DEVICE_FUNC bool isApprox(const Rotation2D& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
161  { return internal::isApprox(m_angle,other.m_angle, prec); }
162 
163 };
164 
171 
176 template<typename Scalar>
177 template<typename Derived>
179 {
180  EIGEN_USING_STD(atan2)
181  EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime==2 && Derived::ColsAtCompileTime==2,YOU_MADE_A_PROGRAMMING_MISTAKE)
182  m_angle = atan2(mat.coeff(1,0), mat.coeff(0,0));
183  return *this;
184 }
185 
188 template<typename Scalar>
190 EIGEN_DEVICE_FUNC Rotation2D<Scalar>::toRotationMatrix(void) const
191 {
192  EIGEN_USING_STD(sin)
193  EIGEN_USING_STD(cos)
194  Scalar sinA = sin(m_angle);
195  Scalar cosA = cos(m_angle);
196  return (Matrix2() << cosA, -sinA, sinA, cosA).finished();
197 }
198 
199 } // end namespace Eigen
200 
201 #endif // EIGEN_ROTATION2D_H
Derived & derived()
Definition: EigenBase.h:48
CoeffReturnType coeff(Index row, Index col) const
Definition: DenseCoeffsBase.h:99
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:52
The matrix class, also used for vectors and row-vectors.
Definition: Matrix.h:182
Represents a rotation/orientation in a 2 dimensional space.
Definition: Rotation2D.h:44
Rotation2D inverse() const
Definition: Rotation2D.h:100
Scalar & angle()
Definition: Rotation2D.h:83
Scalar smallestPositiveAngle() const
Definition: Rotation2D.h:86
Scalar smallestAngle() const
Definition: Rotation2D.h:92
Rotation2D(const Scalar &a)
Definition: Rotation2D.h:64
internal::cast_return_type< Rotation2D, Rotation2D< NewScalarType > >::type cast() const
Definition: Rotation2D.h:144
Scalar angle() const
Definition: Rotation2D.h:80
Rotation2D & operator*=(const Rotation2D &other)
Definition: Rotation2D.h:107
Matrix2 toRotationMatrix() const
Definition: Rotation2D.h:190
Rotation2D operator*(const Rotation2D &other) const
Definition: Rotation2D.h:103
Rotation2D(const Rotation2D< OtherScalarType > &other)
Definition: Rotation2D.h:149
Rotation2D()
Definition: Rotation2D.h:67
Rotation2D & operator=(const MatrixBase< Derived > &m)
Definition: Rotation2D.h:126
Rotation2D slerp(const Scalar &t, const Rotation2D &other) const
Definition: Rotation2D.h:132
bool isApprox(const Rotation2D &other, const typename NumTraits< Scalar >::Real &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: Rotation2D.h:160
Scalar_ Scalar
Definition: Rotation2D.h:53
Rotation2D(const MatrixBase< Derived > &m)
Definition: Rotation2D.h:74
Common base class for compact rotation representations.
Definition: RotationBase.h:32
Rotation2D< float > Rotation2Df
Definition: Rotation2D.h:167
Rotation2D< double > Rotation2Dd
Definition: Rotation2D.h:170
Matrix< Type, 2, 2 > Matrix2
[c++11] 2×2 matrix of type Type.
Definition: Matrix.h:533
Namespace containing all symbols from the Eigen library.
Definition: Core:139
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_cos_op< typename Derived::Scalar >, const Derived > cos(const Eigen::ArrayBase< Derived > &x)
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sin_op< typename Derived::Scalar >, const Derived > sin(const Eigen::ArrayBase< Derived > &x)
Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
Definition: NumTraits.h:231