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