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
13namespace 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
41namespace internal {
42template<typename _Scalar> struct traits<AngleAxis<_Scalar> >
43{
44  typedef _Scalar Scalar;
45};
46}
47
48template<typename _Scalar>
49class AngleAxis : public RotationBase<AngleAxis<_Scalar>,3>
50{
51  typedef RotationBase<AngleAxis<_Scalar>,3> Base;
52
53public:
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
64protected:
65
66  Vector3 m_axis;
67  Scalar m_angle;
68
69public:
70
71  /** Default constructor without initialization. */
72  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  inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
80  /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */
81  template<typename QuatDerived> inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) { *this = q; }
82  /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
83  template<typename Derived>
84  inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
85
86  Scalar angle() const { return m_angle; }
87  Scalar& angle() { return m_angle; }
88
89  const Vector3& axis() const { return m_axis; }
90  Vector3& axis() { return m_axis; }
91
92  /** Concatenates two rotations */
93  inline QuaternionType operator* (const AngleAxis& other) const
94  { return QuaternionType(*this) * QuaternionType(other); }
95
96  /** Concatenates two rotations */
97  inline QuaternionType operator* (const QuaternionType& other) const
98  { return QuaternionType(*this) * other; }
99
100  /** Concatenates two rotations */
101  friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
102  { return a * QuaternionType(b); }
103
104  /** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
105  AngleAxis inverse() const
106  { return AngleAxis(-m_angle, m_axis); }
107
108  template<class QuatDerived>
109  AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
110  template<typename Derived>
111  AngleAxis& operator=(const MatrixBase<Derived>& m);
112
113  template<typename Derived>
114  AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
115  Matrix3 toRotationMatrix(void) const;
116
117  /** \returns \c *this with scalar type casted to \a NewScalarType
118    *
119    * Note that if \a NewScalarType is equal to the current scalar type of \c *this
120    * then this function smartly returns a const reference to \c *this.
121    */
122  template<typename NewScalarType>
123  inline typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type cast() const
124  { return typename internal::cast_return_type<AngleAxis,AngleAxis<NewScalarType> >::type(*this); }
125
126  /** Copy constructor with scalar type conversion */
127  template<typename OtherScalarType>
128  inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other)
129  {
130    m_axis = other.axis().template cast<Scalar>();
131    m_angle = Scalar(other.angle());
132  }
133
134  static inline const AngleAxis Identity() { return AngleAxis(0, Vector3::UnitX()); }
135
136  /** \returns \c true if \c *this is approximately equal to \a other, within the precision
137    * determined by \a prec.
138    *
139    * \sa MatrixBase::isApprox() */
140  bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
141  { return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle,other.m_angle, prec); }
142};
143
144/** \ingroup Geometry_Module
145  * single precision angle-axis type */
146typedef AngleAxis<float> AngleAxisf;
147/** \ingroup Geometry_Module
148  * double precision angle-axis type */
149typedef AngleAxis<double> AngleAxisd;
150
151/** Set \c *this from a \b unit quaternion.
152  * The axis is normalized.
153  *
154  * \warning As any other method dealing with quaternion, if the input quaternion
155  *          is not normalized then the result is undefined.
156  */
157template<typename Scalar>
158template<typename QuatDerived>
159AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q)
160{
161  using std::acos;
162  using std::min;
163  using std::max;
164  using std::sqrt;
165  Scalar n2 = q.vec().squaredNorm();
166  if (n2 < NumTraits<Scalar>::dummy_precision()*NumTraits<Scalar>::dummy_precision())
167  {
168    m_angle = 0;
169    m_axis << 1, 0, 0;
170  }
171  else
172  {
173    m_angle = Scalar(2)*acos((min)((max)(Scalar(-1),q.w()),Scalar(1)));
174    m_axis = q.vec() / sqrt(n2);
175  }
176  return *this;
177}
178
179/** Set \c *this from a 3x3 rotation matrix \a mat.
180  */
181template<typename Scalar>
182template<typename Derived>
183AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
184{
185  // Since a direct conversion would not be really faster,
186  // let's use the robust Quaternion implementation:
187  return *this = QuaternionType(mat);
188}
189
190/**
191* \brief Sets \c *this from a 3x3 rotation matrix.
192**/
193template<typename Scalar>
194template<typename Derived>
195AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat)
196{
197  return *this = QuaternionType(mat);
198}
199
200/** Constructs and \returns an equivalent 3x3 rotation matrix.
201  */
202template<typename Scalar>
203typename AngleAxis<Scalar>::Matrix3
204AngleAxis<Scalar>::toRotationMatrix(void) const
205{
206  using std::sin;
207  using std::cos;
208  Matrix3 res;
209  Vector3 sin_axis  = sin(m_angle) * m_axis;
210  Scalar c = cos(m_angle);
211  Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
212
213  Scalar tmp;
214  tmp = cos1_axis.x() * m_axis.y();
215  res.coeffRef(0,1) = tmp - sin_axis.z();
216  res.coeffRef(1,0) = tmp + sin_axis.z();
217
218  tmp = cos1_axis.x() * m_axis.z();
219  res.coeffRef(0,2) = tmp + sin_axis.y();
220  res.coeffRef(2,0) = tmp - sin_axis.y();
221
222  tmp = cos1_axis.y() * m_axis.z();
223  res.coeffRef(1,2) = tmp - sin_axis.x();
224  res.coeffRef(2,1) = tmp + sin_axis.x();
225
226  res.diagonal() = (cos1_axis.cwiseProduct(m_axis)).array() + c;
227
228  return res;
229}
230
231} // end namespace Eigen
232
233#endif // EIGEN_ANGLEAXIS_H
234