1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
5 //
6 // This Source Code Form is subject to the terms of the Mozilla
7 // Public License v. 2.0. If a copy of the MPL was not distributed
8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9 
10 // no include guard, we'll include this twice from All.h from Eigen2Support, and it's internal anyway
11 
12 namespace Eigen {
13 
14 /** \geometry_module \ingroup Geometry_Module
15   *
16   * \class AngleAxis
17   *
18   * \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
19   *
20   * \param _Scalar the scalar type, i.e., the type of the coefficients.
21   *
22   * The following two typedefs are provided for convenience:
23   * \li \c AngleAxisf for \c float
24   * \li \c AngleAxisd for \c double
25   *
26   * \addexample AngleAxisForEuler \label How to define a rotation from Euler-angles
27   *
28   * Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
29   * mimic Euler-angles. Here is an example:
30   * \include AngleAxis_mimic_euler.cpp
31   * Output: \verbinclude AngleAxis_mimic_euler.out
32   *
33   * \note This class is not aimed to be used to store a rotation transformation,
34   * but rather to make easier the creation of other rotation (Quaternion, rotation Matrix)
35   * and transformation objects.
36   *
37   * \sa class Quaternion, class Transform, MatrixBase::UnitX()
38   */
39 
40 template<typename _Scalar> struct ei_traits<AngleAxis<_Scalar> >
41 {
42   typedef _Scalar Scalar;
43 };
44 
45 template<typename _Scalar>
46 class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
47 {
48   typedef RotationBase<AngleAxis<_Scalar>,3> Base;
49 
50 public:
51 
52   using Base::operator*;
53 
54   enum { Dim = 3 };
55   /** the scalar type of the coefficients */
56   typedef _Scalar Scalar;
57   typedef Matrix<Scalar,3,3> Matrix3;
58   typedef Matrix<Scalar,3,1> Vector3;
59   typedef Quaternion<Scalar> QuaternionType;
60 
61 protected:
62 
63   Vector3 m_axis;
64   Scalar m_angle;
65 
66 public:
67 
68   /** Default constructor without initialization. */
69   AngleAxis() {}
70   /** Constructs and initialize the angle-axis rotation from an \a angle in radian
71     * and an \a axis which must be normalized. */
72   template<typename Derived>
73   inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
74   /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */
75   inline AngleAxis(const QuaternionType& q) { *this = q; }
76   /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
77   template<typename Derived>
78   inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
79 
80   Scalar angle() const { return m_angle; }
81   Scalar& angle() { return m_angle; }
82 
83   const Vector3& axis() const { return m_axis; }
84   Vector3& axis() { return m_axis; }
85 
86   /** Concatenates two rotations */
87   inline QuaternionType operator* (const AngleAxis& other) const
88   { return QuaternionType(*this) * QuaternionType(other); }
89 
90   /** Concatenates two rotations */
91   inline QuaternionType operator* (const QuaternionType& other) const
92   { return QuaternionType(*this) * other; }
93 
94   /** Concatenates two rotations */
95   friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
96   { return a * QuaternionType(b); }
97 
98   /** Concatenates two rotations */
99   inline Matrix3 operator* (const Matrix3& other) const
100   { return toRotationMatrix() * other; }
101 
102   /** Concatenates two rotations */
103   inline friend Matrix3 operator* (const Matrix3& a, const AngleAxis& b)
104   { return a * b.toRotationMatrix(); }
105 
106   /** Applies rotation to vector */
107   inline Vector3 operator* (const Vector3& other) const
108   { return toRotationMatrix() * other; }
109 
110   /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
111   AngleAxis inverse() const
112   { return AngleAxis(-m_angle, m_axis); }
113 
114   AngleAxis& operator=(const QuaternionType& q);
115   template<typename Derived>
116   AngleAxis& operator=(const MatrixBase<Derived>& m);
117 
118   template<typename Derived>
119   AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
120   Matrix3 toRotationMatrix(void) const;
121 
122   /** \returns \c *this with scalar type casted to \a NewScalarType
123     *
124     * Note that if \a NewScalarType is equal to the current scalar type of \c *this
125     * then this function smartly returns a const reference to \c *this.
126     */
127   template<typename NewScalarType>
128   inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
129   { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
130 
131   /** Copy constructor with scalar type conversion */
132   template<typename OtherScalarType>
133   inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
134   {
135     m_axis = other.axis().template cast<Scalar>();
136     m_angle = Scalar(other.angle());
137   }
138 
139   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
140     * determined by \a prec.
141     *
142     * \sa MatrixBase::isApprox() */
143   bool isApprox(const AngleAxis& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
144   { return m_axis.isApprox(other.m_axis, prec) && ei_isApprox(m_angle,other.m_angle, prec); }
145 };
146 
147 /** \ingroup Geometry_Module
148   * single precision angle-axis type */
149 typedef AngleAxis<float> AngleAxisf;
150 /** \ingroup Geometry_Module
151   * double precision angle-axis type */
152 typedef AngleAxis<double> AngleAxisd;
153 
154 /** Set \c *this from a quaternion.
155   * The axis is normalized.
156   */
157 template<typename Scalar>
158 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
159 {
160   Scalar n2 = q.vec().squaredNorm();
161   if (n2 < precision<Scalar>()*precision<Scalar>())
162   {
163     m_angle = 0;
164     m_axis << 1, 0, 0;
165   }
166   else
167   {
168     m_angle = 2*std::acos(q.w());
169     m_axis = q.vec() / ei_sqrt(n2);
170   }
171   return *this;
172 }
173 
174 /** Set \c *this from a 3x3 rotation matrix \a mat.
175   */
176 template<typename Scalar>
177 template<typename Derived>
178 AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
179 {
180   // Since a direct conversion would not be really faster,
181   // let's use the robust Quaternion implementation:
182   return *this = QuaternionType(mat);
183 }
184 
185 /** Constructs and \returns an equivalent 3x3 rotation matrix.
186   */
187 template<typename Scalar>
188 typename AngleAxis<Scalar>::Matrix3
189 AngleAxis<Scalar>::toRotationMatrix(void) const
190 {
191   Matrix3 res;
192   Vector3 sin_axis  = ei_sin(m_angle) * m_axis;
193   Scalar c = ei_cos(m_angle);
194   Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
195 
196   Scalar tmp;
197   tmp = cos1_axis.x() * m_axis.y();
198   res.coeffRef(0,1) = tmp - sin_axis.z();
199   res.coeffRef(1,0) = tmp + sin_axis.z();
200 
201   tmp = cos1_axis.x() * m_axis.z();
202   res.coeffRef(0,2) = tmp + sin_axis.y();
203   res.coeffRef(2,0) = tmp - sin_axis.y();
204 
205   tmp = cos1_axis.y() * m_axis.z();
206   res.coeffRef(1,2) = tmp - sin_axis.x();
207   res.coeffRef(2,1) = tmp + sin_axis.x();
208 
209   res.diagonal() = (cos1_axis.cwise() * m_axis).cwise() + c;
210 
211   return res;
212 }
213 
214 } // end namespace Eigen
215