Quaternion.h
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
13 namespace Eigen {
14 
15 
16 /***************************************************************************
17 * Definition of QuaternionBase<Derived>
18 * The implementation is at the end of the file
19 ***************************************************************************/
20 
21 namespace internal {
22 template<typename Other,
23  int OtherRows=Other::RowsAtCompileTime,
24  int OtherCols=Other::ColsAtCompileTime>
25 struct quaternionbase_assign_impl;
26 }
27 
34 template<class Derived>
35 class QuaternionBase : public RotationBase<Derived, 3>
36 {
37  typedef RotationBase<Derived, 3> Base;
38 public:
39  using Base::operator*;
40  using Base::derived;
41 
42  typedef typename internal::traits<Derived>::Scalar Scalar;
43  typedef typename NumTraits<Scalar>::Real RealScalar;
44  typedef typename internal::traits<Derived>::Coefficients Coefficients;
45  enum {
46  Flags = Eigen::internal::traits<Derived>::Flags
47  };
48 
49  // typedef typename Matrix<Scalar,4,1> Coefficients;
56 
57 
58 
60  inline Scalar x() const { return this->derived().coeffs().coeff(0); }
62  inline Scalar y() const { return this->derived().coeffs().coeff(1); }
64  inline Scalar z() const { return this->derived().coeffs().coeff(2); }
66  inline Scalar w() const { return this->derived().coeffs().coeff(3); }
67 
69  inline Scalar& x() { return this->derived().coeffs().coeffRef(0); }
71  inline Scalar& y() { return this->derived().coeffs().coeffRef(1); }
73  inline Scalar& z() { return this->derived().coeffs().coeffRef(2); }
75  inline Scalar& w() { return this->derived().coeffs().coeffRef(3); }
76 
78  inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
79 
81  inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); }
82 
84  inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); }
85 
87  inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
88 
89  EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
90  template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
91 
92 // disabled this copy operator as it is giving very strange compilation errors when compiling
93 // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's
94 // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase
95 // we didn't have to add, in addition to templated operator=, such a non-templated copy operator.
96 // Derived& operator=(const QuaternionBase& other)
97 // { return operator=<Derived>(other); }
98 
99  Derived& operator=(const AngleAxisType& aa);
100  template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m);
101 
105  static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); }
106 
109  inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; }
110 
114  inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
115 
119  inline Scalar norm() const { return coeffs().norm(); }
120 
123  inline void normalize() { coeffs().normalize(); }
126  inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
127 
133  template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
134 
135  template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
136 
138  Matrix3 toRotationMatrix() const;
139 
141  template<typename Derived1, typename Derived2>
142  Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
143 
144  template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
145  template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
146 
148  Quaternion<Scalar> inverse() const;
149 
152 
157  template<class OtherDerived> Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const;
158 
163  template<class OtherDerived>
164  bool isApprox(const QuaternionBase<OtherDerived>& other, RealScalar prec = NumTraits<Scalar>::dummy_precision()) const
165  { return coeffs().isApprox(other.coeffs(), prec); }
166 
168  EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const;
169 
175  template<typename NewScalarType>
176  inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const
177  {
178  return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived());
179  }
180 
181 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
182 # include EIGEN_QUATERNIONBASE_PLUGIN
183 #endif
184 };
185 
186 /***************************************************************************
187 * Definition/implementation of Quaternion<Scalar>
188 ***************************************************************************/
189 
213 namespace internal {
214 template<typename _Scalar,int _Options>
215 struct traits<Quaternion<_Scalar,_Options> >
216 {
217  typedef Quaternion<_Scalar,_Options> PlainObject;
218  typedef _Scalar Scalar;
219  typedef Matrix<_Scalar,4,1,_Options> Coefficients;
220  enum{
221  IsAligned = internal::traits<Coefficients>::Flags & AlignedBit,
222  Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit
223  };
224 };
225 }
226 
227 template<typename _Scalar, int _Options>
228 class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> >
229 {
230  typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base;
231  enum { IsAligned = internal::traits<Quaternion>::IsAligned };
232 
233 public:
234  typedef _Scalar Scalar;
235 
236  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion)
237  using Base::operator*=;
238 
239  typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
240  typedef typename Base::AngleAxisType AngleAxisType;
241 
243  inline Quaternion() {}
244 
252  inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){}
253 
255  inline Quaternion(const Scalar* data) : m_coeffs(data) {}
256 
258  template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
259 
261  explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
262 
267  template<typename Derived>
268  explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
269 
271  template<typename OtherScalar, int OtherOptions>
272  explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
273  { m_coeffs = other.coeffs().template cast<Scalar>(); }
274 
275  template<typename Derived1, typename Derived2>
276  static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
277 
278  inline Coefficients& coeffs() { return m_coeffs;}
279  inline const Coefficients& coeffs() const { return m_coeffs;}
280 
281  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
282 
283 protected:
284  Coefficients m_coeffs;
285 
286 #ifndef EIGEN_PARSED_BY_DOXYGEN
287  static EIGEN_STRONG_INLINE void _check_template_params()
288  {
289  EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
290  INVALID_MATRIX_TEMPLATE_PARAMETERS)
291  }
292 #endif
293 };
294 
301 
302 /***************************************************************************
303 * Specialization of Map<Quaternion<Scalar>>
304 ***************************************************************************/
305 
306 namespace internal {
307  template<typename _Scalar, int _Options>
308  struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
309  {
310  typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients;
311  };
312 }
313 
314 namespace internal {
315  template<typename _Scalar, int _Options>
316  struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
317  {
318  typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
319  typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase;
320  enum {
321  Flags = TraitsBase::Flags & ~LvalueBit
322  };
323  };
324 }
325 
337 template<typename _Scalar, int _Options>
338 class Map<const Quaternion<_Scalar>, _Options >
339  : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
340 {
342 
343  public:
344  typedef _Scalar Scalar;
345  typedef typename internal::traits<Map>::Coefficients Coefficients;
346  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
347  using Base::operator*=;
348 
355  EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
356 
357  inline const Coefficients& coeffs() const { return m_coeffs;}
358 
359  protected:
360  const Coefficients m_coeffs;
361 };
362 
374 template<typename _Scalar, int _Options>
375 class Map<Quaternion<_Scalar>, _Options >
376  : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> >
377 {
378  typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base;
379 
380  public:
381  typedef _Scalar Scalar;
382  typedef typename internal::traits<Map>::Coefficients Coefficients;
383  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
384  using Base::operator*=;
385 
392  EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
393 
394  inline Coefficients& coeffs() { return m_coeffs; }
395  inline const Coefficients& coeffs() const { return m_coeffs; }
396 
397  protected:
398  Coefficients m_coeffs;
399 };
400 
413 
414 /***************************************************************************
415 * Implementation of QuaternionBase methods
416 ***************************************************************************/
417 
418 // Generic Quaternion * Quaternion product
419 // This product can be specialized for a given architecture via the Arch template argument.
420 namespace internal {
421 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product
422 {
423  static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
424  return Quaternion<Scalar>
425  (
426  a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
427  a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
428  a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
429  a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
430  );
431  }
432 };
433 }
434 
436 template <class Derived>
437 template <class OtherDerived>
438 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
440 {
441  EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
442  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
443  return internal::quat_product<Architecture::Target, Derived, OtherDerived,
444  typename internal::traits<Derived>::Scalar,
445  internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other);
446 }
447 
449 template <class Derived>
450 template <class OtherDerived>
451 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
452 {
453  derived() = derived() * other.derived();
454  return derived();
455 }
456 
464 template <class Derived>
465 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
467 {
468  // Note that this algorithm comes from the optimization by hand
469  // of the conversion to a Matrix followed by a Matrix/Vector product.
470  // It appears to be much faster than the common algorithm found
471  // in the litterature (30 versus 39 flops). It also requires two
472  // Vector3 as temporaries.
473  Vector3 uv = this->vec().cross(v);
474  uv += uv;
475  return v + this->w() * uv + this->vec().cross(uv);
476 }
477 
478 template<class Derived>
480 {
481  coeffs() = other.coeffs();
482  return derived();
483 }
484 
485 template<class Derived>
486 template<class OtherDerived>
487 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
488 {
489  coeffs() = other.coeffs();
490  return derived();
491 }
492 
495 template<class Derived>
496 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
497 {
498  Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
499  this->w() = internal::cos(ha);
500  this->vec() = internal::sin(ha) * aa.axis();
501  return derived();
502 }
503 
510 template<class Derived>
511 template<class MatrixDerived>
513 {
514  EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
515  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
516  internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
517  return derived();
518 }
519 
523 template<class Derived>
524 inline typename QuaternionBase<Derived>::Matrix3
526 {
527  // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
528  // if not inlined then the cost of the return by value is huge ~ +35%,
529  // however, not inlining this function is an order of magnitude slower, so
530  // it has to be inlined, and so the return by value is not an issue
531  Matrix3 res;
532 
533  const Scalar tx = Scalar(2)*this->x();
534  const Scalar ty = Scalar(2)*this->y();
535  const Scalar tz = Scalar(2)*this->z();
536  const Scalar twx = tx*this->w();
537  const Scalar twy = ty*this->w();
538  const Scalar twz = tz*this->w();
539  const Scalar txx = tx*this->x();
540  const Scalar txy = ty*this->x();
541  const Scalar txz = tz*this->x();
542  const Scalar tyy = ty*this->y();
543  const Scalar tyz = tz*this->y();
544  const Scalar tzz = tz*this->z();
545 
546  res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
547  res.coeffRef(0,1) = txy-twz;
548  res.coeffRef(0,2) = txz+twy;
549  res.coeffRef(1,0) = txy+twz;
550  res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
551  res.coeffRef(1,2) = tyz-twx;
552  res.coeffRef(2,0) = txz-twy;
553  res.coeffRef(2,1) = tyz+twx;
554  res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
555 
556  return res;
557 }
558 
569 template<class Derived>
570 template<typename Derived1, typename Derived2>
572 {
573  using std::max;
574  Vector3 v0 = a.normalized();
575  Vector3 v1 = b.normalized();
576  Scalar c = v1.dot(v0);
577 
578  // if dot == -1, vectors are nearly opposites
579  // => accuraletly compute the rotation axis by computing the
580  // intersection of the two planes. This is done by solving:
581  // x^T v0 = 0
582  // x^T v1 = 0
583  // under the constraint:
584  // ||x|| = 1
585  // which yields a singular value problem
586  if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
587  {
588  c = max<Scalar>(c,-1);
589  Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
591  Vector3 axis = svd.matrixV().col(2);
592 
593  Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
594  this->w() = internal::sqrt(w2);
595  this->vec() = axis * internal::sqrt(Scalar(1) - w2);
596  return derived();
597  }
598  Vector3 axis = v0.cross(v1);
599  Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2));
600  Scalar invs = Scalar(1)/s;
601  this->vec() = axis * invs;
602  this->w() = s * Scalar(0.5);
603 
604  return derived();
605 }
606 
607 
618 template<typename Scalar, int Options>
619 template<typename Derived1, typename Derived2>
621 {
622  Quaternion quat;
623  quat.setFromTwoVectors(a, b);
624  return quat;
625 }
626 
627 
634 template <class Derived>
636 {
637  // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
638  Scalar n2 = this->squaredNorm();
639  if (n2 > 0)
640  return Quaternion<Scalar>(conjugate().coeffs() / n2);
641  else
642  {
643  // return an invalid result to flag the error
644  return Quaternion<Scalar>(Coefficients::Zero());
645  }
646 }
647 
654 template <class Derived>
657 {
658  return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
659 }
660 
664 template <class Derived>
665 template <class OtherDerived>
666 inline typename internal::traits<Derived>::Scalar
668 {
669  using std::acos;
670  double d = internal::abs(this->dot(other));
671  if (d>=1.0)
672  return Scalar(0);
673  return static_cast<Scalar>(2 * acos(d));
674 }
675 
679 template <class Derived>
680 template <class OtherDerived>
683 {
684  using std::acos;
685  static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
686  Scalar d = this->dot(other);
687  Scalar absD = internal::abs(d);
688 
689  Scalar scale0;
690  Scalar scale1;
691 
692  if(absD>=one)
693  {
694  scale0 = Scalar(1) - t;
695  scale1 = t;
696  }
697  else
698  {
699  // theta is the angle between the 2 quaternions
700  Scalar theta = acos(absD);
701  Scalar sinTheta = internal::sin(theta);
702 
703  scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
704  scale1 = internal::sin( ( t * theta) ) / sinTheta;
705  }
706  if(d<0) scale1 = -scale1;
707 
708  return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
709 }
710 
711 namespace internal {
712 
713 // set from a rotation matrix
714 template<typename Other>
715 struct quaternionbase_assign_impl<Other,3,3>
716 {
717  typedef typename Other::Scalar Scalar;
718  typedef DenseIndex Index;
719  template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat)
720  {
721  // This algorithm comes from "Quaternion Calculus and Fast Animation",
722  // Ken Shoemake, 1987 SIGGRAPH course notes
723  Scalar t = mat.trace();
724  if (t > Scalar(0))
725  {
726  t = sqrt(t + Scalar(1.0));
727  q.w() = Scalar(0.5)*t;
728  t = Scalar(0.5)/t;
729  q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
730  q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
731  q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
732  }
733  else
734  {
735  DenseIndex i = 0;
736  if (mat.coeff(1,1) > mat.coeff(0,0))
737  i = 1;
738  if (mat.coeff(2,2) > mat.coeff(i,i))
739  i = 2;
740  DenseIndex j = (i+1)%3;
741  DenseIndex k = (j+1)%3;
742 
743  t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
744  q.coeffs().coeffRef(i) = Scalar(0.5) * t;
745  t = Scalar(0.5)/t;
746  q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
747  q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
748  q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
749  }
750  }
751 };
752 
753 // set from a vector of coefficients assumed to be a quaternion
754 template<typename Other>
755 struct quaternionbase_assign_impl<Other,4,1>
756 {
757  typedef typename Other::Scalar Scalar;
758  template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec)
759  {
760  q.coeffs() = vec;
761  }
762 };
763 
764 } // end namespace internal
765 
766 } // end namespace Eigen
767 
768 #endif // EIGEN_QUATERNION_H