Main MRPT website > C++ reference
MRPT logo

AngleAxis.h

Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 //
00006 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 #ifndef EIGEN_ANGLEAXIS_H
00026 #define EIGEN_ANGLEAXIS_H
00027 
00028 /** \geometry_module \ingroup Geometry_Module
00029   *
00030   * \class AngleAxis
00031   *
00032   * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
00033   *
00034   * \param _Scalar the scalar type, i.e., the type of the coefficients.
00035   *
00036   * \warning When setting up an AngleAxis object, the axis vector \b must \b be \b normalized.
00037   *
00038   * The following two typedefs are provided for convenience:
00039   * \li \c AngleAxisf for \c float
00040   * \li \c AngleAxisd for \c double
00041   *
00042   * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
00043   * mimic Euler-angles. Here is an example:
00044   * \include AngleAxis_mimic_euler.cpp
00045   * Output: \verbinclude AngleAxis_mimic_euler.out
00046   *
00047   * \note This class is not aimed to be used to store a rotation transformation,
00048   * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix)
00049   * and transformation objects.
00050   *
00051   * \sa class Quaternion, class Transform, MatrixBase::UnitX()
00052   */
00053 
00054 namespace internal {
00055 template<typename _Scalar> struct traits<AngleAxis<_Scalar> >
00056 {
00057   typedef _Scalar Scalar;
00058 };
00059 }
00060 
00061 template<typename _Scalar>
00062 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
00063 {
00064   typedef RotationBase<AngleAxis<_Scalar>,3> Base;
00065 
00066 public:
00067 
00068   using Base::operator*;
00069 
00070   enum { Dim = 3 };
00071   /** the scalar type of the coefficients */
00072   typedef _Scalar Scalar;
00073   typedef Matrix<Scalar,3,3> Matrix3;
00074   typedef Matrix<Scalar,3,1> Vector3;
00075   typedef Quaternion<Scalar> QuaternionType;
00076 
00077 protected:
00078 
00079   Vector3 m_axis;
00080   Scalar m_angle;
00081 
00082 public:
00083 
00084   /** Default constructor without initialization. */
00085   AngleAxis() {}
00086   /** Constructs and initialize the angle-axis rotation from an \a angle in radian
00087     * and an \a axis which \b must \b be \b normalized.
00088     *
00089     * \warning If the \a axis vector is not normalized, then the angle-axis object
00090     *          represents an invalid rotation. */
00091   template<typename Derived>
00092   inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
00093   /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */
00094   template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
00095   /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
00096   template<typename Derived>
00097   inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
00098 
00099   Scalar angle() const { return m_angle; }
00100   Scalar& angle() { return m_angle; }
00101 
00102   const Vector3& axis() const { return m_axis; }
00103   Vector3& axis() { return m_axis; }
00104 
00105   /** Concatenates two rotations */
00106   inline QuaternionType operator* (const AngleAxis& other) const
00107   { return QuaternionType(*this) * QuaternionType(other); }
00108 
00109   /** Concatenates two rotations */
00110   inline QuaternionType operator* (const QuaternionType& other) const
00111   { return QuaternionType(*this) * other; }
00112 
00113   /** Concatenates two rotations */
00114   friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
00115   { return a * QuaternionType(b); }
00116 
00117   /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
00118   AngleAxis inverse() const
00119   { return AngleAxis(-m_angle, m_axis); }
00120 
00121   template<class QuatDerived>
00122   AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
00123   template<typename Derived>
00124   AngleAxis& operator=(const MatrixBase<Derived>& m);
00125 
00126   template<typename Derived>
00127   AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
00128   Matrix3 toRotationMatrix(void) const;
00129 
00130   /** \returns \c *this with scalar type casted to \a NewScalarType
00131     *
00132     * Note that if \a NewScalarType is equal to the current scalar type of \c *this
00133     * then this function smartly returns a const reference to \c *this.
00134     */
00135   template<typename NewScalarType>
00136   inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
00137   { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
00138 
00139   /** Copy constructor with scalar type conversion */
00140   template<typename OtherScalarType>
00141   inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
00142   {
00143     m_axis = other.axis().template cast<Scalar>();
00144     m_angle = Scalar(other.angle());
00145   }
00146 
00147   inline static const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); }
00148 
00149   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
00150     * determined by \a prec.
00151     *
00152     * \sa MatrixBase::isApprox() */
00153   bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
00154   { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
00155 };
00156 
00157 /** \ingroup Geometry_Module
00158   * single precision angle-axis type */
00159 typedef AngleAxis<float> AngleAxisf;
00160 /** \ingroup Geometry_Module
00161   * double precision angle-axis type */
00162 typedef AngleAxis<double> AngleAxisd;
00163 
00164 /** Set \c *this from a \b unit quaternion.
00165   * The axis is normalized.
00166   * 
00167   * \warning As any other method dealing with quaternion, if the input quaternion
00168   *          is not normalized then the result is undefined.
00169   */
00170 template<typename Scalar>
00171 template<typename QuatDerived>
00172 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
00173 {
00174   Scalar n2 = q.vec().squaredNorm();
00175   if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision())
00176   {
00177     m_angle = 0;
00178     m_axis << 1, 0, 0;
00179   }
00180   else
00181   {
00182     m_angle = Scalar(2)*std::acos(std::min(std::max(Scalar(-1),q.w()),Scalar(1)));
00183     m_axis = q.vec() / internal::sqrt(n2);
00184   }
00185   return *this;
00186 }
00187 
00188 /** Set \c *this from a 3x3 rotation matrix \a mat.
00189   */
00190 template<typename Scalar>
00191 template<typename Derived>
00192 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
00193 {
00194   // Since a direct conversion would not be really faster,
00195   // let's use the robust Quaternion implementation:
00196   return *this = QuaternionType(mat);
00197 }
00198 
00199 /**
00200 * \brief Sets \c *this from a 3x3 rotation matrix.
00201 **/
00202 template<typename Scalar>
00203 template<typename Derived>
00204 AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
00205 {
00206   return *this = QuaternionType(mat);
00207 }
00208 
00209 /** Constructs and \returns an equivalent 3x3 rotation matrix.
00210   */
00211 template<typename Scalar>
00212 typename AngleAxis<Scalar>::Matrix3
00213 AngleAxis<Scalar>::toRotationMatrix(void) const
00214 {
00215   Matrix3 res;
00216   Vector3 sin_axis  = internal::sin(m_angle) * m_axis;
00217   Scalar c = internal::cos(m_angle);
00218   Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
00219 
00220   Scalar tmp;
00221   tmp = cos1_axis.x() * m_axis.y();
00222   res.coeffRef(0,1) = tmp - sin_axis.z();
00223   res.coeffRef(1,0) = tmp + sin_axis.z();
00224 
00225   tmp = cos1_axis.x() * m_axis.z();
00226   res.coeffRef(0,2) = tmp + sin_axis.y();
00227   res.coeffRef(2,0) = tmp - sin_axis.y();
00228 
00229   tmp = cos1_axis.y() * m_axis.z();
00230   res.coeffRef(1,2) = tmp - sin_axis.x();
00231   res.coeffRef(2,1) = tmp + sin_axis.x();
00232 
00233   res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
00234 
00235   return res;
00236 }
00237 
00238 #endif // EIGEN_ANGLEAXIS_H



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011