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 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 */
157typedef AngleAxis<float> AngleAxisf;
158/** \ingroup Geometry_Module
159 * double precision angle-axis type */
160typedef 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 */
168template<typename Scalar>
169template<typename QuatDerived>
170EIGEN_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() < Scalar(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 */
195template<typename Scalar>
196template<typename Derived>
197EIGEN_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**/
207template<typename Scalar>
208template<typename Derived>
209EIGEN_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 */
216template<typename Scalar>
217typename AngleAxis<Scalar>::Matrix3
218EIGEN_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