Jaesung Oh / Eigen

Eigne Matrix Class Library

Dependents:   MPC_current_control HydraulicControlBoard_SW AHRS Test_ekf ... more

src/Geometry/AngleAxis.h@1:3b8049da21b8, 2019-09-24 (annotated)

Committer:
jsoh91
Date:
Tue Sep 24 00:18:23 2019 +0000
Revision:
1:3b8049da21b8
Parent:
0:13a5d365ba16 
ignore and revise some of error parts

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_ANGLEAXIS_H
ykuroda 0:13a5d365ba16 11 #define EIGEN_ANGLEAXIS_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 AngleAxis
ykuroda 0:13a5d365ba16 18 *
ykuroda 0:13a5d365ba16 19 * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
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 * \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized.
ykuroda 0:13a5d365ba16 24 *
ykuroda 0:13a5d365ba16 25 * The following two typedefs are provided for convenience:
ykuroda 0:13a5d365ba16 26 * \li \c AngleAxisf for \c float
ykuroda 0:13a5d365ba16 27 * \li \c AngleAxisd for \c double
ykuroda 0:13a5d365ba16 28 *
ykuroda 0:13a5d365ba16 29 * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
ykuroda 0:13a5d365ba16 30 * mimic Euler-angles. Here is an example:
ykuroda 0:13a5d365ba16 31 * \include AngleAxis_mimic_euler.cpp
ykuroda 0:13a5d365ba16 32 * Output: \verbinclude AngleAxis_mimic_euler.out
ykuroda 0:13a5d365ba16 33 *
ykuroda 0:13a5d365ba16 34 * \note This class is not aimed to be used to store a rotation transformation,
ykuroda 0:13a5d365ba16 35 * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix)
ykuroda 0:13a5d365ba16 36 * and transformation objects.
ykuroda 0:13a5d365ba16 37 *
ykuroda 0:13a5d365ba16 38 * \sa class Quaternion, class Transform, MatrixBase::UnitX()
ykuroda 0:13a5d365ba16 39 */
ykuroda 0:13a5d365ba16 40
ykuroda 0:13a5d365ba16 41 namespace internal {
ykuroda 0:13a5d365ba16 42 template<typename _Scalar> struct traits<AngleAxis<_Scalar> >
ykuroda 0:13a5d365ba16 43 {
ykuroda 0:13a5d365ba16 44 typedef _Scalar Scalar;
ykuroda 0:13a5d365ba16 45 };
ykuroda 0:13a5d365ba16 46 }
ykuroda 0:13a5d365ba16 47
ykuroda 0:13a5d365ba16 48 template<typename _Scalar>
ykuroda 0:13a5d365ba16 49 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
ykuroda 0:13a5d365ba16 50 {
ykuroda 0:13a5d365ba16 51 typedef RotationBase<AngleAxis<_Scalar>,3> Base;
ykuroda 0:13a5d365ba16 52
ykuroda 0:13a5d365ba16 53 public:
ykuroda 0:13a5d365ba16 54
ykuroda 0:13a5d365ba16 55 using Base::operator*;
ykuroda 0:13a5d365ba16 56
ykuroda 0:13a5d365ba16 57 enum { Dim = 3 };
ykuroda 0:13a5d365ba16 58 /** the scalar type of the coefficients */
ykuroda 0:13a5d365ba16 59 typedef _Scalar Scalar;
ykuroda 0:13a5d365ba16 60 typedef Matrix<Scalar,3,3> Matrix3;
ykuroda 0:13a5d365ba16 61 typedef Matrix<Scalar,3,1> Vector3;
ykuroda 0:13a5d365ba16 62 typedef Quaternion<Scalar> QuaternionType;
ykuroda 0:13a5d365ba16 63
ykuroda 0:13a5d365ba16 64 protected:
ykuroda 0:13a5d365ba16 65
ykuroda 0:13a5d365ba16 66 Vector3 m_axis;
ykuroda 0:13a5d365ba16 67 Scalar m_angle;
ykuroda 0:13a5d365ba16 68
ykuroda 0:13a5d365ba16 69 public:
ykuroda 0:13a5d365ba16 70
ykuroda 0:13a5d365ba16 71 /** Default constructor without initialization. */
ykuroda 0:13a5d365ba16 72 AngleAxis() {}
ykuroda 0:13a5d365ba16 73 /** Constructs and initialize the angle-axis rotation from an \a angle in radian
ykuroda 0:13a5d365ba16 74 * and an \a axis which \b must \b be \b normalized.
ykuroda 0:13a5d365ba16 75 *
ykuroda 0:13a5d365ba16 76 * \warning If the \a axis vector is not normalized, then the angle-axis object
ykuroda 0:13a5d365ba16 77 * represents an invalid rotation. */
ykuroda 0:13a5d365ba16 78 template<typename Derived>
ykuroda 0:13a5d365ba16 79 inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
ykuroda 0:13a5d365ba16 80 /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */
ykuroda 0:13a5d365ba16 81 template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
ykuroda 0:13a5d365ba16 82 /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
ykuroda 0:13a5d365ba16 83 template<typename Derived>
ykuroda 0:13a5d365ba16 84 inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
ykuroda 0:13a5d365ba16 85
ykuroda 0:13a5d365ba16 86 /** \returns the value of the rotation angle in radian */
ykuroda 0:13a5d365ba16 87 Scalar angle() const { return m_angle; }
ykuroda 0:13a5d365ba16 88 /** \returns a read-write reference to the stored angle in radian */
ykuroda 0:13a5d365ba16 89 Scalar& angle() { return m_angle; }
ykuroda 0:13a5d365ba16 90
ykuroda 0:13a5d365ba16 91 /** \returns the rotation axis */
ykuroda 0:13a5d365ba16 92 const Vector3& axis() const { return m_axis; }
ykuroda 0:13a5d365ba16 93 /** \returns a read-write reference to the stored rotation axis.
ykuroda 0:13a5d365ba16 94 *
ykuroda 0:13a5d365ba16 95 * \warning The rotation axis must remain a \b unit vector.
ykuroda 0:13a5d365ba16 96 */
ykuroda 0:13a5d365ba16 97 Vector3& axis() { return m_axis; }
ykuroda 0:13a5d365ba16 98
ykuroda 0:13a5d365ba16 99 /** Concatenates two rotations */
ykuroda 0:13a5d365ba16 100 inline QuaternionType operator* (const AngleAxis& other) const
ykuroda 0:13a5d365ba16 101 { return QuaternionType(*this) * QuaternionType(other); }
ykuroda 0:13a5d365ba16 102
ykuroda 0:13a5d365ba16 103 /** Concatenates two rotations */
ykuroda 0:13a5d365ba16 104 inline QuaternionType operator* (const QuaternionType& other) const
ykuroda 0:13a5d365ba16 105 { return QuaternionType(*this) * other; }
ykuroda 0:13a5d365ba16 106
ykuroda 0:13a5d365ba16 107 /** Concatenates two rotations */
ykuroda 0:13a5d365ba16 108 friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
ykuroda 0:13a5d365ba16 109 { return a * QuaternionType(b); }
ykuroda 0:13a5d365ba16 110
ykuroda 0:13a5d365ba16 111 /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
ykuroda 0:13a5d365ba16 112 AngleAxis inverse() const
ykuroda 0:13a5d365ba16 113 { return AngleAxis(-m_angle, m_axis); }
ykuroda 0:13a5d365ba16 114
ykuroda 0:13a5d365ba16 115 template<class QuatDerived>
ykuroda 0:13a5d365ba16 116 AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
ykuroda 0:13a5d365ba16 117 template<typename Derived>
ykuroda 0:13a5d365ba16 118 AngleAxis& operator=(const MatrixBase<Derived>& m);
ykuroda 0:13a5d365ba16 119
ykuroda 0:13a5d365ba16 120 template<typename Derived>
ykuroda 0:13a5d365ba16 121 AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
ykuroda 0:13a5d365ba16 122 Matrix3 toRotationMatrix(void) const;
ykuroda 0:13a5d365ba16 123
ykuroda 0:13a5d365ba16 124 /** \returns \c *this with scalar type casted to \a NewScalarType
ykuroda 0:13a5d365ba16 125 *
ykuroda 0:13a5d365ba16 126 * Note that if \a NewScalarType is equal to the current scalar type of \c *this
ykuroda 0:13a5d365ba16 127 * then this function smartly returns a const reference to \c *this.
ykuroda 0:13a5d365ba16 128 */
ykuroda 0:13a5d365ba16 129 template<typename NewScalarType>
ykuroda 0:13a5d365ba16 130 inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
ykuroda 0:13a5d365ba16 131 { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
ykuroda 0:13a5d365ba16 132
ykuroda 0:13a5d365ba16 133 /** Copy constructor with scalar type conversion */
ykuroda 0:13a5d365ba16 134 template<typename OtherScalarType>
ykuroda 0:13a5d365ba16 135 inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
ykuroda 0:13a5d365ba16 136 {
ykuroda 0:13a5d365ba16 137 m_axis = other.axis().template cast<Scalar>();
ykuroda 0:13a5d365ba16 138 m_angle = Scalar(other.angle());
ykuroda 0:13a5d365ba16 139 }
ykuroda 0:13a5d365ba16 140
ykuroda 0:13a5d365ba16 141 static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); }
ykuroda 0:13a5d365ba16 142
ykuroda 0:13a5d365ba16 143 /** \returns \c true if \c *this is approximately equal to \a other, within the precision
ykuroda 0:13a5d365ba16 144 * determined by \a prec.
ykuroda 0:13a5d365ba16 145 *
ykuroda 0:13a5d365ba16 146 * \sa MatrixBase::isApprox() */
ykuroda 0:13a5d365ba16 147 bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
ykuroda 0:13a5d365ba16 148 { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
ykuroda 0:13a5d365ba16 149 };
ykuroda 0:13a5d365ba16 150
ykuroda 0:13a5d365ba16 151 /** \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 152 * single precision angle-axis type */
ykuroda 0:13a5d365ba16 153 typedef AngleAxis<float> AngleAxisf;
ykuroda 0:13a5d365ba16 154 /** \ingroup Geometry_Module
ykuroda 0:13a5d365ba16 155 * double precision angle-axis type */
ykuroda 0:13a5d365ba16 156 typedef AngleAxis<double> AngleAxisd;
ykuroda 0:13a5d365ba16 157
ykuroda 0:13a5d365ba16 158 /** Set \c *this from a \b unit quaternion.
ykuroda 0:13a5d365ba16 159 * The axis is normalized.
ykuroda 0:13a5d365ba16 160 *
ykuroda 0:13a5d365ba16 161 * \warning As any other method dealing with quaternion, if the input quaternion
ykuroda 0:13a5d365ba16 162 * is not normalized then the result is undefined.
ykuroda 0:13a5d365ba16 163 */
ykuroda 0:13a5d365ba16 164 template<typename Scalar>
ykuroda 0:13a5d365ba16 165 template<typename QuatDerived>
ykuroda 0:13a5d365ba16 166 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
ykuroda 0:13a5d365ba16 167 {
ykuroda 0:13a5d365ba16 168 using std::acos;
ykuroda 0:13a5d365ba16 169 using std::min;
ykuroda 0:13a5d365ba16 170 using std::max;
ykuroda 0:13a5d365ba16 171 using std::sqrt;
ykuroda 0:13a5d365ba16 172 Scalar n2 = q.vec().squaredNorm();
ykuroda 0:13a5d365ba16 173 if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision())
ykuroda 0:13a5d365ba16 174 {
ykuroda 0:13a5d365ba16 175 m_angle = Scalar(0);
ykuroda 0:13a5d365ba16 176 m_axis << Scalar(1), Scalar(0), Scalar(0);
ykuroda 0:13a5d365ba16 177 }
ykuroda 0:13a5d365ba16 178 else
ykuroda 0:13a5d365ba16 179 {
ykuroda 0:13a5d365ba16 180 m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w()),Scalar(1)));
ykuroda 0:13a5d365ba16 181 m_axis = q.vec() / sqrt(n2);
ykuroda 0:13a5d365ba16 182 }
ykuroda 0:13a5d365ba16 183 return *this;
ykuroda 0:13a5d365ba16 184 }
ykuroda 0:13a5d365ba16 185
ykuroda 0:13a5d365ba16 186 /** Set \c *this from a 3x3 rotation matrix \a mat.
ykuroda 0:13a5d365ba16 187 */
ykuroda 0:13a5d365ba16 188 template<typename Scalar>
ykuroda 0:13a5d365ba16 189 template<typename Derived>
ykuroda 0:13a5d365ba16 190 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
ykuroda 0:13a5d365ba16 191 {
ykuroda 0:13a5d365ba16 192 // Since a direct conversion would not be really faster,
ykuroda 0:13a5d365ba16 193 // let's use the robust Quaternion implementation:
ykuroda 0:13a5d365ba16 194 return *this = QuaternionType(mat);
ykuroda 0:13a5d365ba16 195 }
ykuroda 0:13a5d365ba16 196
ykuroda 0:13a5d365ba16 197 /**
ykuroda 0:13a5d365ba16 198 * \brief Sets \c *this from a 3x3 rotation matrix.
ykuroda 0:13a5d365ba16 199 **/
ykuroda 0:13a5d365ba16 200 template<typename Scalar>
ykuroda 0:13a5d365ba16 201 template<typename Derived>
ykuroda 0:13a5d365ba16 202 AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
ykuroda 0:13a5d365ba16 203 {
ykuroda 0:13a5d365ba16 204 return *this = QuaternionType(mat);
ykuroda 0:13a5d365ba16 205 }
ykuroda 0:13a5d365ba16 206
ykuroda 0:13a5d365ba16 207 /** Constructs and \returns an equivalent 3x3 rotation matrix.
ykuroda 0:13a5d365ba16 208 */
ykuroda 0:13a5d365ba16 209 template<typename Scalar>
ykuroda 0:13a5d365ba16 210 typename AngleAxis<Scalar>::Matrix3
ykuroda 0:13a5d365ba16 211 AngleAxis<Scalar>::toRotationMatrix(void) const
ykuroda 0:13a5d365ba16 212 {
ykuroda 0:13a5d365ba16 213 using std::sin;
ykuroda 0:13a5d365ba16 214 using std::cos;
ykuroda 0:13a5d365ba16 215 Matrix3 res;
ykuroda 0:13a5d365ba16 216 Vector3 sin_axis = sin(m_angle) * m_axis;
ykuroda 0:13a5d365ba16 217 Scalar c = cos(m_angle);
ykuroda 0:13a5d365ba16 218 Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
ykuroda 0:13a5d365ba16 219
ykuroda 0:13a5d365ba16 220 Scalar tmp;
ykuroda 0:13a5d365ba16 221 tmp = cos1_axis.x() * m_axis.y();
ykuroda 0:13a5d365ba16 222 res.coeffRef(0,1) = tmp - sin_axis.z();
ykuroda 0:13a5d365ba16 223 res.coeffRef(1,0) = tmp + sin_axis.z();
ykuroda 0:13a5d365ba16 224
ykuroda 0:13a5d365ba16 225 tmp = cos1_axis.x() * m_axis.z();
ykuroda 0:13a5d365ba16 226 res.coeffRef(0,2) = tmp + sin_axis.y();
ykuroda 0:13a5d365ba16 227 res.coeffRef(2,0) = tmp - sin_axis.y();
ykuroda 0:13a5d365ba16 228
ykuroda 0:13a5d365ba16 229 tmp = cos1_axis.y() * m_axis.z();
ykuroda 0:13a5d365ba16 230 res.coeffRef(1,2) = tmp - sin_axis.x();
ykuroda 0:13a5d365ba16 231 res.coeffRef(2,1) = tmp + sin_axis.x();
ykuroda 0:13a5d365ba16 232
ykuroda 0:13a5d365ba16 233 res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
ykuroda 0:13a5d365ba16 234
ykuroda 0:13a5d365ba16 235 return res;
ykuroda 0:13a5d365ba16 236 }
ykuroda 0:13a5d365ba16 237
ykuroda 0:13a5d365ba16 238 } // end namespace Eigen
ykuroda 0:13a5d365ba16 239
ykuroda 0:13a5d365ba16 240 #endif // EIGEN_ANGLEAXIS_H