Quaternion.h
Go to the documentation of this file.
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 // Eigen is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU Lesser General Public
9 // License as published by the Free Software Foundation; either
10 // version 3 of the License, or (at your option) any later version.
11 //
12 // Alternatively, you can redistribute it and/or
13 // modify it under the terms of the GNU General Public License as
14 // published by the Free Software Foundation; either version 2 of
15 // the License, or (at your option) any later version.
16 //
17 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
18 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
19 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
20 // GNU General Public License for more details.
21 //
22 // You should have received a copy of the GNU Lesser General Public
23 // License and a copy of the GNU General Public License along with
24 // Eigen. If not, see <http://www.gnu.org/licenses/>.
25 
26 #ifndef EIGEN_QUATERNION_H
27 #define EIGEN_QUATERNION_H
28 namespace Eigen {
29 
30 
31 /***************************************************************************
32 * Definition of QuaternionBase<Derived>
33 * The implementation is at the end of the file
34 ***************************************************************************/
35 
36 namespace internal {
37 template<typename Other,
38  int OtherRows=Other::RowsAtCompileTime,
39  int OtherCols=Other::ColsAtCompileTime>
40 struct quaternionbase_assign_impl;
41 }
42 
49 template<class Derived>
50 class QuaternionBase : public RotationBase<Derived, 3>
51 {
53 public:
54  using Base::operator*;
55  using Base::derived;
56 
57  typedef typename internal::traits<Derived>::Scalar Scalar;
59  typedef typename internal::traits<Derived>::Coefficients Coefficients;
60  enum {
61  Flags = Eigen::internal::traits<Derived>::Flags
62  };
63 
64  // typedef typename Matrix<Scalar,4,1> Coefficients;
71 
72 
73 
75  inline Scalar x() const { return this->derived().coeffs().coeff(0); }
77  inline Scalar y() const { return this->derived().coeffs().coeff(1); }
79  inline Scalar z() const { return this->derived().coeffs().coeff(2); }
81  inline Scalar w() const { return this->derived().coeffs().coeff(3); }
82 
84  inline Scalar& x() { return this->derived().coeffs().coeffRef(0); }
86  inline Scalar& y() { return this->derived().coeffs().coeffRef(1); }
88  inline Scalar& z() { return this->derived().coeffs().coeffRef(2); }
90  inline Scalar& w() { return this->derived().coeffs().coeffRef(3); }
91 
93  inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
94 
96  inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); }
97 
99  inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); }
100 
102  inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
103 
105  template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
106 
107 // disabled this copy operator as it is giving very strange compilation errors when compiling
108 // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's
109 // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase
110 // we didn't have to add, in addition to templated operator=, such a non-templated copy operator.
111 // Derived& operator=(const QuaternionBase& other)
112 // { return operator=<Derived>(other); }
113 
114  Derived& operator=(const AngleAxisType& aa);
115  template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m);
116 
120  static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); }
121 
124  inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; }
125 
129  inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
130 
134  inline Scalar norm() const { return coeffs().norm(); }
135 
138  inline void normalize() { coeffs().normalize(); }
141  inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
142 
148  template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
149 
150  template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
151 
153  Matrix3 toRotationMatrix() const;
154 
156  template<typename Derived1, typename Derived2>
157  Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
158 
159  template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
160  template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
161 
163  Quaternion<Scalar> inverse() const;
164 
167 
172  template<class OtherDerived> Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const;
173 
178  template<class OtherDerived>
180  { return coeffs().isApprox(other.coeffs(), prec); }
181 
184 
190  template<typename NewScalarType>
191  inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const
192  {
193  return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived());
194  }
195 
196 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
197 # include EIGEN_QUATERNIONBASE_PLUGIN
198 #endif
199 };
200 
201 /***************************************************************************
202 * Definition/implementation of Quaternion<Scalar>
203 ***************************************************************************/
204 
227 namespace internal {
228 template<typename _Scalar,int _Options>
229 struct traits<Quaternion<_Scalar,_Options> >
230 {
231  typedef Quaternion<_Scalar,_Options> PlainObject;
232  typedef _Scalar Scalar;
233  typedef Matrix<_Scalar,4,1,_Options> Coefficients;
234  enum{
235  IsAligned = internal::traits<Coefficients>::Flags & AlignedBit,
236  Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit
237  };
238 };
239 }
240 
241 template<typename _Scalar, int _Options>
242 class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> >
243 {
245  enum { IsAligned = internal::traits<Quaternion>::IsAligned };
246 
247 public:
248  typedef _Scalar Scalar;
249 
251  using Base::operator*=;
252 
253  typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
254  typedef typename Base::AngleAxisType AngleAxisType;
255 
257  inline Quaternion() {}
258 
266  inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){}
267 
269  inline Quaternion(const Scalar* data) : m_coeffs(data) {}
270 
272  template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
273 
275  explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
276 
281  template<typename Derived>
282  explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
283 
285  template<typename OtherScalar, int OtherOptions>
286  explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
287  { m_coeffs = other.coeffs().template cast<Scalar>(); }
288 
289  template<typename Derived1, typename Derived2>
290  static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
291 
292  inline Coefficients& coeffs() { return m_coeffs;}
293  inline const Coefficients& coeffs() const { return m_coeffs;}
294 
296 
297 protected:
298  Coefficients m_coeffs;
299 
300 #ifndef EIGEN_PARSED_BY_DOXYGEN
301  static EIGEN_STRONG_INLINE void _check_template_params()
302  {
303  EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
304  INVALID_MATRIX_TEMPLATE_PARAMETERS)
305  }
306 #endif
307 };
308 
315 
316 /***************************************************************************
317 * Specialization of Map<Quaternion<Scalar>>
318 ***************************************************************************/
319 
320 namespace internal {
321  template<typename _Scalar, int _Options>
322  struct traits<Map<Quaternion<_Scalar>, _Options> >:
323  traits<Quaternion<_Scalar, _Options> >
324  {
325  typedef _Scalar Scalar;
326  typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients;
327 
328  typedef traits<Quaternion<_Scalar, _Options> > TraitsBase;
329  enum {
330  IsAligned = TraitsBase::IsAligned,
331 
332  Flags = TraitsBase::Flags
333  };
334  };
335 }
336 
337 namespace internal {
338  template<typename _Scalar, int _Options>
339  struct traits<Map<const Quaternion<_Scalar>, _Options> >:
340  traits<Quaternion<_Scalar> >
341  {
342  typedef _Scalar Scalar;
343  typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
344 
345  typedef traits<Quaternion<_Scalar, _Options> > TraitsBase;
346  enum {
347  IsAligned = TraitsBase::IsAligned,
348  Flags = TraitsBase::Flags & ~LvalueBit
349  };
350  };
351 }
352 
363 template<typename _Scalar, int _Options>
364 class Map<const Quaternion<_Scalar>, _Options >
365  : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
366 {
368 
369  public:
370  typedef _Scalar Scalar;
371  typedef typename internal::traits<Map>::Coefficients Coefficients;
373  using Base::operator*=;
374 
381  EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
382 
383  inline const Coefficients& coeffs() const { return m_coeffs;}
384 
385  protected:
387 };
388 
399 template<typename _Scalar, int _Options>
400 class Map<Quaternion<_Scalar>, _Options >
401  : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> >
402 {
403  typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base;
404 
405  public:
406  typedef _Scalar Scalar;
407  typedef typename internal::traits<Map>::Coefficients Coefficients;
409  using Base::operator*=;
410 
417  EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
418 
419  inline Coefficients& coeffs() { return m_coeffs; }
420  inline const Coefficients& coeffs() const { return m_coeffs; }
421 
422  protected:
424 };
425 
438 
439 /***************************************************************************
440 * Implementation of QuaternionBase methods
441 ***************************************************************************/
442 
443 // Generic Quaternion * Quaternion product
444 // This product can be specialized for a given architecture via the Arch template argument.
445 namespace internal {
446 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product
447 {
449  return Quaternion<Scalar>
450  (
451  a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
452  a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
453  a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
454  a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
455  );
456  }
457 };
458 }
459 
461 template <class Derived>
462 template <class OtherDerived>
463 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
465 {
466  EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
467  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
468  return internal::quat_product<Architecture::Target, Derived, OtherDerived,
469  typename internal::traits<Derived>::Scalar,
470  internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other);
471 }
472 
474 template <class Derived>
475 template <class OtherDerived>
477 {
478  derived() = derived() * other.derived();
479  return derived();
480 }
481 
489 template <class Derived>
492 {
493  // Note that this algorithm comes from the optimization by hand
494  // of the conversion to a Matrix followed by a Matrix/Vector product.
495  // It appears to be much faster than the common algorithm found
496  // in the litterature (30 versus 39 flops). It also requires two
497  // Vector3 as temporaries.
498  Vector3 uv = this->vec().cross(v);
499  uv += uv;
500  return v + this->w() * uv + this->vec().cross(uv);
501 }
502 
503 template<class Derived>
505 {
506  coeffs() = other.coeffs();
507  return derived();
508 }
509 
510 template<class Derived>
511 template<class OtherDerived>
513 {
514  coeffs() = other.coeffs();
515  return derived();
516 }
517 
520 template<class Derived>
522 {
523  Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
524  this->w() = internal::cos(ha);
525  this->vec() = internal::sin(ha) * aa.axis();
526  return derived();
527 }
528 
535 template<class Derived>
536 template<class MatrixDerived>
538 {
539  EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
540  YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
541  internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
542  return derived();
543 }
544 
548 template<class Derived>
549 inline typename QuaternionBase<Derived>::Matrix3
551 {
552  // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
553  // if not inlined then the cost of the return by value is huge ~ +35%,
554  // however, not inlining this function is an order of magnitude slower, so
555  // it has to be inlined, and so the return by value is not an issue
556  Matrix3 res;
557 
558  const Scalar tx = Scalar(2)*this->x();
559  const Scalar ty = Scalar(2)*this->y();
560  const Scalar tz = Scalar(2)*this->z();
561  const Scalar twx = tx*this->w();
562  const Scalar twy = ty*this->w();
563  const Scalar twz = tz*this->w();
564  const Scalar txx = tx*this->x();
565  const Scalar txy = ty*this->x();
566  const Scalar txz = tz*this->x();
567  const Scalar tyy = ty*this->y();
568  const Scalar tyz = tz*this->y();
569  const Scalar tzz = tz*this->z();
570 
571  res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
572  res.coeffRef(0,1) = txy-twz;
573  res.coeffRef(0,2) = txz+twy;
574  res.coeffRef(1,0) = txy+twz;
575  res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
576  res.coeffRef(1,2) = tyz-twx;
577  res.coeffRef(2,0) = txz-twy;
578  res.coeffRef(2,1) = tyz+twx;
579  res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
580 
581  return res;
582 }
583 
594 template<class Derived>
595 template<typename Derived1, typename Derived2>
597 {
598  using std::max;
599  Vector3 v0 = a.normalized();
600  Vector3 v1 = b.normalized();
601  Scalar c = v1.dot(v0);
602 
603  // if dot == -1, vectors are nearly opposites
604  // => accuraletly compute the rotation axis by computing the
605  // intersection of the two planes. This is done by solving:
606  // x^T v0 = 0
607  // x^T v1 = 0
608  // under the constraint:
609  // ||x|| = 1
610  // which yields a singular value problem
612  {
613  c = max<Scalar>(c,-1);
614  Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
616  Vector3 axis = svd.matrixV().col(2);
617 
618  Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
619  this->w() = internal::sqrt(w2);
620  this->vec() = axis * internal::sqrt(Scalar(1) - w2);
621  return derived();
622  }
623  Vector3 axis = v0.cross(v1);
624  Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2));
625  Scalar invs = Scalar(1)/s;
626  this->vec() = axis * invs;
627  this->w() = s * Scalar(0.5);
628 
629  return derived();
630 }
631 
632 
643 template<typename Scalar, int Options>
644 template<typename Derived1, typename Derived2>
646 {
647  Quaternion quat;
648  quat.setFromTwoVectors(a, b);
649  return quat;
650 }
651 
652 
659 template <class Derived>
661 {
662  // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
663  Scalar n2 = this->squaredNorm();
664  if (n2 > 0)
665  return Quaternion<Scalar>(conjugate().coeffs() / n2);
666  else
667  {
668  // return an invalid result to flag the error
669  return Quaternion<Scalar>(Coefficients::Zero());
670  }
671 }
672 
679 template <class Derived>
682 {
683  return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
684 }
685 
689 template <class Derived>
690 template <class OtherDerived>
691 inline typename internal::traits<Derived>::Scalar
693 {
694  using std::acos;
695  double d = internal::abs(this->dot(other));
696  if (d>=1.0)
697  return Scalar(0);
698  return static_cast<Scalar>(2 * acos(d));
699 }
700 
704 template <class Derived>
705 template <class OtherDerived>
708 {
709  using std::acos;
710  static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
711  Scalar d = this->dot(other);
712  Scalar absD = internal::abs(d);
713 
714  Scalar scale0;
715  Scalar scale1;
716 
717  if(absD>=one)
718  {
719  scale0 = Scalar(1) - t;
720  scale1 = t;
721  }
722  else
723  {
724  // theta is the angle between the 2 quaternions
725  Scalar theta = acos(absD);
726  Scalar sinTheta = internal::sin(theta);
727 
728  scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
729  scale1 = internal::sin( ( t * theta) ) / sinTheta;
730  }
731  if(d<0) scale1 = -scale1;
732 
733  return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
734 }
735 
736 namespace internal {
737 
738 // set from a rotation matrix
739 template<typename Other>
740 struct quaternionbase_assign_impl<Other,3,3>
741 {
742  typedef typename Other::Scalar Scalar;
743  typedef DenseIndex Index;
744  template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat)
745  {
746  // This algorithm comes from "Quaternion Calculus and Fast Animation",
747  // Ken Shoemake, 1987 SIGGRAPH course notes
748  Scalar t = mat.trace();
749  if (t > Scalar(0))
750  {
751  t = sqrt(t + Scalar(1.0));
752  q.w() = Scalar(0.5)*t;
753  t = Scalar(0.5)/t;
754  q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
755  q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
756  q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
757  }
758  else
759  {
760  DenseIndex i = 0;
761  if (mat.coeff(1,1) > mat.coeff(0,0))
762  i = 1;
763  if (mat.coeff(2,2) > mat.coeff(i,i))
764  i = 2;
765  DenseIndex j = (i+1)%3;
766  DenseIndex k = (j+1)%3;
767 
768  t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
769  q.coeffs().coeffRef(i) = Scalar(0.5) * t;
770  t = Scalar(0.5)/t;
771  q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
772  q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
773  q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
774  }
775  }
776 };
777 
778 // set from a vector of coefficients assumed to be a quaternion
779 template<typename Other>
780 struct quaternionbase_assign_impl<Other,4,1>
781 {
782  typedef typename Other::Scalar Scalar;
783  template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec)
784  {
785  q.coeffs() = vec;
786  }
787 };
788 
789 } // end namespace internal
790 
791 } // end namespace Eigen
792 
793 #endif // EIGEN_QUATERNION_H