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