Jaesung Oh / Eigen

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?

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_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