Cycles: svn merge -r41225:41232 ^/trunk/blender
[blender.git] / extern / Eigen2 / Eigen / src / Geometry / Quaternion.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_QUATERNION_H
26 #define EIGEN_QUATERNION_H
27
28 template<typename Other,
29          int OtherRows=Other::RowsAtCompileTime,
30          int OtherCols=Other::ColsAtCompileTime>
31 struct ei_quaternion_assign_impl;
32
33 /** \geometry_module \ingroup Geometry_Module
34   *
35   * \class Quaternion
36   *
37   * \brief The quaternion class used to represent 3D orientations and rotations
38   *
39   * \param _Scalar the scalar type, i.e., the type of the coefficients
40   *
41   * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of
42   * orientations and rotations of objects in three dimensions. Compared to other representations
43   * like Euler angles or 3x3 matrices, quatertions offer the following advantages:
44   * \li \b compact storage (4 scalars)
45   * \li \b efficient to compose (28 flops),
46   * \li \b stable spherical interpolation
47   *
48   * The following two typedefs are provided for convenience:
49   * \li \c Quaternionf for \c float
50   * \li \c Quaterniond for \c double
51   *
52   * \sa  class AngleAxis, class Transform
53   */
54
55 template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> >
56 {
57   typedef _Scalar Scalar;
58 };
59
60 template<typename _Scalar>
61 class Quaternion : public RotationBase<Quaternion<_Scalar>,3>
62 {
63   typedef RotationBase<Quaternion<_Scalar>,3> Base;
64
65 public:
66   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,4)
67
68   using Base::operator*;
69
70   /** the scalar type of the coefficients */
71   typedef _Scalar Scalar;
72
73   /** the type of the Coefficients 4-vector */
74   typedef Matrix<Scalar, 4, 1> Coefficients;
75   /** the type of a 3D vector */
76   typedef Matrix<Scalar,3,1> Vector3;
77   /** the equivalent rotation matrix type */
78   typedef Matrix<Scalar,3,3> Matrix3;
79   /** the equivalent angle-axis type */
80   typedef AngleAxis<Scalar> AngleAxisType;
81
82   /** \returns the \c x coefficient */
83   inline Scalar x() const { return m_coeffs.coeff(0); }
84   /** \returns the \c y coefficient */
85   inline Scalar y() const { return m_coeffs.coeff(1); }
86   /** \returns the \c z coefficient */
87   inline Scalar z() const { return m_coeffs.coeff(2); }
88   /** \returns the \c w coefficient */
89   inline Scalar w() const { return m_coeffs.coeff(3); }
90
91   /** \returns a reference to the \c x coefficient */
92   inline Scalar& x() { return m_coeffs.coeffRef(0); }
93   /** \returns a reference to the \c y coefficient */
94   inline Scalar& y() { return m_coeffs.coeffRef(1); }
95   /** \returns a reference to the \c z coefficient */
96   inline Scalar& z() { return m_coeffs.coeffRef(2); }
97   /** \returns a reference to the \c w coefficient */
98   inline Scalar& w() { return m_coeffs.coeffRef(3); }
99
100   /** \returns a read-only vector expression of the imaginary part (x,y,z) */
101   inline const Block<Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); }
102
103   /** \returns a vector expression of the imaginary part (x,y,z) */
104   inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); }
105
106   /** \returns a read-only vector expression of the coefficients (x,y,z,w) */
107   inline const Coefficients& coeffs() const { return m_coeffs; }
108
109   /** \returns a vector expression of the coefficients (x,y,z,w) */
110   inline Coefficients& coeffs() { return m_coeffs; }
111
112   /** Default constructor leaving the quaternion uninitialized. */
113   inline Quaternion() {}
114
115   /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
116     * its four coefficients \a w, \a x, \a y and \a z.
117     *
118     * \warning Note the order of the arguments: the real \a w coefficient first,
119     * while internally the coefficients are stored in the following order:
120     * [\c x, \c y, \c z, \c w]
121     */
122   inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z)
123   { m_coeffs << x, y, z, w; }
124
125   /** Copy constructor */
126   inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
127
128   /** Constructs and initializes a quaternion from the angle-axis \a aa */
129   explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
130
131   /** Constructs and initializes a quaternion from either:
132     *  - a rotation matrix expression,
133     *  - a 4D vector expression representing quaternion coefficients.
134     * \sa operator=(MatrixBase<Derived>)
135     */
136   template<typename Derived>
137   explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
138
139   Quaternion& operator=(const Quaternion& other);
140   Quaternion& operator=(const AngleAxisType& aa);
141   template<typename Derived>
142   Quaternion& operator=(const MatrixBase<Derived>& m);
143
144   /** \returns a quaternion representing an identity rotation
145     * \sa MatrixBase::Identity()
146     */
147   inline static Quaternion Identity() { return Quaternion(1, 0, 0, 0); }
148
149   /** \sa Quaternion::Identity(), MatrixBase::setIdentity()
150     */
151   inline Quaternion& setIdentity() { m_coeffs << 0, 0, 0, 1; return *this; }
152
153   /** \returns the squared norm of the quaternion's coefficients
154     * \sa Quaternion::norm(), MatrixBase::squaredNorm()
155     */
156   inline Scalar squaredNorm() const { return m_coeffs.squaredNorm(); }
157
158   /** \returns the norm of the quaternion's coefficients
159     * \sa Quaternion::squaredNorm(), MatrixBase::norm()
160     */
161   inline Scalar norm() const { return m_coeffs.norm(); }
162
163   /** Normalizes the quaternion \c *this
164     * \sa normalized(), MatrixBase::normalize() */
165   inline void normalize() { m_coeffs.normalize(); }
166   /** \returns a normalized version of \c *this
167     * \sa normalize(), MatrixBase::normalized() */
168   inline Quaternion normalized() const { return Quaternion(m_coeffs.normalized()); }
169
170   /** \returns the dot product of \c *this and \a other
171     * Geometrically speaking, the dot product of two unit quaternions
172     * corresponds to the cosine of half the angle between the two rotations.
173     * \sa angularDistance()
174     */
175   inline Scalar dot(const Quaternion& other) const { return m_coeffs.dot(other.m_coeffs); }
176
177   inline Scalar angularDistance(const Quaternion& other) const;
178
179   Matrix3 toRotationMatrix(void) const;
180
181   template<typename Derived1, typename Derived2>
182   Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
183
184   inline Quaternion operator* (const Quaternion& q) const;
185   inline Quaternion& operator*= (const Quaternion& q);
186
187   Quaternion inverse(void) const;
188   Quaternion conjugate(void) const;
189
190   Quaternion slerp(Scalar t, const Quaternion& other) const;
191
192   template<typename Derived>
193   Vector3 operator* (const MatrixBase<Derived>& vec) const;
194
195   /** \returns \c *this with scalar type casted to \a NewScalarType
196     *
197     * Note that if \a NewScalarType is equal to the current scalar type of \c *this
198     * then this function smartly returns a const reference to \c *this.
199     */
200   template<typename NewScalarType>
201   inline typename ei_cast_return_type<Quaternion,Quaternion<NewScalarType> >::type cast() const
202   { return typename ei_cast_return_type<Quaternion,Quaternion<NewScalarType> >::type(*this); }
203
204   /** Copy constructor with scalar type conversion */
205   template<typename OtherScalarType>
206   inline explicit Quaternion(const Quaternion<OtherScalarType>& other)
207   { m_coeffs = other.coeffs().template cast<Scalar>(); }
208
209   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
210     * determined by \a prec.
211     *
212     * \sa MatrixBase::isApprox() */
213   bool isApprox(const Quaternion& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
214   { return m_coeffs.isApprox(other.m_coeffs, prec); }
215
216 protected:
217   Coefficients m_coeffs;
218 };
219
220 /** \ingroup Geometry_Module
221   * single precision quaternion type */
222 typedef Quaternion<float> Quaternionf;
223 /** \ingroup Geometry_Module
224   * double precision quaternion type */
225 typedef Quaternion<double> Quaterniond;
226
227 // Generic Quaternion * Quaternion product
228 template<int Arch,typename Scalar> inline Quaternion<Scalar>
229 ei_quaternion_product(const Quaternion<Scalar>& a, const Quaternion<Scalar>& b)
230 {
231   return Quaternion<Scalar>
232   (
233     a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
234     a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
235     a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
236     a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
237   );
238 }
239
240 #ifdef EIGEN_VECTORIZE_SSE
241 template<> inline Quaternion<float>
242 ei_quaternion_product<EiArch_SSE,float>(const Quaternion<float>& _a, const Quaternion<float>& _b)
243 {
244   const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000));
245   Quaternion<float> res;
246   __m128 a = _a.coeffs().packet<Aligned>(0);
247   __m128 b = _b.coeffs().packet<Aligned>(0);
248   __m128 flip1 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,1,2,0,2),
249                                        ei_vec4f_swizzle1(b,2,0,1,2)),mask);
250   __m128 flip2 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,3,3,3,1),
251                                        ei_vec4f_swizzle1(b,0,1,2,1)),mask);
252   ei_pstore(&res.x(),
253             _mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,ei_vec4f_swizzle1(b,3,3,3,3)),
254                                   _mm_mul_ps(ei_vec4f_swizzle1(a,2,0,1,0),
255                                              ei_vec4f_swizzle1(b,1,2,0,0))),
256                        _mm_add_ps(flip1,flip2)));
257   return res;
258 }
259 #endif
260
261 /** \returns the concatenation of two rotations as a quaternion-quaternion product */
262 template <typename Scalar>
263 inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const
264 {
265   return ei_quaternion_product<EiArch>(*this,other);
266 }
267
268 /** \sa operator*(Quaternion) */
269 template <typename Scalar>
270 inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& other)
271 {
272   return (*this = *this * other);
273 }
274
275 /** Rotation of a vector by a quaternion.
276   * \remarks If the quaternion is used to rotate several points (>1)
277   * then it is much more efficient to first convert it to a 3x3 Matrix.
278   * Comparison of the operation cost for n transformations:
279   *   - Quaternion:    30n
280   *   - Via a Matrix3: 24 + 15n
281   */
282 template <typename Scalar>
283 template<typename Derived>
284 inline typename Quaternion<Scalar>::Vector3
285 Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
286 {
287     // Note that this algorithm comes from the optimization by hand
288     // of the conversion to a Matrix followed by a Matrix/Vector product.
289     // It appears to be much faster than the common algorithm found
290     // in the litterature (30 versus 39 flops). It also requires two
291     // Vector3 as temporaries.
292     Vector3 uv;
293     uv = 2 * this->vec().cross(v);
294     return v + this->w() * uv + this->vec().cross(uv);
295 }
296
297 template<typename Scalar>
298 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other)
299 {
300   m_coeffs = other.m_coeffs;
301   return *this;
302 }
303
304 /** Set \c *this from an angle-axis \a aa and returns a reference to \c *this
305   */
306 template<typename Scalar>
307 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa)
308 {
309   Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
310   this->w() = ei_cos(ha);
311   this->vec() = ei_sin(ha) * aa.axis();
312   return *this;
313 }
314
315 /** Set \c *this from the expression \a xpr:
316   *   - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion
317   *   - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
318   *     and \a xpr is converted to a quaternion
319   */
320 template<typename Scalar>
321 template<typename Derived>
322 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr)
323 {
324   ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived());
325   return *this;
326 }
327
328 /** Convert the quaternion to a 3x3 rotation matrix */
329 template<typename Scalar>
330 inline typename Quaternion<Scalar>::Matrix3
331 Quaternion<Scalar>::toRotationMatrix(void) const
332 {
333   // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
334   // if not inlined then the cost of the return by value is huge ~ +35%,
335   // however, not inlining this function is an order of magnitude slower, so
336   // it has to be inlined, and so the return by value is not an issue
337   Matrix3 res;
338
339   const Scalar tx  = 2*this->x();
340   const Scalar ty  = 2*this->y();
341   const Scalar tz  = 2*this->z();
342   const Scalar twx = tx*this->w();
343   const Scalar twy = ty*this->w();
344   const Scalar twz = tz*this->w();
345   const Scalar txx = tx*this->x();
346   const Scalar txy = ty*this->x();
347   const Scalar txz = tz*this->x();
348   const Scalar tyy = ty*this->y();
349   const Scalar tyz = tz*this->y();
350   const Scalar tzz = tz*this->z();
351
352   res.coeffRef(0,0) = 1-(tyy+tzz);
353   res.coeffRef(0,1) = txy-twz;
354   res.coeffRef(0,2) = txz+twy;
355   res.coeffRef(1,0) = txy+twz;
356   res.coeffRef(1,1) = 1-(txx+tzz);
357   res.coeffRef(1,2) = tyz-twx;
358   res.coeffRef(2,0) = txz-twy;
359   res.coeffRef(2,1) = tyz+twx;
360   res.coeffRef(2,2) = 1-(txx+tyy);
361
362   return res;
363 }
364
365 /** Sets *this to be a quaternion representing a rotation sending the vector \a a to the vector \a b.
366   *
367   * \returns a reference to *this.
368   *
369   * Note that the two input vectors do \b not have to be normalized.
370   */
371 template<typename Scalar>
372 template<typename Derived1, typename Derived2>
373 inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
374 {
375   Vector3 v0 = a.normalized();
376   Vector3 v1 = b.normalized();
377   Scalar c = v0.dot(v1);
378
379   // if dot == 1, vectors are the same
380   if (ei_isApprox(c,Scalar(1)))
381   {
382     // set to identity
383     this->w() = 1; this->vec().setZero();
384     return *this;
385   }
386   // if dot == -1, vectors are opposites
387   if (ei_isApprox(c,Scalar(-1)))
388   {
389     this->vec() = v0.unitOrthogonal();
390     this->w() = 0;
391     return *this;
392   }
393
394   Vector3 axis = v0.cross(v1);
395   Scalar s = ei_sqrt((Scalar(1)+c)*Scalar(2));
396   Scalar invs = Scalar(1)/s;
397   this->vec() = axis * invs;
398   this->w() = s * Scalar(0.5);
399
400   return *this;
401 }
402
403 /** \returns the multiplicative inverse of \c *this
404   * Note that in most cases, i.e., if you simply want the opposite rotation,
405   * and/or the quaternion is normalized, then it is enough to use the conjugate.
406   *
407   * \sa Quaternion::conjugate()
408   */
409 template <typename Scalar>
410 inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
411 {
412   // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
413   Scalar n2 = this->squaredNorm();
414   if (n2 > 0)
415     return Quaternion(conjugate().coeffs() / n2);
416   else
417   {
418     // return an invalid result to flag the error
419     return Quaternion(Coefficients::Zero());
420   }
421 }
422
423 /** \returns the conjugate of the \c *this which is equal to the multiplicative inverse
424   * if the quaternion is normalized.
425   * The conjugate of a quaternion represents the opposite rotation.
426   *
427   * \sa Quaternion::inverse()
428   */
429 template <typename Scalar>
430 inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const
431 {
432   return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
433 }
434
435 /** \returns the angle (in radian) between two rotations
436   * \sa dot()
437   */
438 template <typename Scalar>
439 inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other) const
440 {
441   double d = ei_abs(this->dot(other));
442   if (d>=1.0)
443     return 0;
444   return Scalar(2) * std::acos(d);
445 }
446
447 /** \returns the spherical linear interpolation between the two quaternions
448   * \c *this and \a other at the parameter \a t
449   */
450 template <typename Scalar>
451 Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) const
452 {
453   static const Scalar one = Scalar(1) - precision<Scalar>();
454   Scalar d = this->dot(other);
455   Scalar absD = ei_abs(d);
456   if (absD>=one)
457     return *this;
458
459   // theta is the angle between the 2 quaternions
460   Scalar theta = std::acos(absD);
461   Scalar sinTheta = ei_sin(theta);
462
463   Scalar scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
464   Scalar scale1 = ei_sin( ( t * theta) ) / sinTheta;
465   if (d<0)
466     scale1 = -scale1;
467
468   return Quaternion(scale0 * m_coeffs + scale1 * other.m_coeffs);
469 }
470
471 // set from a rotation matrix
472 template<typename Other>
473 struct ei_quaternion_assign_impl<Other,3,3>
474 {
475   typedef typename Other::Scalar Scalar;
476   inline static void run(Quaternion<Scalar>& q, const Other& mat)
477   {
478     // This algorithm comes from  "Quaternion Calculus and Fast Animation",
479     // Ken Shoemake, 1987 SIGGRAPH course notes
480     Scalar t = mat.trace();
481     if (t > 0)
482     {
483       t = ei_sqrt(t + Scalar(1.0));
484       q.w() = Scalar(0.5)*t;
485       t = Scalar(0.5)/t;
486       q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
487       q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
488       q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
489     }
490     else
491     {
492       int i = 0;
493       if (mat.coeff(1,1) > mat.coeff(0,0))
494         i = 1;
495       if (mat.coeff(2,2) > mat.coeff(i,i))
496         i = 2;
497       int j = (i+1)%3;
498       int k = (j+1)%3;
499
500       t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
501       q.coeffs().coeffRef(i) = Scalar(0.5) * t;
502       t = Scalar(0.5)/t;
503       q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
504       q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
505       q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
506     }
507   }
508 };
509
510 // set from a vector of coefficients assumed to be a quaternion
511 template<typename Other>
512 struct ei_quaternion_assign_impl<Other,4,1>
513 {
514   typedef typename Other::Scalar Scalar;
515   inline static void run(Quaternion<Scalar>& q, const Other& vec)
516   {
517     q.coeffs() = vec;
518   }
519 };
520
521 #endif // EIGEN_QUATERNION_H