src/Geometry/AngleAxis.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
0005 //
0006 // This Source Code Form is subject to the terms of the Mozilla
0007 // Public License v. 2.0. If a copy of the MPL was not distributed
0008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
0009
0010 #ifndef EIGEN_ANGLEAXIS_H
0011 #define EIGEN_ANGLEAXIS_H
0012
0013 namespace Eigen {
0014
0015 /** \geometry_module \ingroup Geometry_Module
0016   *
0017   * \class AngleAxis
0018   *
0019   * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
0020   *
0021   * \param _Scalar the scalar type, i.e., the type of the coefficients.
0022   *
0023   * \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized.
0024   *
0025   * The following two typedefs are provided for convenience:
0026   * \li \c AngleAxisf for \c float
0027   * \li \c AngleAxisd for \c double
0028   *
0029   * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
0030   * mimic Euler-angles. Here is an example:
0031   * \include AngleAxis_mimic_euler.cpp
0032   * Output: \verbinclude AngleAxis_mimic_euler.out
0033   *
0034   * \note This class is not aimed to be used to store a rotation transformation,
0035   * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix)
0036   * and transformation objects.
0037   *
0038   * \sa class Quaternion, class Transform, MatrixBase::UnitX()
0039   */
0040
0041 namespace internal {
0042 template<typename _Scalar> struct traits<AngleAxis<_Scalar> >
0043 {
0044   typedef _Scalar Scalar;
0045 };
0046 }
0047
0048 template<typename _Scalar>
0049 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
0050 {
0051   typedef RotationBase<AngleAxis<_Scalar>,3> Base;
0052
0053 public:
0054
0055   using Base::operator*;
0056
0057   enum { Dim = 3 };
0058   /** the scalar type of the coefficients */
0059   typedef _Scalar Scalar;
0060   typedef Matrix<Scalar,3,3> Matrix3;
0061   typedef Matrix<Scalar,3,1> Vector3;
0062   typedef Quaternion<Scalar> QuaternionType;
0063
0064 protected:
0065
0066   Vector3 m_axis;
0067   Scalar m_angle;
0068
0069 public:
0070
0071   /** Default constructor without initialization. */
0072   EIGEN_DEVICE_FUNC AngleAxis() {}
0073   /** Constructs and initialize the angle-axis rotation from an \a angle in radian
0074     * and an \a axis which \b must \b be \b normalized.
0075     *
0076     * \warning If the \a axis vector is not normalized, then the angle-axis object
0077     *          represents an invalid rotation. */
0078   template<typename Derived>
0079   EIGEN_DEVICE_FUNC
0080   inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
0081   /** Constructs and initialize the angle-axis rotation from a quaternion \a q.
0082     * This function implicitly normalizes the quaternion \a q.
0083     */
0084   template<typename QuatDerived>
0085   EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
0086   /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
0087   template<typename Derived>
0088   EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
0089
0090   /** \returns the value of the rotation angle in radian */
0091   EIGEN_DEVICE_FUNC Scalar angle() const { return m_angle; }
0092   /** \returns a read-write reference to the stored angle in radian */
0093   EIGEN_DEVICE_FUNC Scalar& angle() { return m_angle; }
0094
0095   /** \returns the rotation axis */
0096   EIGEN_DEVICE_FUNC const Vector3& axis() const { return m_axis; }
0097   /** \returns a read-write reference to the stored rotation axis.
0098     *
0099     * \warning The rotation axis must remain a \b unit vector.
0100     */
0101   EIGEN_DEVICE_FUNC Vector3& axis() { return m_axis; }
0102
0103   /** Concatenates two rotations */
0104   EIGEN_DEVICE_FUNC inline QuaternionType operator* (const AngleAxis& other) const
0105   { return QuaternionType(*this) * QuaternionType(other); }
0106
0107   /** Concatenates two rotations */
0108   EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& other) const
0109   { return QuaternionType(*this) * other; }
0110
0111   /** Concatenates two rotations */
0112   friend EIGEN_DEVICE_FUNC inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
0113   { return a * QuaternionType(b); }
0114
0115   /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
0116   EIGEN_DEVICE_FUNC AngleAxis inverse() const
0117   { return AngleAxis(-m_angle, m_axis); }
0118
0119   template<class QuatDerived>
0120   EIGEN_DEVICE_FUNC AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
0121   template<typename Derived>
0122   EIGEN_DEVICE_FUNC AngleAxis& operator=(const MatrixBase<Derived>& m);
0123
0124   template<typename Derived>
0125   EIGEN_DEVICE_FUNC AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
0126   EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const;
0127
0128   /** \returns \c *this with scalar type casted to \a NewScalarType
0129     *
0130     * Note that if \a NewScalarType is equal to the current scalar type of \c *this
0131     * then this function smartly returns a const reference to \c *this.
0132     */
0133   template<typename NewScalarType>
0134   EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
0135   { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
0136
0137   /** Copy constructor with scalar type conversion */
0138   template<typename OtherScalarType>
0139   EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
0140   {
0141     m_axis = other.axis().template cast<Scalar>();
0142     m_angle = Scalar(other.angle());
0143   }
0144
0145   EIGEN_DEVICE_FUNC static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); }
0146
0147   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
0148     * determined by \a prec.
0149     *
0150     * \sa MatrixBase::isApprox() */
0151   EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
0152   { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
0153 };
0154
0155 /** \ingroup Geometry_Module
0156   * single precision angle-axis type */
0157 typedef AngleAxis<float> AngleAxisf;
0158 /** \ingroup Geometry_Module
0159   * double precision angle-axis type */
0160 typedef AngleAxis<double> AngleAxisd;
0161
0162 /** Set \c *this from a \b unit quaternion.
0163   *
0164   * The resulting axis is normalized, and the computed angle is in the [0,pi] range.
0165   *
0166   * This function implicitly normalizes the quaternion \a q.
0167   */
0168 template<typename Scalar>
0169 template<typename QuatDerived>
0170 EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
0171 {
0172   EIGEN_USING_STD(atan2)
0173   EIGEN_USING_STD(abs)
0174   Scalar n = q.vec().norm();
0175   if(n<NumTraits<Scalar>::epsilon())
0176     n = q.vec().stableNorm();
0177
0178   if (n != Scalar(0))
0179   {
0180     m_angle = Scalar(2)*atan2(n, abs(q.w()));
0181     if(q.w() < Scalar(0))
0182       n = -n;
0183     m_axis  = q.vec() / n;
0184   }
0185   else
0186   {
0187     m_angle = Scalar(0);
0188     m_axis << Scalar(1), Scalar(0), Scalar(0);
0189   }
0190   return *this;
0191 }
0192
0193 /** Set \c *this from a 3x3 rotation matrix \a mat.
0194   */
0195 template<typename Scalar>
0196 template<typename Derived>
0197 EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
0198 {
0199   // Since a direct conversion would not be really faster,
0200   // let's use the robust Quaternion implementation:
0201   return *this = QuaternionType(mat);
0202 }
0203
0204 /**
0205 * \brief Sets \c *this from a 3x3 rotation matrix.
0206 **/
0207 template<typename Scalar>
0208 template<typename Derived>
0209 EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
0210 {
0211   return *this = QuaternionType(mat);
0212 }
0213
0214 /** Constructs and \returns an equivalent 3x3 rotation matrix.
0215   */
0216 template<typename Scalar>
0217 typename AngleAxis<Scalar>::Matrix3
0218 EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const
0219 {
0220   EIGEN_USING_STD(sin)
0221   EIGEN_USING_STD(cos)
0222   Matrix3 res;
0223   Vector3 sin_axis  = sin(m_angle) * m_axis;
0224   Scalar c = cos(m_angle);
0225   Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
0226
0227   Scalar tmp;
0228   tmp = cos1_axis.x() * m_axis.y();
0229   res.coeffRef(0,1) = tmp - sin_axis.z();
0230   res.coeffRef(1,0) = tmp + sin_axis.z();
0231
0232   tmp = cos1_axis.x() * m_axis.z();
0233   res.coeffRef(0,2) = tmp + sin_axis.y();
0234   res.coeffRef(2,0) = tmp - sin_axis.y();
0235
0236   tmp = cos1_axis.y() * m_axis.z();
0237   res.coeffRef(1,2) = tmp - sin_axis.x();
0238   res.coeffRef(2,1) = tmp + sin_axis.x();
0239
0240   res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
0241
0242   return res;
0243 }
0244
0245 } // end namespace Eigen
0246
0247 #endif // EIGEN_ANGLEAXIS_H