Eigne Matrix Class Library
Dependents: MPC_current_control HydraulicControlBoard_SW AHRS Test_ekf ... more
src/Geometry/RotationBase.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?
User | Revision | Line number | New 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_ROTATIONBASE_H |
ykuroda | 0:13a5d365ba16 | 11 | #define EIGEN_ROTATIONBASE_H |
ykuroda | 0:13a5d365ba16 | 12 | |
ykuroda | 0:13a5d365ba16 | 13 | namespace Eigen { |
ykuroda | 0:13a5d365ba16 | 14 | |
ykuroda | 0:13a5d365ba16 | 15 | // forward declaration |
ykuroda | 0:13a5d365ba16 | 16 | namespace internal { |
ykuroda | 0:13a5d365ba16 | 17 | template<typename RotationDerived, typename MatrixType, bool IsVector=MatrixType::IsVectorAtCompileTime> |
ykuroda | 0:13a5d365ba16 | 18 | struct rotation_base_generic_product_selector; |
ykuroda | 0:13a5d365ba16 | 19 | } |
ykuroda | 0:13a5d365ba16 | 20 | |
ykuroda | 0:13a5d365ba16 | 21 | /** \class RotationBase |
ykuroda | 0:13a5d365ba16 | 22 | * |
ykuroda | 0:13a5d365ba16 | 23 | * \brief Common base class for compact rotation representations |
ykuroda | 0:13a5d365ba16 | 24 | * |
ykuroda | 0:13a5d365ba16 | 25 | * \param Derived is the derived type, i.e., a rotation type |
ykuroda | 0:13a5d365ba16 | 26 | * \param _Dim the dimension of the space |
ykuroda | 0:13a5d365ba16 | 27 | */ |
ykuroda | 0:13a5d365ba16 | 28 | template<typename Derived, int _Dim> |
ykuroda | 0:13a5d365ba16 | 29 | class RotationBase |
ykuroda | 0:13a5d365ba16 | 30 | { |
ykuroda | 0:13a5d365ba16 | 31 | public: |
ykuroda | 0:13a5d365ba16 | 32 | enum { Dim = _Dim }; |
ykuroda | 0:13a5d365ba16 | 33 | /** the scalar type of the coefficients */ |
ykuroda | 0:13a5d365ba16 | 34 | typedef typename internal::traits<Derived>::Scalar Scalar; |
ykuroda | 0:13a5d365ba16 | 35 | |
ykuroda | 0:13a5d365ba16 | 36 | /** corresponding linear transformation matrix type */ |
ykuroda | 0:13a5d365ba16 | 37 | typedef Matrix<Scalar,Dim,Dim> RotationMatrixType; |
ykuroda | 0:13a5d365ba16 | 38 | typedef Matrix<Scalar,Dim,1> VectorType; |
ykuroda | 0:13a5d365ba16 | 39 | |
ykuroda | 0:13a5d365ba16 | 40 | public: |
ykuroda | 0:13a5d365ba16 | 41 | inline const Derived& derived() const { return *static_cast<const Derived*>(this); } |
ykuroda | 0:13a5d365ba16 | 42 | inline Derived& derived() { return *static_cast<Derived*>(this); } |
ykuroda | 0:13a5d365ba16 | 43 | |
ykuroda | 0:13a5d365ba16 | 44 | /** \returns an equivalent rotation matrix */ |
ykuroda | 0:13a5d365ba16 | 45 | inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); } |
ykuroda | 0:13a5d365ba16 | 46 | |
ykuroda | 0:13a5d365ba16 | 47 | /** \returns an equivalent rotation matrix |
ykuroda | 0:13a5d365ba16 | 48 | * This function is added to be conform with the Transform class' naming scheme. |
ykuroda | 0:13a5d365ba16 | 49 | */ |
ykuroda | 0:13a5d365ba16 | 50 | inline RotationMatrixType matrix() const { return derived().toRotationMatrix(); } |
ykuroda | 0:13a5d365ba16 | 51 | |
ykuroda | 0:13a5d365ba16 | 52 | /** \returns the inverse rotation */ |
ykuroda | 0:13a5d365ba16 | 53 | inline Derived inverse() const { return derived().inverse(); } |
ykuroda | 0:13a5d365ba16 | 54 | |
ykuroda | 0:13a5d365ba16 | 55 | /** \returns the concatenation of the rotation \c *this with a translation \a t */ |
ykuroda | 0:13a5d365ba16 | 56 | inline Transform<Scalar,Dim,Isometry> operator*(const Translation<Scalar,Dim>& t) const |
ykuroda | 0:13a5d365ba16 | 57 | { return Transform<Scalar,Dim,Isometry>(*this) * t; } |
ykuroda | 0:13a5d365ba16 | 58 | |
ykuroda | 0:13a5d365ba16 | 59 | /** \returns the concatenation of the rotation \c *this with a uniform scaling \a s */ |
ykuroda | 0:13a5d365ba16 | 60 | inline RotationMatrixType operator*(const UniformScaling<Scalar>& s) const |
ykuroda | 0:13a5d365ba16 | 61 | { return toRotationMatrix() * s.factor(); } |
ykuroda | 0:13a5d365ba16 | 62 | |
ykuroda | 0:13a5d365ba16 | 63 | /** \returns the concatenation of the rotation \c *this with a generic expression \a e |
ykuroda | 0:13a5d365ba16 | 64 | * \a e can be: |
ykuroda | 0:13a5d365ba16 | 65 | * - a DimxDim linear transformation matrix |
ykuroda | 0:13a5d365ba16 | 66 | * - a DimxDim diagonal matrix (axis aligned scaling) |
ykuroda | 0:13a5d365ba16 | 67 | * - a vector of size Dim |
ykuroda | 0:13a5d365ba16 | 68 | */ |
ykuroda | 0:13a5d365ba16 | 69 | template<typename OtherDerived> |
ykuroda | 0:13a5d365ba16 | 70 | EIGEN_STRONG_INLINE typename internal::rotation_base_generic_product_selector<Derived,OtherDerived,OtherDerived::IsVectorAtCompileTime>::ReturnType |
ykuroda | 0:13a5d365ba16 | 71 | operator*(const EigenBase<OtherDerived>& e) const |
ykuroda | 0:13a5d365ba16 | 72 | { return internal::rotation_base_generic_product_selector<Derived,OtherDerived>::run(derived(), e.derived()); } |
ykuroda | 0:13a5d365ba16 | 73 | |
ykuroda | 0:13a5d365ba16 | 74 | /** \returns the concatenation of a linear transformation \a l with the rotation \a r */ |
ykuroda | 0:13a5d365ba16 | 75 | template<typename OtherDerived> friend |
ykuroda | 0:13a5d365ba16 | 76 | inline RotationMatrixType operator*(const EigenBase<OtherDerived>& l, const Derived& r) |
ykuroda | 0:13a5d365ba16 | 77 | { return l.derived() * r.toRotationMatrix(); } |
ykuroda | 0:13a5d365ba16 | 78 | |
ykuroda | 0:13a5d365ba16 | 79 | /** \returns the concatenation of a scaling \a l with the rotation \a r */ |
ykuroda | 0:13a5d365ba16 | 80 | friend inline Transform<Scalar,Dim,Affine> operator*(const DiagonalMatrix<Scalar,Dim>& l, const Derived& r) |
ykuroda | 0:13a5d365ba16 | 81 | { |
ykuroda | 0:13a5d365ba16 | 82 | Transform<Scalar,Dim,Affine> res(r); |
ykuroda | 0:13a5d365ba16 | 83 | res.linear().applyOnTheLeft(l); |
ykuroda | 0:13a5d365ba16 | 84 | return res; |
ykuroda | 0:13a5d365ba16 | 85 | } |
ykuroda | 0:13a5d365ba16 | 86 | |
ykuroda | 0:13a5d365ba16 | 87 | /** \returns the concatenation of the rotation \c *this with a transformation \a t */ |
ykuroda | 0:13a5d365ba16 | 88 | template<int Mode, int Options> |
ykuroda | 0:13a5d365ba16 | 89 | inline Transform<Scalar,Dim,Mode> operator*(const Transform<Scalar,Dim,Mode,Options>& t) const |
ykuroda | 0:13a5d365ba16 | 90 | { return toRotationMatrix() * t; } |
ykuroda | 0:13a5d365ba16 | 91 | |
ykuroda | 0:13a5d365ba16 | 92 | template<typename OtherVectorType> |
ykuroda | 0:13a5d365ba16 | 93 | inline VectorType _transformVector(const OtherVectorType& v) const |
ykuroda | 0:13a5d365ba16 | 94 | { return toRotationMatrix() * v; } |
ykuroda | 0:13a5d365ba16 | 95 | }; |
ykuroda | 0:13a5d365ba16 | 96 | |
ykuroda | 0:13a5d365ba16 | 97 | namespace internal { |
ykuroda | 0:13a5d365ba16 | 98 | |
ykuroda | 0:13a5d365ba16 | 99 | // implementation of the generic product rotation * matrix |
ykuroda | 0:13a5d365ba16 | 100 | template<typename RotationDerived, typename MatrixType> |
ykuroda | 0:13a5d365ba16 | 101 | struct rotation_base_generic_product_selector<RotationDerived,MatrixType,false> |
ykuroda | 0:13a5d365ba16 | 102 | { |
ykuroda | 0:13a5d365ba16 | 103 | enum { Dim = RotationDerived::Dim }; |
ykuroda | 0:13a5d365ba16 | 104 | typedef Matrix<typename RotationDerived::Scalar,Dim,Dim> ReturnType; |
ykuroda | 0:13a5d365ba16 | 105 | static inline ReturnType run(const RotationDerived& r, const MatrixType& m) |
ykuroda | 0:13a5d365ba16 | 106 | { return r.toRotationMatrix() * m; } |
ykuroda | 0:13a5d365ba16 | 107 | }; |
ykuroda | 0:13a5d365ba16 | 108 | |
ykuroda | 0:13a5d365ba16 | 109 | template<typename RotationDerived, typename Scalar, int Dim, int MaxDim> |
ykuroda | 0:13a5d365ba16 | 110 | struct rotation_base_generic_product_selector< RotationDerived, DiagonalMatrix<Scalar,Dim,MaxDim>, false > |
ykuroda | 0:13a5d365ba16 | 111 | { |
ykuroda | 0:13a5d365ba16 | 112 | typedef Transform<Scalar,Dim,Affine> ReturnType; |
ykuroda | 0:13a5d365ba16 | 113 | static inline ReturnType run(const RotationDerived& r, const DiagonalMatrix<Scalar,Dim,MaxDim>& m) |
ykuroda | 0:13a5d365ba16 | 114 | { |
ykuroda | 0:13a5d365ba16 | 115 | ReturnType res(r); |
ykuroda | 0:13a5d365ba16 | 116 | res.linear() *= m; |
ykuroda | 0:13a5d365ba16 | 117 | return res; |
ykuroda | 0:13a5d365ba16 | 118 | } |
ykuroda | 0:13a5d365ba16 | 119 | }; |
ykuroda | 0:13a5d365ba16 | 120 | |
ykuroda | 0:13a5d365ba16 | 121 | template<typename RotationDerived,typename OtherVectorType> |
ykuroda | 0:13a5d365ba16 | 122 | struct rotation_base_generic_product_selector<RotationDerived,OtherVectorType,true> |
ykuroda | 0:13a5d365ba16 | 123 | { |
ykuroda | 0:13a5d365ba16 | 124 | enum { Dim = RotationDerived::Dim }; |
ykuroda | 0:13a5d365ba16 | 125 | typedef Matrix<typename RotationDerived::Scalar,Dim,1> ReturnType; |
ykuroda | 0:13a5d365ba16 | 126 | static EIGEN_STRONG_INLINE ReturnType run(const RotationDerived& r, const OtherVectorType& v) |
ykuroda | 0:13a5d365ba16 | 127 | { |
ykuroda | 0:13a5d365ba16 | 128 | return r._transformVector(v); |
ykuroda | 0:13a5d365ba16 | 129 | } |
ykuroda | 0:13a5d365ba16 | 130 | }; |
ykuroda | 0:13a5d365ba16 | 131 | |
ykuroda | 0:13a5d365ba16 | 132 | } // end namespace internal |
ykuroda | 0:13a5d365ba16 | 133 | |
ykuroda | 0:13a5d365ba16 | 134 | /** \geometry_module |
ykuroda | 0:13a5d365ba16 | 135 | * |
ykuroda | 0:13a5d365ba16 | 136 | * \brief Constructs a Dim x Dim rotation matrix from the rotation \a r |
ykuroda | 0:13a5d365ba16 | 137 | */ |
ykuroda | 0:13a5d365ba16 | 138 | template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols> |
ykuroda | 0:13a5d365ba16 | 139 | template<typename OtherDerived> |
ykuroda | 0:13a5d365ba16 | 140 | Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols> |
ykuroda | 0:13a5d365ba16 | 141 | ::Matrix(const RotationBase<OtherDerived,ColsAtCompileTime>& r) |
ykuroda | 0:13a5d365ba16 | 142 | { |
ykuroda | 0:13a5d365ba16 | 143 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim)) |
ykuroda | 0:13a5d365ba16 | 144 | *this = r.toRotationMatrix(); |
ykuroda | 0:13a5d365ba16 | 145 | } |
ykuroda | 0:13a5d365ba16 | 146 | |
ykuroda | 0:13a5d365ba16 | 147 | /** \geometry_module |
ykuroda | 0:13a5d365ba16 | 148 | * |
ykuroda | 0:13a5d365ba16 | 149 | * \brief Set a Dim x Dim rotation matrix from the rotation \a r |
ykuroda | 0:13a5d365ba16 | 150 | */ |
ykuroda | 0:13a5d365ba16 | 151 | template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols> |
ykuroda | 0:13a5d365ba16 | 152 | template<typename OtherDerived> |
ykuroda | 0:13a5d365ba16 | 153 | Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>& |
ykuroda | 0:13a5d365ba16 | 154 | Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols> |
ykuroda | 0:13a5d365ba16 | 155 | ::operator=(const RotationBase<OtherDerived,ColsAtCompileTime>& r) |
ykuroda | 0:13a5d365ba16 | 156 | { |
ykuroda | 0:13a5d365ba16 | 157 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,int(OtherDerived::Dim),int(OtherDerived::Dim)) |
ykuroda | 0:13a5d365ba16 | 158 | return *this = r.toRotationMatrix(); |
ykuroda | 0:13a5d365ba16 | 159 | } |
ykuroda | 0:13a5d365ba16 | 160 | |
ykuroda | 0:13a5d365ba16 | 161 | namespace internal { |
ykuroda | 0:13a5d365ba16 | 162 | |
ykuroda | 0:13a5d365ba16 | 163 | /** \internal |
ykuroda | 0:13a5d365ba16 | 164 | * |
ykuroda | 0:13a5d365ba16 | 165 | * Helper function to return an arbitrary rotation object to a rotation matrix. |
ykuroda | 0:13a5d365ba16 | 166 | * |
ykuroda | 0:13a5d365ba16 | 167 | * \param Scalar the numeric type of the matrix coefficients |
ykuroda | 0:13a5d365ba16 | 168 | * \param Dim the dimension of the current space |
ykuroda | 0:13a5d365ba16 | 169 | * |
ykuroda | 0:13a5d365ba16 | 170 | * It returns a Dim x Dim fixed size matrix. |
ykuroda | 0:13a5d365ba16 | 171 | * |
ykuroda | 0:13a5d365ba16 | 172 | * Default specializations are provided for: |
ykuroda | 0:13a5d365ba16 | 173 | * - any scalar type (2D), |
ykuroda | 0:13a5d365ba16 | 174 | * - any matrix expression, |
ykuroda | 0:13a5d365ba16 | 175 | * - any type based on RotationBase (e.g., Quaternion, AngleAxis, Rotation2D) |
ykuroda | 0:13a5d365ba16 | 176 | * |
ykuroda | 0:13a5d365ba16 | 177 | * Currently toRotationMatrix is only used by Transform. |
ykuroda | 0:13a5d365ba16 | 178 | * |
ykuroda | 0:13a5d365ba16 | 179 | * \sa class Transform, class Rotation2D, class Quaternion, class AngleAxis |
ykuroda | 0:13a5d365ba16 | 180 | */ |
ykuroda | 0:13a5d365ba16 | 181 | template<typename Scalar, int Dim> |
ykuroda | 0:13a5d365ba16 | 182 | static inline Matrix<Scalar,2,2> toRotationMatrix(const Scalar& s) |
ykuroda | 0:13a5d365ba16 | 183 | { |
ykuroda | 0:13a5d365ba16 | 184 | EIGEN_STATIC_ASSERT(Dim==2,YOU_MADE_A_PROGRAMMING_MISTAKE) |
ykuroda | 0:13a5d365ba16 | 185 | return Rotation2D<Scalar>(s).toRotationMatrix(); |
ykuroda | 0:13a5d365ba16 | 186 | } |
ykuroda | 0:13a5d365ba16 | 187 | |
ykuroda | 0:13a5d365ba16 | 188 | template<typename Scalar, int Dim, typename OtherDerived> |
ykuroda | 0:13a5d365ba16 | 189 | static inline Matrix<Scalar,Dim,Dim> toRotationMatrix(const RotationBase<OtherDerived,Dim>& r) |
ykuroda | 0:13a5d365ba16 | 190 | { |
ykuroda | 0:13a5d365ba16 | 191 | return r.toRotationMatrix(); |
ykuroda | 0:13a5d365ba16 | 192 | } |
ykuroda | 0:13a5d365ba16 | 193 | |
ykuroda | 0:13a5d365ba16 | 194 | template<typename Scalar, int Dim, typename OtherDerived> |
ykuroda | 0:13a5d365ba16 | 195 | static inline const MatrixBase<OtherDerived>& toRotationMatrix(const MatrixBase<OtherDerived>& mat) |
ykuroda | 0:13a5d365ba16 | 196 | { |
ykuroda | 0:13a5d365ba16 | 197 | EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime==Dim && OtherDerived::ColsAtCompileTime==Dim, |
ykuroda | 0:13a5d365ba16 | 198 | YOU_MADE_A_PROGRAMMING_MISTAKE) |
ykuroda | 0:13a5d365ba16 | 199 | return mat; |
ykuroda | 0:13a5d365ba16 | 200 | } |
ykuroda | 0:13a5d365ba16 | 201 | |
ykuroda | 0:13a5d365ba16 | 202 | } // end namespace internal |
ykuroda | 0:13a5d365ba16 | 203 | |
ykuroda | 0:13a5d365ba16 | 204 | } // end namespace Eigen |
ykuroda | 0:13a5d365ba16 | 205 | |
ykuroda | 0:13a5d365ba16 | 206 | #endif // EIGEN_ROTATIONBASE_H |