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 {
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 
212 namespace internal {
213 template<typename _Scalar,int _Options>
214 struct traits<Quaternion<_Scalar,_Options> >
215 {
216  typedef Quaternion<_Scalar,_Options> PlainObject;
217  typedef _Scalar Scalar;
218  typedef Matrix<_Scalar,4,1,_Options> Coefficients;
219  enum{
220  IsAligned = internal::traits<Coefficients>::Flags & AlignedBit,
221  Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit
222  };
223 };
224 }
225 
226 template<typename _Scalar, int _Options>
227 class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> >
228 {
230  enum { IsAligned = internal::traits<Quaternion>::IsAligned };
231 
232 public:
233  typedef _Scalar Scalar;
234 
235  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion)
236  using Base::operator*=;
237 
238  typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
239  typedef typename Base::AngleAxisType AngleAxisType;
240 
242  inline Quaternion() {}
243 
251  inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){}
252 
254  inline Quaternion(const Scalar* data) : m_coeffs(data) {}
255 
257  template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
258 
260  explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
261 
266  template<typename Derived>
267  explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
268 
270  template<typename OtherScalar, int OtherOptions>
271  explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
272  { m_coeffs = other.coeffs().template cast<Scalar>(); }
273 
274  template<typename Derived1, typename Derived2>
275  static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
276 
277  inline Coefficients& coeffs() { return m_coeffs;}
278  inline const Coefficients& coeffs() const { return m_coeffs;}
279 
280  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
281 
282 protected:
283  Coefficients m_coeffs;
284 
285 #ifndef EIGEN_PARSED_BY_DOXYGEN
286  static EIGEN_STRONG_INLINE void _check_template_params()
287  {
288  EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
289  INVALID_MATRIX_TEMPLATE_PARAMETERS)
290  }
291 #endif
292 };
293 
300 
301 /***************************************************************************
302 * Specialization of Map<Quaternion<Scalar>>
303 ***************************************************************************/
304 
305 namespace internal {
306  template<typename _Scalar, int _Options>
307  struct traits<Map<Quaternion<_Scalar>, _Options> >:
308  traits<Quaternion<_Scalar, _Options> >
309  {
310  typedef _Scalar Scalar;
311  typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients;
312 
313  typedef traits<Quaternion<_Scalar, _Options> > TraitsBase;
314  enum {
315  IsAligned = TraitsBase::IsAligned,
316 
317  Flags = TraitsBase::Flags
318  };
319  };
320 }
321 
322 namespace internal {
323  template<typename _Scalar, int _Options>
324  struct traits<Map<const Quaternion<_Scalar>, _Options> >:
325  traits<Quaternion<_Scalar> >
326  {
327  typedef _Scalar Scalar;
328  typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
329 
330  typedef traits<Quaternion<_Scalar, _Options> > TraitsBase;
331  enum {
332  IsAligned = TraitsBase::IsAligned,
333  Flags = TraitsBase::Flags & ~LvalueBit
334  };
335  };
336 }
337 
348 template<typename _Scalar, int _Options>
349 class Map<const Quaternion<_Scalar>, _Options >
350  : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
351 {
353 
354  public:
355  typedef _Scalar Scalar;
356  typedef typename internal::traits<Map>::Coefficients Coefficients;
357  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
358  using Base::operator*=;
359 
366  EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
367 
368  inline const Coefficients& coeffs() const { return m_coeffs;}
369 
370  protected:
371  const Coefficients m_coeffs;
372 };
373 
384 template<typename _Scalar, int _Options>
385 class Map<Quaternion<_Scalar>, _Options >
386  : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> >
387 {
388  typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base;
389 
390  public:
391  typedef _Scalar Scalar;
392  typedef typename internal::traits<Map>::Coefficients Coefficients;
393  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
394  using Base::operator*=;
395 
402  EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
403 
404  inline Coefficients& coeffs() { return m_coeffs; }
405  inline const Coefficients& coeffs() const { return m_coeffs; }
406 
407  protected:
408  Coefficients m_coeffs;
409 };
410 
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.
430 namespace internal {
431 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product
432 {
433  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 
446 template <class Derived>
447 template <class OtherDerived>
448 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
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,
455  internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other);
456 }
457 
459 template <class Derived>
460 template <class OtherDerived>
461 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
462 {
463  derived() = derived() * other.derived();
464  return derived();
465 }
466 
474 template <class Derived>
475 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
477 {
478  // Note that this algorithm comes from the optimization by hand
479  // of the conversion to a Matrix followed by a Matrix/Vector product.
480  // It appears to be much faster than the common algorithm found
481  // in the litterature (30 versus 39 flops). It also requires two
482  // Vector3 as temporaries.
483  Vector3 uv = this->vec().cross(v);
484  uv += uv;
485  return v + this->w() * uv + this->vec().cross(uv);
486 }
487 
488 template<class Derived>
490 {
491  coeffs() = other.coeffs();
492  return derived();
493 }
494 
495 template<class Derived>
496 template<class OtherDerived>
497 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
498 {
499  coeffs() = other.coeffs();
500  return derived();
501 }
502 
505 template<class Derived>
506 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
507 {
508  Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
509  this->w() = internal::cos(ha);
510  this->vec() = internal::sin(ha) * aa.axis();
511  return derived();
512 }
513 
520 template<class Derived>
521 template<class MatrixDerived>
523 {
524  EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
525  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
526  internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
527  return derived();
528 }
529 
533 template<class Derived>
534 inline typename QuaternionBase<Derived>::Matrix3
536 {
537  // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
538  // if not inlined then the cost of the return by value is huge ~ +35%,
539  // however, not inlining this function is an order of magnitude slower, so
540  // it has to be inlined, and so the return by value is not an issue
541  Matrix3 res;
542 
543  const Scalar tx = Scalar(2)*this->x();
544  const Scalar ty = Scalar(2)*this->y();
545  const Scalar tz = Scalar(2)*this->z();
546  const Scalar twx = tx*this->w();
547  const Scalar twy = ty*this->w();
548  const Scalar twz = tz*this->w();
549  const Scalar txx = tx*this->x();
550  const Scalar txy = ty*this->x();
551  const Scalar txz = tz*this->x();
552  const Scalar tyy = ty*this->y();
553  const Scalar tyz = tz*this->y();
554  const Scalar tzz = tz*this->z();
555 
556  res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
557  res.coeffRef(0,1) = txy-twz;
558  res.coeffRef(0,2) = txz+twy;
559  res.coeffRef(1,0) = txy+twz;
560  res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
561  res.coeffRef(1,2) = tyz-twx;
562  res.coeffRef(2,0) = txz-twy;
563  res.coeffRef(2,1) = tyz+twx;
564  res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
565 
566  return res;
567 }
568 
579 template<class Derived>
580 template<typename Derived1, typename Derived2>
582 {
583  using std::max;
584  Vector3 v0 = a.normalized();
585  Vector3 v1 = b.normalized();
586  Scalar c = v1.dot(v0);
587 
588  // if dot == -1, vectors are nearly opposites
589  // => accuraletly compute the rotation axis by computing the
590  // intersection of the two planes. This is done by solving:
591  // x^T v0 = 0
592  // x^T v1 = 0
593  // under the constraint:
594  // ||x|| = 1
595  // which yields a singular value problem
596  if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
597  {
598  c = max<Scalar>(c,-1);
599  Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
601  Vector3 axis = svd.matrixV().col(2);
602 
603  Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
604  this->w() = internal::sqrt(w2);
605  this->vec() = axis * internal::sqrt(Scalar(1) - w2);
606  return derived();
607  }
608  Vector3 axis = v0.cross(v1);
609  Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2));
610  Scalar invs = Scalar(1)/s;
611  this->vec() = axis * invs;
612  this->w() = s * Scalar(0.5);
613 
614  return derived();
615 }
616 
617 
628 template<typename Scalar, int Options>
629 template<typename Derived1, typename Derived2>
631 {
632  Quaternion quat;
633  quat.setFromTwoVectors(a, b);
634  return quat;
635 }
636 
637 
644 template <class Derived>
646 {
647  // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
648  Scalar n2 = this->squaredNorm();
649  if (n2 > 0)
650  return Quaternion<Scalar>(conjugate().coeffs() / n2);
651  else
652  {
653  // return an invalid result to flag the error
654  return Quaternion<Scalar>(Coefficients::Zero());
655  }
656 }
657 
664 template <class Derived>
667 {
668  return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
669 }
670 
674 template <class Derived>
675 template <class OtherDerived>
676 inline typename internal::traits<Derived>::Scalar
678 {
679  using std::acos;
680  double d = internal::abs(this->dot(other));
681  if (d>=1.0)
682  return Scalar(0);
683  return static_cast<Scalar>(2 * acos(d));
684 }
685 
689 template <class Derived>
690 template <class OtherDerived>
693 {
694  using std::acos;
695  static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
696  Scalar d = this->dot(other);
697  Scalar absD = internal::abs(d);
698 
699  Scalar scale0;
700  Scalar scale1;
701 
702  if(absD>=one)
703  {
704  scale0 = Scalar(1) - t;
705  scale1 = t;
706  }
707  else
708  {
709  // theta is the angle between the 2 quaternions
710  Scalar theta = acos(absD);
711  Scalar sinTheta = internal::sin(theta);
712 
713  scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
714  scale1 = internal::sin( ( t * theta) ) / sinTheta;
715  }
716  if(d<0) scale1 = -scale1;
717 
718  return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
719 }
720 
721 namespace internal {
722 
723 // set from a rotation matrix
724 template<typename Other>
725 struct quaternionbase_assign_impl<Other,3,3>
726 {
727  typedef typename Other::Scalar Scalar;
728  typedef DenseIndex Index;
729  template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat)
730  {
731  // This algorithm comes from "Quaternion Calculus and Fast Animation",
732  // Ken Shoemake, 1987 SIGGRAPH course notes
733  Scalar t = mat.trace();
734  if (t > Scalar(0))
735  {
736  t = sqrt(t + Scalar(1.0));
737  q.w() = Scalar(0.5)*t;
738  t = Scalar(0.5)/t;
739  q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
740  q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
741  q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
742  }
743  else
744  {
745  DenseIndex i = 0;
746  if (mat.coeff(1,1) > mat.coeff(0,0))
747  i = 1;
748  if (mat.coeff(2,2) > mat.coeff(i,i))
749  i = 2;
750  DenseIndex j = (i+1)%3;
751  DenseIndex k = (j+1)%3;
752 
753  t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
754  q.coeffs().coeffRef(i) = Scalar(0.5) * t;
755  t = Scalar(0.5)/t;
756  q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
757  q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
758  q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
759  }
760  }
761 };
762 
763 // set from a vector of coefficients assumed to be a quaternion
764 template<typename Other>
765 struct quaternionbase_assign_impl<Other,4,1>
766 {
767  typedef typename Other::Scalar Scalar;
768  template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec)
769  {
770  q.coeffs() = vec;
771  }
772 };
773 
774 } // end namespace internal
775 
776 } // end namespace Eigen
777 
778 #endif // EIGEN_QUATERNION_H