Quaternion.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 // Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr>
00006 //
00007 // This Source Code Form is subject to the terms of the Mozilla
00008 // Public License v. 2.0. If a copy of the MPL was not distributed
00009 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00010 
00011 #ifndef EIGEN_QUATERNION_H
00012 #define EIGEN_QUATERNION_H
00013 namespace Eigen { 
00014 
00015 
00016 /***************************************************************************
00017 * Definition of QuaternionBase<Derived>
00018 * The implementation is at the end of the file
00019 ***************************************************************************/
00020 
00021 namespace internal {
00022 template<typename Other,
00023          int OtherRows=Other::RowsAtCompileTime,
00024          int OtherCols=Other::ColsAtCompileTime>
00025 struct quaternionbase_assign_impl;
00026 }
00027 
00034 template<class Derived>
00035 class QuaternionBase : public RotationBase<Derived, 3>
00036 {
00037   typedef RotationBase<Derived, 3> Base;
00038 public:
00039   using Base::operator*;
00040   using Base::derived;
00041 
00042   typedef typename internal::traits<Derived>::Scalar Scalar;
00043   typedef typename NumTraits<Scalar>::Real RealScalar;
00044   typedef typename internal::traits<Derived>::Coefficients Coefficients;
00045   enum {
00046     Flags = Eigen::internal::traits<Derived>::Flags
00047   };
00048 
00049  // typedef typename Matrix<Scalar,4,1> Coefficients;
00051   typedef Matrix<Scalar,3,1> Vector3;
00053   typedef Matrix<Scalar,3,3> Matrix3;
00055   typedef AngleAxis<Scalar> AngleAxisType;
00056 
00057 
00058 
00060   inline Scalar x() const { return this->derived().coeffs().coeff(0); }
00062   inline Scalar y() const { return this->derived().coeffs().coeff(1); }
00064   inline Scalar z() const { return this->derived().coeffs().coeff(2); }
00066   inline Scalar w() const { return this->derived().coeffs().coeff(3); }
00067 
00069   inline Scalar& x() { return this->derived().coeffs().coeffRef(0); }
00071   inline Scalar& y() { return this->derived().coeffs().coeffRef(1); }
00073   inline Scalar& z() { return this->derived().coeffs().coeffRef(2); }
00075   inline Scalar& w() { return this->derived().coeffs().coeffRef(3); }
00076 
00078   inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
00079 
00081   inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); }
00082 
00084   inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); }
00085 
00087   inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
00088 
00089   EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
00090   template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
00091 
00092 // disabled this copy operator as it is giving very strange compilation errors when compiling
00093 // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's
00094 // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase
00095 // we didn't have to add, in addition to templated operator=, such a non-templated copy operator.
00096 //  Derived& operator=(const QuaternionBase& other)
00097 //  { return operator=<Derived>(other); }
00098 
00099   Derived& operator=(const AngleAxisType& aa);
00100   template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m);
00101 
00105   static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); }
00106 
00109   inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; }
00110 
00114   inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
00115 
00119   inline Scalar norm() const { return coeffs().norm(); }
00120 
00123   inline void normalize() { coeffs().normalize(); }
00126   inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
00127 
00133   template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
00134 
00135   template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
00136 
00138   Matrix3 toRotationMatrix() const;
00139 
00141   template<typename Derived1, typename Derived2>
00142   Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00143 
00144   template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
00145   template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
00146 
00148   Quaternion<Scalar> inverse() const;
00149 
00151   Quaternion<Scalar> conjugate() const;
00152 
00157   template<class OtherDerived> Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const;
00158 
00163   template<class OtherDerived>
00164   bool isApprox(const QuaternionBase<OtherDerived>& other, RealScalar prec = NumTraits<Scalar>::dummy_precision()) const
00165   { return coeffs().isApprox(other.coeffs(), prec); }
00166 
00168   EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const;
00169 
00175   template<typename NewScalarType>
00176   inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const
00177   {
00178     return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived());
00179   }
00180 
00181 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
00182 # include EIGEN_QUATERNIONBASE_PLUGIN
00183 #endif
00184 };
00185 
00186 /***************************************************************************
00187 * Definition/implementation of Quaternion<Scalar>
00188 ***************************************************************************/
00189 
00212 namespace internal {
00213 template<typename _Scalar,int _Options>
00214 struct traits<Quaternion<_Scalar,_Options> >
00215 {
00216   typedef Quaternion<_Scalar,_Options> PlainObject;
00217   typedef _Scalar Scalar;
00218   typedef Matrix<_Scalar,4,1,_Options> Coefficients;
00219   enum{
00220     IsAligned = internal::traits<Coefficients>::Flags & AlignedBit,
00221     Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit
00222   };
00223 };
00224 }
00225 
00226 template<typename _Scalar, int _Options>
00227 class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> >
00228 {
00229   typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base;
00230   enum { IsAligned = internal::traits<Quaternion>::IsAligned };
00231 
00232 public:
00233   typedef _Scalar Scalar;
00234 
00235   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion)
00236   using Base::operator*=;
00237 
00238   typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
00239   typedef typename Base::AngleAxisType AngleAxisType;
00240 
00242   inline Quaternion() {}
00243 
00251   inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){}
00252 
00254   inline Quaternion(const Scalar* data) : m_coeffs(data) {}
00255 
00257   template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
00258 
00260   explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
00261 
00266   template<typename Derived>
00267   explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
00268 
00270   template<typename OtherScalar, int OtherOptions>
00271   explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
00272   { m_coeffs = other.coeffs().template cast<Scalar>(); }
00273 
00274   template<typename Derived1, typename Derived2>
00275   static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00276 
00277   inline Coefficients& coeffs() { return m_coeffs;}
00278   inline const Coefficients& coeffs() const { return m_coeffs;}
00279 
00280   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
00281 
00282 protected:
00283   Coefficients m_coeffs;
00284   
00285 #ifndef EIGEN_PARSED_BY_DOXYGEN
00286     static EIGEN_STRONG_INLINE void _check_template_params()
00287     {
00288       EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
00289         INVALID_MATRIX_TEMPLATE_PARAMETERS)
00290     }
00291 #endif
00292 };
00293 
00296 typedef Quaternion<float> Quaternionf;
00299 typedef Quaternion<double> Quaterniond;
00300 
00301 /***************************************************************************
00302 * Specialization of Map<Quaternion<Scalar>>
00303 ***************************************************************************/
00304 
00305 namespace internal {
00306   template<typename _Scalar, int _Options>
00307   struct traits<Map<Quaternion<_Scalar>, _Options> >:
00308   traits<Quaternion<_Scalar, _Options> >
00309   {
00310     typedef _Scalar Scalar;
00311     typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients;
00312 
00313     typedef traits<Quaternion<_Scalar, _Options> > TraitsBase;
00314     enum {
00315       IsAligned = TraitsBase::IsAligned,
00316 
00317       Flags = TraitsBase::Flags
00318     };
00319   };
00320 }
00321 
00322 namespace internal {
00323   template<typename _Scalar, int _Options>
00324   struct traits<Map<const Quaternion<_Scalar>, _Options> >:
00325   traits<Quaternion<_Scalar> >
00326   {
00327     typedef _Scalar Scalar;
00328     typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
00329 
00330     typedef traits<Quaternion<_Scalar, _Options> > TraitsBase;
00331     enum {
00332       IsAligned = TraitsBase::IsAligned,
00333       Flags = TraitsBase::Flags & ~LvalueBit
00334     };
00335   };
00336 }
00337 
00348 template<typename _Scalar, int _Options>
00349 class Map<const Quaternion<_Scalar>, _Options >
00350   : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
00351 {
00352     typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base;
00353 
00354   public:
00355     typedef _Scalar Scalar;
00356     typedef typename internal::traits<Map>::Coefficients Coefficients;
00357     EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00358     using Base::operator*=;
00359 
00366     EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
00367 
00368     inline const Coefficients& coeffs() const { return m_coeffs;}
00369 
00370   protected:
00371     const Coefficients m_coeffs;
00372 };
00373 
00384 template<typename _Scalar, int _Options>
00385 class Map<Quaternion<_Scalar>, _Options >
00386   : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> >
00387 {
00388     typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base;
00389 
00390   public:
00391     typedef _Scalar Scalar;
00392     typedef typename internal::traits<Map>::Coefficients Coefficients;
00393     EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00394     using Base::operator*=;
00395 
00402     EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
00403 
00404     inline Coefficients& coeffs() { return m_coeffs; }
00405     inline const Coefficients& coeffs() const { return m_coeffs; }
00406 
00407   protected:
00408     Coefficients m_coeffs;
00409 };
00410 
00413 typedef Map<Quaternion<float>, 0>         QuaternionMapf;
00416 typedef Map<Quaternion<double>, 0>        QuaternionMapd;
00419 typedef Map<Quaternion<float>, Aligned>   QuaternionMapAlignedf;
00422 typedef Map<Quaternion<double>, Aligned>  QuaternionMapAlignedd;
00423 
00424 /***************************************************************************
00425 * Implementation of QuaternionBase methods
00426 ***************************************************************************/
00427 
00428 // Generic Quaternion * Quaternion product
00429 // This product can be specialized for a given architecture via the Arch template argument.
00430 namespace internal {
00431 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product
00432 {
00433   static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
00434     return Quaternion<Scalar>
00435     (
00436       a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
00437       a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
00438       a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
00439       a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
00440     );
00441   }
00442 };
00443 }
00444 
00446 template <class Derived>
00447 template <class OtherDerived>
00448 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
00449 QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const
00450 {
00451   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
00452    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00453   return internal::quat_product<Architecture::Target, Derived, OtherDerived,
00454                          typename internal::traits<Derived>::Scalar,
00455                          internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other);
00456 }
00457 
00459 template <class Derived>
00460 template <class OtherDerived>
00461 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
00462 {
00463   derived() = derived() * other.derived();
00464   return derived();
00465 }
00466 
00474 template <class Derived>
00475 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
00476 QuaternionBase<Derived>::_transformVector(Vector3 v) const
00477 {
00478     // Note that this algorithm comes from the optimization by hand
00479     // of the conversion to a Matrix followed by a Matrix/Vector product.
00480     // It appears to be much faster than the common algorithm found
00481     // in the litterature (30 versus 39 flops). It also requires two
00482     // Vector3 as temporaries.
00483     Vector3 uv = this->vec().cross(v);
00484     uv += uv;
00485     return v + this->w() * uv + this->vec().cross(uv);
00486 }
00487 
00488 template<class Derived>
00489 EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other)
00490 {
00491   coeffs() = other.coeffs();
00492   return derived();
00493 }
00494 
00495 template<class Derived>
00496 template<class OtherDerived>
00497 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
00498 {
00499   coeffs() = other.coeffs();
00500   return derived();
00501 }
00502 
00505 template<class Derived>
00506 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
00507 {
00508   Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
00509   this->w() = internal::cos(ha);
00510   this->vec() = internal::sin(ha) * aa.axis();
00511   return derived();
00512 }
00513 
00520 template<class Derived>
00521 template<class MatrixDerived>
00522 inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr)
00523 {
00524   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
00525    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00526   internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
00527   return derived();
00528 }
00529 
00533 template<class Derived>
00534 inline typename QuaternionBase<Derived>::Matrix3
00535 QuaternionBase<Derived>::toRotationMatrix(void) const
00536 {
00537   // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
00538   // if not inlined then the cost of the return by value is huge ~ +35%,
00539   // however, not inlining this function is an order of magnitude slower, so
00540   // it has to be inlined, and so the return by value is not an issue
00541   Matrix3 res;
00542 
00543   const Scalar tx  = Scalar(2)*this->x();
00544   const Scalar ty  = Scalar(2)*this->y();
00545   const Scalar tz  = Scalar(2)*this->z();
00546   const Scalar twx = tx*this->w();
00547   const Scalar twy = ty*this->w();
00548   const Scalar twz = tz*this->w();
00549   const Scalar txx = tx*this->x();
00550   const Scalar txy = ty*this->x();
00551   const Scalar txz = tz*this->x();
00552   const Scalar tyy = ty*this->y();
00553   const Scalar tyz = tz*this->y();
00554   const Scalar tzz = tz*this->z();
00555 
00556   res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
00557   res.coeffRef(0,1) = txy-twz;
00558   res.coeffRef(0,2) = txz+twy;
00559   res.coeffRef(1,0) = txy+twz;
00560   res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
00561   res.coeffRef(1,2) = tyz-twx;
00562   res.coeffRef(2,0) = txz-twy;
00563   res.coeffRef(2,1) = tyz+twx;
00564   res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
00565 
00566   return res;
00567 }
00568 
00579 template<class Derived>
00580 template<typename Derived1, typename Derived2>
00581 inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00582 {
00583   using std::max;
00584   Vector3 v0 = a.normalized();
00585   Vector3 v1 = b.normalized();
00586   Scalar c = v1.dot(v0);
00587 
00588   // if dot == -1, vectors are nearly opposites
00589   // => accuraletly compute the rotation axis by computing the
00590   //    intersection of the two planes. This is done by solving:
00591   //       x^T v0 = 0
00592   //       x^T v1 = 0
00593   //    under the constraint:
00594   //       ||x|| = 1
00595   //    which yields a singular value problem
00596   if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
00597   {
00598     c = max<Scalar>(c,-1);
00599     Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
00600     JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
00601     Vector3 axis = svd.matrixV().col(2);
00602 
00603     Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
00604     this->w() = internal::sqrt(w2);
00605     this->vec() = axis * internal::sqrt(Scalar(1) - w2);
00606     return derived();
00607   }
00608   Vector3 axis = v0.cross(v1);
00609   Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2));
00610   Scalar invs = Scalar(1)/s;
00611   this->vec() = axis * invs;
00612   this->w() = s * Scalar(0.5);
00613 
00614   return derived();
00615 }
00616 
00617 
00628 template<typename Scalar, int Options>
00629 template<typename Derived1, typename Derived2>
00630 Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00631 {
00632     Quaternion quat;
00633     quat.setFromTwoVectors(a, b);
00634     return quat;
00635 }
00636 
00637 
00644 template <class Derived>
00645 inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const
00646 {
00647   // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
00648   Scalar n2 = this->squaredNorm();
00649   if (n2 > 0)
00650     return Quaternion<Scalar>(conjugate().coeffs() / n2);
00651   else
00652   {
00653     // return an invalid result to flag the error
00654     return Quaternion<Scalar>(Coefficients::Zero());
00655   }
00656 }
00657 
00664 template <class Derived>
00665 inline Quaternion<typename internal::traits<Derived>::Scalar>
00666 QuaternionBase<Derived>::conjugate() const
00667 {
00668   return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
00669 }
00670 
00674 template <class Derived>
00675 template <class OtherDerived>
00676 inline typename internal::traits<Derived>::Scalar
00677 QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const
00678 {
00679   using std::acos;
00680   double d = internal::abs(this->dot(other));
00681   if (d>=1.0)
00682     return Scalar(0);
00683   return static_cast<Scalar>(2 * acos(d));
00684 }
00685 
00689 template <class Derived>
00690 template <class OtherDerived>
00691 Quaternion<typename internal::traits<Derived>::Scalar>
00692 QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const
00693 {
00694   using std::acos;
00695   static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
00696   Scalar d = this->dot(other);
00697   Scalar absD = internal::abs(d);
00698 
00699   Scalar scale0;
00700   Scalar scale1;
00701 
00702   if(absD>=one)
00703   {
00704     scale0 = Scalar(1) - t;
00705     scale1 = t;
00706   }
00707   else
00708   {
00709     // theta is the angle between the 2 quaternions
00710     Scalar theta = acos(absD);
00711     Scalar sinTheta = internal::sin(theta);
00712 
00713     scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
00714     scale1 = internal::sin( ( t * theta) ) / sinTheta;
00715   }
00716   if(d<0) scale1 = -scale1;
00717 
00718   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
00719 }
00720 
00721 namespace internal {
00722 
00723 // set from a rotation matrix
00724 template<typename Other>
00725 struct quaternionbase_assign_impl<Other,3,3>
00726 {
00727   typedef typename Other::Scalar Scalar;
00728   typedef DenseIndex Index;
00729   template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat)
00730   {
00731     // This algorithm comes from  "Quaternion Calculus and Fast Animation",
00732     // Ken Shoemake, 1987 SIGGRAPH course notes
00733     Scalar t = mat.trace();
00734     if (t > Scalar(0))
00735     {
00736       t = sqrt(t + Scalar(1.0));
00737       q.w() = Scalar(0.5)*t;
00738       t = Scalar(0.5)/t;
00739       q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
00740       q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
00741       q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
00742     }
00743     else
00744     {
00745       DenseIndex i = 0;
00746       if (mat.coeff(1,1) > mat.coeff(0,0))
00747         i = 1;
00748       if (mat.coeff(2,2) > mat.coeff(i,i))
00749         i = 2;
00750       DenseIndex j = (i+1)%3;
00751       DenseIndex k = (j+1)%3;
00752 
00753       t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
00754       q.coeffs().coeffRef(i) = Scalar(0.5) * t;
00755       t = Scalar(0.5)/t;
00756       q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
00757       q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
00758       q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
00759     }
00760   }
00761 };
00762 
00763 // set from a vector of coefficients assumed to be a quaternion
00764 template<typename Other>
00765 struct quaternionbase_assign_impl<Other,4,1>
00766 {
00767   typedef typename Other::Scalar Scalar;
00768   template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec)
00769   {
00770     q.coeffs() = vec;
00771   }
00772 };
00773 
00774 } // end namespace internal
00775 
00776 } // end namespace Eigen
00777 
00778 #endif // EIGEN_QUATERNION_H


win_eigen
Author(s): Daniel Stonier
autogenerated on Mon Oct 6 2014 12:25:46