1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
5// Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr>
6//
7// This Source Code Form is subject to the terms of the Mozilla
8// Public License v. 2.0. If a copy of the MPL was not distributed
9// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
10
11#ifndef EIGEN_QUATERNION_H
12#define EIGEN_QUATERNION_H
13namespace Eigen {
14
15
16/***************************************************************************
17* Definition of QuaternionBase<Derived>
18* The implementation is at the end of the file
19***************************************************************************/
20
21namespace internal {
22template<typename Other,
23 int OtherRows=Other::RowsAtCompileTime,
24 int OtherCols=Other::ColsAtCompileTime>
25struct quaternionbase_assign_impl;
26}
27
28/** \geometry_module \ingroup Geometry_Module
29 * \class QuaternionBase
30 * \brief Base class for quaternion expressions
31 * \tparam Derived derived type (CRTP)
32 * \sa class Quaternion
33 */
34template<class Derived>
35class QuaternionBase : public RotationBase<Derived, 3>
36{
37 public:
38 typedef RotationBase<Derived, 3> Base;
39
40 using Base::operator*;
41 using Base::derived;
42
43 typedef typename internal::traits<Derived>::Scalar Scalar;
44 typedef typename NumTraits<Scalar>::Real RealScalar;
45 typedef typename internal::traits<Derived>::Coefficients Coefficients;
46 typedef typename Coefficients::CoeffReturnType CoeffReturnType;
47 typedef typename internal::conditional<bool(internal::traits<Derived>::Flags&LvalueBit),
48 Scalar&, CoeffReturnType>::type NonConstCoeffReturnType;
49
50
51 enum {
52 Flags = Eigen::internal::traits<Derived>::Flags
53 };
54
55 // typedef typename Matrix<Scalar,4,1> Coefficients;
56 /** the type of a 3D vector */
57 typedef Matrix<Scalar,3,1> Vector3;
58 /** the equivalent rotation matrix type */
59 typedef Matrix<Scalar,3,3> Matrix3;
60 /** the equivalent angle-axis type */
61 typedef AngleAxis<Scalar> AngleAxisType;
62
63
64
65 /** \returns the \c x coefficient */
66 EIGEN_DEVICE_FUNC inline CoeffReturnType x() const { return this->derived().coeffs().coeff(0); }
67 /** \returns the \c y coefficient */
68 EIGEN_DEVICE_FUNC inline CoeffReturnType y() const { return this->derived().coeffs().coeff(1); }
69 /** \returns the \c z coefficient */
70 EIGEN_DEVICE_FUNC inline CoeffReturnType z() const { return this->derived().coeffs().coeff(2); }
71 /** \returns the \c w coefficient */
72 EIGEN_DEVICE_FUNC inline CoeffReturnType w() const { return this->derived().coeffs().coeff(3); }
73
74 /** \returns a reference to the \c x coefficient (if Derived is a non-const lvalue) */
75 EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType x() { return this->derived().coeffs().x(); }
76 /** \returns a reference to the \c y coefficient (if Derived is a non-const lvalue) */
77 EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType y() { return this->derived().coeffs().y(); }
78 /** \returns a reference to the \c z coefficient (if Derived is a non-const lvalue) */
79 EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType z() { return this->derived().coeffs().z(); }
80 /** \returns a reference to the \c w coefficient (if Derived is a non-const lvalue) */
81 EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType w() { return this->derived().coeffs().w(); }
82
83 /** \returns a read-only vector expression of the imaginary part (x,y,z) */
84 EIGEN_DEVICE_FUNC inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
85
86 /** \returns a vector expression of the imaginary part (x,y,z) */
87 EIGEN_DEVICE_FUNC inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); }
88
89 /** \returns a read-only vector expression of the coefficients (x,y,z,w) */
90 EIGEN_DEVICE_FUNC inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); }
91
92 /** \returns a vector expression of the coefficients (x,y,z,w) */
93 EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
94
95 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
96 template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
97
98// disabled this copy operator as it is giving very strange compilation errors when compiling
99// test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's
100// useful; however notice that we already have the templated operator= above and e.g. in MatrixBase
101// we didn't have to add, in addition to templated operator=, such a non-templated copy operator.
102// Derived& operator=(const QuaternionBase& other)
103// { return operator=<Derived>(other); }
104
105 EIGEN_DEVICE_FUNC Derived& operator=(const AngleAxisType& aa);
106 template<class OtherDerived> EIGEN_DEVICE_FUNC Derived& operator=(const MatrixBase<OtherDerived>& m);
107
108 /** \returns a quaternion representing an identity rotation
109 * \sa MatrixBase::Identity()
110 */
111 EIGEN_DEVICE_FUNC static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); }
112
113 /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity()
114 */
115 EIGEN_DEVICE_FUNC inline QuaternionBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1); return *this; }
116
117 /** \returns the squared norm of the quaternion's coefficients
118 * \sa QuaternionBase::norm(), MatrixBase::squaredNorm()
119 */
120 EIGEN_DEVICE_FUNC inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
121
122 /** \returns the norm of the quaternion's coefficients
123 * \sa QuaternionBase::squaredNorm(), MatrixBase::norm()
124 */
125 EIGEN_DEVICE_FUNC inline Scalar norm() const { return coeffs().norm(); }
126
127 /** Normalizes the quaternion \c *this
128 * \sa normalized(), MatrixBase::normalize() */
129 EIGEN_DEVICE_FUNC inline void normalize() { coeffs().normalize(); }
130 /** \returns a normalized copy of \c *this
131 * \sa normalize(), MatrixBase::normalized() */
132 EIGEN_DEVICE_FUNC inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
133
134 /** \returns the dot product of \c *this and \a other
135 * Geometrically speaking, the dot product of two unit quaternions
136 * corresponds to the cosine of half the angle between the two rotations.
137 * \sa angularDistance()
138 */
139 template<class OtherDerived> EIGEN_DEVICE_FUNC inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
140
141 template<class OtherDerived> EIGEN_DEVICE_FUNC Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
142
143 /** \returns an equivalent 3x3 rotation matrix */
144 EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix() const;
145
146 /** \returns the quaternion which transform \a a into \a b through a rotation */
147 template<typename Derived1, typename Derived2>
148 EIGEN_DEVICE_FUNC Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
149
150 template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
151 template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
152
153 /** \returns the quaternion describing the inverse rotation */
154 EIGEN_DEVICE_FUNC Quaternion<Scalar> inverse() const;
155
156 /** \returns the conjugated quaternion */
157 EIGEN_DEVICE_FUNC Quaternion<Scalar> conjugate() const;
158
159 template<class OtherDerived> EIGEN_DEVICE_FUNC Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const;
160
161 /** \returns \c true if \c *this is approximately equal to \a other, within the precision
162 * determined by \a prec.
163 *
164 * \sa MatrixBase::isApprox() */
165 template<class OtherDerived>
166 EIGEN_DEVICE_FUNC bool isApprox(const QuaternionBase<OtherDerived>& other, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
167 { return coeffs().isApprox(other.coeffs(), prec); }
168
169 /** return the result vector of \a v through the rotation*/
170 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const;
171
172 /** \returns \c *this with scalar type casted to \a NewScalarType
173 *
174 * Note that if \a NewScalarType is equal to the current scalar type of \c *this
175 * then this function smartly returns a const reference to \c *this.
176 */
177 template<typename NewScalarType>
178 EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const
179 {
180 return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived());
181 }
182
183#ifdef EIGEN_QUATERNIONBASE_PLUGIN
184# include EIGEN_QUATERNIONBASE_PLUGIN
185#endif
186};
187
188/***************************************************************************
189* Definition/implementation of Quaternion<Scalar>
190***************************************************************************/
191
192/** \geometry_module \ingroup Geometry_Module
193 *
194 * \class Quaternion
195 *
196 * \brief The quaternion class used to represent 3D orientations and rotations
197 *
198 * \tparam _Scalar the scalar type, i.e., the type of the coefficients
199 * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign.
200 *
201 * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of
202 * orientations and rotations of objects in three dimensions. Compared to other representations
203 * like Euler angles or 3x3 matrices, quaternions offer the following advantages:
204 * \li \b compact storage (4 scalars)
205 * \li \b efficient to compose (28 flops),
206 * \li \b stable spherical interpolation
207 *
208 * The following two typedefs are provided for convenience:
209 * \li \c Quaternionf for \c float
210 * \li \c Quaterniond for \c double
211 *
212 * \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized.
213 *
214 * \sa class AngleAxis, class Transform
215 */
216
217namespace internal {
218template<typename _Scalar,int _Options>
219struct traits<Quaternion<_Scalar,_Options> >
220{
221 typedef Quaternion<_Scalar,_Options> PlainObject;
222 typedef _Scalar Scalar;
223 typedef Matrix<_Scalar,4,1,_Options> Coefficients;
224 enum{
225 Alignment = internal::traits<Coefficients>::Alignment,
226 Flags = LvalueBit
227 };
228};
229}
230
231template<typename _Scalar, int _Options>
232class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> >
233{
234public:
235 typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base;
236 enum { NeedsAlignment = internal::traits<Quaternion>::Alignment>0 };
237
238 typedef _Scalar Scalar;
239
240 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion)
241 using Base::operator*=;
242
243 typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
244 typedef typename Base::AngleAxisType AngleAxisType;
245
246 /** Default constructor leaving the quaternion uninitialized. */
247 EIGEN_DEVICE_FUNC inline Quaternion() {}
248
249 /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
250 * its four coefficients \a w, \a x, \a y and \a z.
251 *
252 * \warning Note the order of the arguments: the real \a w coefficient first,
253 * while internally the coefficients are stored in the following order:
254 * [\c x, \c y, \c z, \c w]
255 */
256 EIGEN_DEVICE_FUNC inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){}
257
258 /** Constructs and initialize a quaternion from the array data */
259 EIGEN_DEVICE_FUNC explicit inline Quaternion(const Scalar* data) : m_coeffs(data) {}
260
261 /** Copy constructor */
262 template<class Derived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
263
264 /** Constructs and initializes a quaternion from the angle-axis \a aa */
265 EIGEN_DEVICE_FUNC explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
266
267 /** Constructs and initializes a quaternion from either:
268 * - a rotation matrix expression,
269 * - a 4D vector expression representing quaternion coefficients.
270 */
271 template<typename Derived>
272 EIGEN_DEVICE_FUNC explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
273
274 /** Explicit copy constructor with scalar conversion */
275 template<typename OtherScalar, int OtherOptions>
276 EIGEN_DEVICE_FUNC explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
277 { m_coeffs = other.coeffs().template cast<Scalar>(); }
278
279 EIGEN_DEVICE_FUNC static Quaternion UnitRandom();
280
281 template<typename Derived1, typename Derived2>
282 EIGEN_DEVICE_FUNC static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
283
284 EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs;}
285 EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs;}
286
287 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(NeedsAlignment))
288
289#ifdef EIGEN_QUATERNION_PLUGIN
290# include EIGEN_QUATERNION_PLUGIN
291#endif
292
293protected:
294 Coefficients m_coeffs;
295
296#ifndef EIGEN_PARSED_BY_DOXYGEN
297 static EIGEN_STRONG_INLINE void _check_template_params()
298 {
299 EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
300 INVALID_MATRIX_TEMPLATE_PARAMETERS)
301 }
302#endif
303};
304
305/** \ingroup Geometry_Module
306 * single precision quaternion type */
307typedef Quaternion<float> Quaternionf;
308/** \ingroup Geometry_Module
309 * double precision quaternion type */
310typedef Quaternion<double> Quaterniond;
311
312/***************************************************************************
313* Specialization of Map<Quaternion<Scalar>>
314***************************************************************************/
315
316namespace internal {
317 template<typename _Scalar, int _Options>
318 struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
319 {
320 typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients;
321 };
322}
323
324namespace internal {
325 template<typename _Scalar, int _Options>
326 struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
327 {
328 typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
329 typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase;
330 enum {
331 Flags = TraitsBase::Flags & ~LvalueBit
332 };
333 };
334}
335
336/** \ingroup Geometry_Module
337 * \brief Quaternion expression mapping a constant memory buffer
338 *
339 * \tparam _Scalar the type of the Quaternion coefficients
340 * \tparam _Options see class Map
341 *
342 * This is a specialization of class Map for Quaternion. This class allows to view
343 * a 4 scalar memory buffer as an Eigen's Quaternion object.
344 *
345 * \sa class Map, class Quaternion, class QuaternionBase
346 */
347template<typename _Scalar, int _Options>
348class Map<const Quaternion<_Scalar>, _Options >
349 : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
350{
351 public:
352 typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base;
353
354 typedef _Scalar Scalar;
355 typedef typename internal::traits<Map>::Coefficients Coefficients;
356 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
357 using Base::operator*=;
358
359 /** Constructs a Mapped Quaternion object from the pointer \a coeffs
360 *
361 * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
362 * \code *coeffs == {x, y, z, w} \endcode
363 *
364 * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
365 EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
366
367 EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs;}
368
369 protected:
370 const Coefficients m_coeffs;
371};
372
373/** \ingroup Geometry_Module
374 * \brief Expression of a quaternion from a memory buffer
375 *
376 * \tparam _Scalar the type of the Quaternion coefficients
377 * \tparam _Options see class Map
378 *
379 * This is a specialization of class Map for Quaternion. This class allows to view
380 * a 4 scalar memory buffer as an Eigen's Quaternion object.
381 *
382 * \sa class Map, class Quaternion, class QuaternionBase
383 */
384template<typename _Scalar, int _Options>
385class Map<Quaternion<_Scalar>, _Options >
386 : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> >
387{
388 public:
389 typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base;
390
391 typedef _Scalar Scalar;
392 typedef typename internal::traits<Map>::Coefficients Coefficients;
393 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
394 using Base::operator*=;
395
396 /** Constructs a Mapped Quaternion object from the pointer \a coeffs
397 *
398 * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
399 * \code *coeffs == {x, y, z, w} \endcode
400 *
401 * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
402 EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
403
404 EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; }
405 EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; }
406
407 protected:
408 Coefficients m_coeffs;
409};
410
411/** \ingroup Geometry_Module
412 * Map an unaligned array of single precision scalars as a quaternion */
413typedef Map<Quaternion<float>, 0> QuaternionMapf;
414/** \ingroup Geometry_Module
415 * Map an unaligned array of double precision scalars as a quaternion */
416typedef Map<Quaternion<double>, 0> QuaternionMapd;
417/** \ingroup Geometry_Module
418 * Map a 16-byte aligned array of single precision scalars as a quaternion */
419typedef Map<Quaternion<float>, Aligned> QuaternionMapAlignedf;
420/** \ingroup Geometry_Module
421 * Map a 16-byte aligned array of double precision scalars as a quaternion */
422typedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd;
423
424/***************************************************************************
425* Implementation of QuaternionBase methods
426***************************************************************************/
427
428// Generic Quaternion * Quaternion product
429// This product can be specialized for a given architecture via the Arch template argument.
430namespace internal {
431template<int Arch, class Derived1, class Derived2, typename Scalar> struct quat_product
432{
433 EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
434 return Quaternion<Scalar>
435 (
436 a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
437 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
438 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
439 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
440 );
441 }
442};
443}
444
445/** \returns the concatenation of two rotations as a quaternion-quaternion product */
446template <class Derived>
447template <class OtherDerived>
448EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
449QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const
450{
451 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
452 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
453 return internal::quat_product<Architecture::Target, Derived, OtherDerived,
454 typename internal::traits<Derived>::Scalar>::run(*this, other);
455}
456
457/** \sa operator*(Quaternion) */
458template <class Derived>
459template <class OtherDerived>
460EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
461{
462 derived() = derived() * other.derived();
463 return derived();
464}
465
466/** Rotation of a vector by a quaternion.
467 * \remarks If the quaternion is used to rotate several points (>1)
468 * then it is much more efficient to first convert it to a 3x3 Matrix.
469 * Comparison of the operation cost for n transformations:
470 * - Quaternion2: 30n
471 * - Via a Matrix3: 24 + 15n
472 */
473template <class Derived>
474EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
475QuaternionBase<Derived>::_transformVector(const Vector3& v) const
476{
477 // Note that this algorithm comes from the optimization by hand
478 // of the conversion to a Matrix followed by a Matrix/Vector product.
479 // It appears to be much faster than the common algorithm found
480 // in the literature (30 versus 39 flops). It also requires two
481 // Vector3 as temporaries.
482 Vector3 uv = this->vec().cross(v);
483 uv += uv;
484 return v + this->w() * uv + this->vec().cross(uv);
485}
486
487template<class Derived>
488EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other)
489{
490 coeffs() = other.coeffs();
491 return derived();
492}
493
494template<class Derived>
495template<class OtherDerived>
496EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
497{
498 coeffs() = other.coeffs();
499 return derived();
500}
501
502/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this
503 */
504template<class Derived>
505EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
506{
507 EIGEN_USING_STD_MATH(cos)
508 EIGEN_USING_STD_MATH(sin)
509 Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
510 this->w() = cos(ha);
511 this->vec() = sin(ha) * aa.axis();
512 return derived();
513}
514
515/** Set \c *this from the expression \a xpr:
516 * - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion
517 * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
518 * and \a xpr is converted to a quaternion
519 */
520
521template<class Derived>
522template<class MatrixDerived>
523EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr)
524{
525 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
526 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
527 internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
528 return derived();
529}
530
531/** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to
532 * be normalized, otherwise the result is undefined.
533 */
534template<class Derived>
535EIGEN_DEVICE_FUNC inline typename QuaternionBase<Derived>::Matrix3
536QuaternionBase<Derived>::toRotationMatrix(void) const
537{
538 // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
539 // if not inlined then the cost of the return by value is huge ~ +35%,
540 // however, not inlining this function is an order of magnitude slower, so
541 // it has to be inlined, and so the return by value is not an issue
542 Matrix3 res;
543
544 const Scalar tx = Scalar(2)*this->x();
545 const Scalar ty = Scalar(2)*this->y();
546 const Scalar tz = Scalar(2)*this->z();
547 const Scalar twx = tx*this->w();
548 const Scalar twy = ty*this->w();
549 const Scalar twz = tz*this->w();
550 const Scalar txx = tx*this->x();
551 const Scalar txy = ty*this->x();
552 const Scalar txz = tz*this->x();
553 const Scalar tyy = ty*this->y();
554 const Scalar tyz = tz*this->y();
555 const Scalar tzz = tz*this->z();
556
557 res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
558 res.coeffRef(0,1) = txy-twz;
559 res.coeffRef(0,2) = txz+twy;
560 res.coeffRef(1,0) = txy+twz;
561 res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
562 res.coeffRef(1,2) = tyz-twx;
563 res.coeffRef(2,0) = txz-twy;
564 res.coeffRef(2,1) = tyz+twx;
565 res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
566
567 return res;
568}
569
570/** Sets \c *this to be a quaternion representing a rotation between
571 * the two arbitrary vectors \a a and \a b. In other words, the built
572 * rotation represent a rotation sending the line of direction \a a
573 * to the line of direction \a b, both lines passing through the origin.
574 *
575 * \returns a reference to \c *this.
576 *
577 * Note that the two input vectors do \b not have to be normalized, and
578 * do not need to have the same norm.
579 */
580template<class Derived>
581template<typename Derived1, typename Derived2>
582EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
583{
584 EIGEN_USING_STD_MATH(sqrt)
585 Vector3 v0 = a.normalized();
586 Vector3 v1 = b.normalized();
587 Scalar c = v1.dot(v0);
588
589 // if dot == -1, vectors are nearly opposites
590 // => accurately compute the rotation axis by computing the
591 // intersection of the two planes. This is done by solving:
592 // x^T v0 = 0
593 // x^T v1 = 0
594 // under the constraint:
595 // ||x|| = 1
596 // which yields a singular value problem
597 if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
598 {
599 c = numext::maxi(c,Scalar(-1));
600 Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
601 JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
602 Vector3 axis = svd.matrixV().col(2);
603
604 Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
605 this->w() = sqrt(w2);
606 this->vec() = axis * sqrt(Scalar(1) - w2);
607 return derived();
608 }
609 Vector3 axis = v0.cross(v1);
610 Scalar s = sqrt((Scalar(1)+c)*Scalar(2));
611 Scalar invs = Scalar(1)/s;
612 this->vec() = axis * invs;
613 this->w() = s * Scalar(0.5);
614
615 return derived();
616}
617
618/** \returns a random unit quaternion following a uniform distribution law on SO(3)
619 *
620 * \note The implementation is based on http://planning.cs.uiuc.edu/node198.html
621 */
622template<typename Scalar, int Options>
623EIGEN_DEVICE_FUNC Quaternion<Scalar,Options> Quaternion<Scalar,Options>::UnitRandom()
624{
625 EIGEN_USING_STD_MATH(sqrt)
626 EIGEN_USING_STD_MATH(sin)
627 EIGEN_USING_STD_MATH(cos)
628 const Scalar u1 = internal::random<Scalar>(0, 1),
629 u2 = internal::random<Scalar>(0, 2*EIGEN_PI),
630 u3 = internal::random<Scalar>(0, 2*EIGEN_PI);
631 const Scalar a = sqrt(1 - u1),
632 b = sqrt(u1);
633 return Quaternion (a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3));
634}
635
636
637/** Returns a quaternion representing a rotation between
638 * the two arbitrary vectors \a a and \a b. In other words, the built
639 * rotation represent a rotation sending the line of direction \a a
640 * to the line of direction \a b, both lines passing through the origin.
641 *
642 * \returns resulting quaternion
643 *
644 * Note that the two input vectors do \b not have to be normalized, and
645 * do not need to have the same norm.
646 */
647template<typename Scalar, int Options>
648template<typename Derived1, typename Derived2>
649EIGEN_DEVICE_FUNC Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
650{
651 Quaternion quat;
652 quat.setFromTwoVectors(a, b);
653 return quat;
654}
655
656
657/** \returns the multiplicative inverse of \c *this
658 * Note that in most cases, i.e., if you simply want the opposite rotation,
659 * and/or the quaternion is normalized, then it is enough to use the conjugate.
660 *
661 * \sa QuaternionBase::conjugate()
662 */
663template <class Derived>
664EIGEN_DEVICE_FUNC inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const
665{
666 // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
667 Scalar n2 = this->squaredNorm();
668 if (n2 > Scalar(0))
669 return Quaternion<Scalar>(conjugate().coeffs() / n2);
670 else
671 {
672 // return an invalid result to flag the error
673 return Quaternion<Scalar>(Coefficients::Zero());
674 }
675}
676
677// Generic conjugate of a Quaternion
678namespace internal {
679template<int Arch, class Derived, typename Scalar> struct quat_conj
680{
681 EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived>& q){
682 return Quaternion<Scalar>(q.w(),-q.x(),-q.y(),-q.z());
683 }
684};
685}
686
687/** \returns the conjugate of the \c *this which is equal to the multiplicative inverse
688 * if the quaternion is normalized.
689 * The conjugate of a quaternion represents the opposite rotation.
690 *
691 * \sa Quaternion2::inverse()
692 */
693template <class Derived>
694EIGEN_DEVICE_FUNC inline Quaternion<typename internal::traits<Derived>::Scalar>
695QuaternionBase<Derived>::conjugate() const
696{
697 return internal::quat_conj<Architecture::Target, Derived,
698 typename internal::traits<Derived>::Scalar>::run(*this);
699
700}
701
702/** \returns the angle (in radian) between two rotations
703 * \sa dot()
704 */
705template <class Derived>
706template <class OtherDerived>
707EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar
708QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const
709{
710 EIGEN_USING_STD_MATH(atan2)
711 Quaternion<Scalar> d = (*this) * other.conjugate();
712 return Scalar(2) * atan2( d.vec().norm(), numext::abs(d.w()) );
713}
714
715
716
717/** \returns the spherical linear interpolation between the two quaternions
718 * \c *this and \a other at the parameter \a t in [0;1].
719 *
720 * This represents an interpolation for a constant motion between \c *this and \a other,
721 * see also http://en.wikipedia.org/wiki/Slerp.
722 */
723template <class Derived>
724template <class OtherDerived>
725EIGEN_DEVICE_FUNC Quaternion<typename internal::traits<Derived>::Scalar>
726QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const
727{
728 EIGEN_USING_STD_MATH(acos)
729 EIGEN_USING_STD_MATH(sin)
730 const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
731 Scalar d = this->dot(other);
732 Scalar absD = numext::abs(d);
733
734 Scalar scale0;
735 Scalar scale1;
736
737 if(absD>=one)
738 {
739 scale0 = Scalar(1) - t;
740 scale1 = t;
741 }
742 else
743 {
744 // theta is the angle between the 2 quaternions
745 Scalar theta = acos(absD);
746 Scalar sinTheta = sin(theta);
747
748 scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;
749 scale1 = sin( ( t * theta) ) / sinTheta;
750 }
751 if(d<Scalar(0)) scale1 = -scale1;
752
753 return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
754}
755
756namespace internal {
757
758// set from a rotation matrix
759template<typename Other>
760struct quaternionbase_assign_impl<Other,3,3>
761{
762 typedef typename Other::Scalar Scalar;
763 template<class Derived> EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& a_mat)
764 {
765 const typename internal::nested_eval<Other,2>::type mat(a_mat);
766 EIGEN_USING_STD_MATH(sqrt)
767 // This algorithm comes from "Quaternion Calculus and Fast Animation",
768 // Ken Shoemake, 1987 SIGGRAPH course notes
769 Scalar t = mat.trace();
770 if (t > Scalar(0))
771 {
772 t = sqrt(t + Scalar(1.0));
773 q.w() = Scalar(0.5)*t;
774 t = Scalar(0.5)/t;
775 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
776 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
777 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
778 }
779 else
780 {
781 Index i = 0;
782 if (mat.coeff(1,1) > mat.coeff(0,0))
783 i = 1;
784 if (mat.coeff(2,2) > mat.coeff(i,i))
785 i = 2;
786 Index j = (i+1)%3;
787 Index k = (j+1)%3;
788
789 t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
790 q.coeffs().coeffRef(i) = Scalar(0.5) * t;
791 t = Scalar(0.5)/t;
792 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
793 q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
794 q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
795 }
796 }
797};
798
799// set from a vector of coefficients assumed to be a quaternion
800template<typename Other>
801struct quaternionbase_assign_impl<Other,4,1>
802{
803 typedef typename Other::Scalar Scalar;
804 template<class Derived> EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& vec)
805 {
806 q.coeffs() = vec;
807 }
808};
809
810} // end namespace internal
811
812} // end namespace Eigen
813
814#endif // EIGEN_QUATERNION_H
815