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(const Scalar& t, const QuaternionBase<OtherDerived>& other) const;
00158 
00163   template<class OtherDerived>
00164   bool isApprox(const QuaternionBase<OtherDerived>& other, const 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 
00213 namespace internal {
00214 template<typename _Scalar,int _Options>
00215 struct traits<Quaternion<_Scalar,_Options> >
00216 {
00217   typedef Quaternion<_Scalar,_Options> PlainObject;
00218   typedef _Scalar Scalar;
00219   typedef Matrix<_Scalar,4,1,_Options> Coefficients;
00220   enum{
00221     IsAligned = internal::traits<Coefficients>::Flags & AlignedBit,
00222     Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit
00223   };
00224 };
00225 }
00226 
00227 template<typename _Scalar, int _Options>
00228 class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> >
00229 {
00230   typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base;
00231   enum { IsAligned = internal::traits<Quaternion>::IsAligned };
00232 
00233 public:
00234   typedef _Scalar Scalar;
00235 
00236   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion)
00237   using Base::operator*=;
00238 
00239   typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
00240   typedef typename Base::AngleAxisType AngleAxisType;
00241 
00243   inline Quaternion() {}
00244 
00252   inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){}
00253 
00255   inline Quaternion(const Scalar* data) : m_coeffs(data) {}
00256 
00258   template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
00259 
00261   explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
00262 
00267   template<typename Derived>
00268   explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
00269 
00271   template<typename OtherScalar, int OtherOptions>
00272   explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
00273   { m_coeffs = other.coeffs().template cast<Scalar>(); }
00274 
00275   template<typename Derived1, typename Derived2>
00276   static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00277 
00278   inline Coefficients& coeffs() { return m_coeffs;}
00279   inline const Coefficients& coeffs() const { return m_coeffs;}
00280 
00281   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
00282 
00283 protected:
00284   Coefficients m_coeffs;
00285   
00286 #ifndef EIGEN_PARSED_BY_DOXYGEN
00287     static EIGEN_STRONG_INLINE void _check_template_params()
00288     {
00289       EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
00290         INVALID_MATRIX_TEMPLATE_PARAMETERS)
00291     }
00292 #endif
00293 };
00294 
00297 typedef Quaternion<float> Quaternionf;
00300 typedef Quaternion<double> Quaterniond;
00301 
00302 /***************************************************************************
00303 * Specialization of Map<Quaternion<Scalar>>
00304 ***************************************************************************/
00305 
00306 namespace internal {
00307   template<typename _Scalar, int _Options>
00308   struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
00309   {
00310     typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients;
00311   };
00312 }
00313 
00314 namespace internal {
00315   template<typename _Scalar, int _Options>
00316   struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
00317   {
00318     typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
00319     typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase;
00320     enum {
00321       Flags = TraitsBase::Flags & ~LvalueBit
00322     };
00323   };
00324 }
00325 
00337 template<typename _Scalar, int _Options>
00338 class Map<const Quaternion<_Scalar>, _Options >
00339   : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
00340 {
00341     typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base;
00342 
00343   public:
00344     typedef _Scalar Scalar;
00345     typedef typename internal::traits<Map>::Coefficients Coefficients;
00346     EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00347     using Base::operator*=;
00348 
00355     EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
00356 
00357     inline const Coefficients& coeffs() const { return m_coeffs;}
00358 
00359   protected:
00360     const Coefficients m_coeffs;
00361 };
00362 
00374 template<typename _Scalar, int _Options>
00375 class Map<Quaternion<_Scalar>, _Options >
00376   : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> >
00377 {
00378     typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base;
00379 
00380   public:
00381     typedef _Scalar Scalar;
00382     typedef typename internal::traits<Map>::Coefficients Coefficients;
00383     EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00384     using Base::operator*=;
00385 
00392     EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
00393 
00394     inline Coefficients& coeffs() { return m_coeffs; }
00395     inline const Coefficients& coeffs() const { return m_coeffs; }
00396 
00397   protected:
00398     Coefficients m_coeffs;
00399 };
00400 
00403 typedef Map<Quaternion<float>, 0>         QuaternionMapf;
00406 typedef Map<Quaternion<double>, 0>        QuaternionMapd;
00409 typedef Map<Quaternion<float>, Aligned>   QuaternionMapAlignedf;
00412 typedef Map<Quaternion<double>, Aligned>  QuaternionMapAlignedd;
00413 
00414 /***************************************************************************
00415 * Implementation of QuaternionBase methods
00416 ***************************************************************************/
00417 
00418 // Generic Quaternion * Quaternion product
00419 // This product can be specialized for a given architecture via the Arch template argument.
00420 namespace internal {
00421 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product
00422 {
00423   static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
00424     return Quaternion<Scalar>
00425     (
00426       a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
00427       a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
00428       a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
00429       a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
00430     );
00431   }
00432 };
00433 }
00434 
00436 template <class Derived>
00437 template <class OtherDerived>
00438 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
00439 QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const
00440 {
00441   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
00442    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00443   return internal::quat_product<Architecture::Target, Derived, OtherDerived,
00444                          typename internal::traits<Derived>::Scalar,
00445                          internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other);
00446 }
00447 
00449 template <class Derived>
00450 template <class OtherDerived>
00451 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
00452 {
00453   derived() = derived() * other.derived();
00454   return derived();
00455 }
00456 
00464 template <class Derived>
00465 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
00466 QuaternionBase<Derived>::_transformVector(Vector3 v) const
00467 {
00468     // Note that this algorithm comes from the optimization by hand
00469     // of the conversion to a Matrix followed by a Matrix/Vector product.
00470     // It appears to be much faster than the common algorithm found
00471     // in the litterature (30 versus 39 flops). It also requires two
00472     // Vector3 as temporaries.
00473     Vector3 uv = this->vec().cross(v);
00474     uv += uv;
00475     return v + this->w() * uv + this->vec().cross(uv);
00476 }
00477 
00478 template<class Derived>
00479 EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other)
00480 {
00481   coeffs() = other.coeffs();
00482   return derived();
00483 }
00484 
00485 template<class Derived>
00486 template<class OtherDerived>
00487 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
00488 {
00489   coeffs() = other.coeffs();
00490   return derived();
00491 }
00492 
00495 template<class Derived>
00496 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
00497 {
00498   using std::cos;
00499   using std::sin;
00500   Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
00501   this->w() = cos(ha);
00502   this->vec() = sin(ha) * aa.axis();
00503   return derived();
00504 }
00505 
00512 template<class Derived>
00513 template<class MatrixDerived>
00514 inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr)
00515 {
00516   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
00517    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00518   internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
00519   return derived();
00520 }
00521 
00525 template<class Derived>
00526 inline typename QuaternionBase<Derived>::Matrix3
00527 QuaternionBase<Derived>::toRotationMatrix(void) const
00528 {
00529   // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
00530   // if not inlined then the cost of the return by value is huge ~ +35%,
00531   // however, not inlining this function is an order of magnitude slower, so
00532   // it has to be inlined, and so the return by value is not an issue
00533   Matrix3 res;
00534 
00535   const Scalar tx  = Scalar(2)*this->x();
00536   const Scalar ty  = Scalar(2)*this->y();
00537   const Scalar tz  = Scalar(2)*this->z();
00538   const Scalar twx = tx*this->w();
00539   const Scalar twy = ty*this->w();
00540   const Scalar twz = tz*this->w();
00541   const Scalar txx = tx*this->x();
00542   const Scalar txy = ty*this->x();
00543   const Scalar txz = tz*this->x();
00544   const Scalar tyy = ty*this->y();
00545   const Scalar tyz = tz*this->y();
00546   const Scalar tzz = tz*this->z();
00547 
00548   res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
00549   res.coeffRef(0,1) = txy-twz;
00550   res.coeffRef(0,2) = txz+twy;
00551   res.coeffRef(1,0) = txy+twz;
00552   res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
00553   res.coeffRef(1,2) = tyz-twx;
00554   res.coeffRef(2,0) = txz-twy;
00555   res.coeffRef(2,1) = tyz+twx;
00556   res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
00557 
00558   return res;
00559 }
00560 
00571 template<class Derived>
00572 template<typename Derived1, typename Derived2>
00573 inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00574 {
00575   using std::max;
00576   using std::sqrt;
00577   Vector3 v0 = a.normalized();
00578   Vector3 v1 = b.normalized();
00579   Scalar c = v1.dot(v0);
00580 
00581   // if dot == -1, vectors are nearly opposites
00582   // => accuraletly compute the rotation axis by computing the
00583   //    intersection of the two planes. This is done by solving:
00584   //       x^T v0 = 0
00585   //       x^T v1 = 0
00586   //    under the constraint:
00587   //       ||x|| = 1
00588   //    which yields a singular value problem
00589   if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
00590   {
00591     c = max<Scalar>(c,-1);
00592     Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
00593     JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
00594     Vector3 axis = svd.matrixV().col(2);
00595 
00596     Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
00597     this->w() = sqrt(w2);
00598     this->vec() = axis * sqrt(Scalar(1) - w2);
00599     return derived();
00600   }
00601   Vector3 axis = v0.cross(v1);
00602   Scalar s = sqrt((Scalar(1)+c)*Scalar(2));
00603   Scalar invs = Scalar(1)/s;
00604   this->vec() = axis * invs;
00605   this->w() = s * Scalar(0.5);
00606 
00607   return derived();
00608 }
00609 
00610 
00621 template<typename Scalar, int Options>
00622 template<typename Derived1, typename Derived2>
00623 Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00624 {
00625     Quaternion quat;
00626     quat.setFromTwoVectors(a, b);
00627     return quat;
00628 }
00629 
00630 
00637 template <class Derived>
00638 inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const
00639 {
00640   // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
00641   Scalar n2 = this->squaredNorm();
00642   if (n2 > 0)
00643     return Quaternion<Scalar>(conjugate().coeffs() / n2);
00644   else
00645   {
00646     // return an invalid result to flag the error
00647     return Quaternion<Scalar>(Coefficients::Zero());
00648   }
00649 }
00650 
00657 template <class Derived>
00658 inline Quaternion<typename internal::traits<Derived>::Scalar>
00659 QuaternionBase<Derived>::conjugate() const
00660 {
00661   return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
00662 }
00663 
00667 template <class Derived>
00668 template <class OtherDerived>
00669 inline typename internal::traits<Derived>::Scalar
00670 QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const
00671 {
00672   using std::acos;
00673   using std::abs;
00674   double d = abs(this->dot(other));
00675   if (d>=1.0)
00676     return Scalar(0);
00677   return static_cast<Scalar>(2 * acos(d));
00678 }
00679 
00683 template <class Derived>
00684 template <class OtherDerived>
00685 Quaternion<typename internal::traits<Derived>::Scalar>
00686 QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const
00687 {
00688   using std::acos;
00689   using std::sin;
00690   using std::abs;
00691   static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
00692   Scalar d = this->dot(other);
00693   Scalar absD = abs(d);
00694 
00695   Scalar scale0;
00696   Scalar scale1;
00697 
00698   if(absD>=one)
00699   {
00700     scale0 = Scalar(1) - t;
00701     scale1 = t;
00702   }
00703   else
00704   {
00705     // theta is the angle between the 2 quaternions
00706     Scalar theta = acos(absD);
00707     Scalar sinTheta = sin(theta);
00708 
00709     scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;
00710     scale1 = sin( ( t * theta) ) / sinTheta;
00711   }
00712   if(d<0) scale1 = -scale1;
00713 
00714   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
00715 }
00716 
00717 namespace internal {
00718 
00719 // set from a rotation matrix
00720 template<typename Other>
00721 struct quaternionbase_assign_impl<Other,3,3>
00722 {
00723   typedef typename Other::Scalar Scalar;
00724   typedef DenseIndex Index;
00725   template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat)
00726   {
00727     using std::sqrt;
00728     // This algorithm comes from  "Quaternion Calculus and Fast Animation",
00729     // Ken Shoemake, 1987 SIGGRAPH course notes
00730     Scalar t = mat.trace();
00731     if (t > Scalar(0))
00732     {
00733       t = sqrt(t + Scalar(1.0));
00734       q.w() = Scalar(0.5)*t;
00735       t = Scalar(0.5)/t;
00736       q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
00737       q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
00738       q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
00739     }
00740     else
00741     {
00742       DenseIndex i = 0;
00743       if (mat.coeff(1,1) > mat.coeff(0,0))
00744         i = 1;
00745       if (mat.coeff(2,2) > mat.coeff(i,i))
00746         i = 2;
00747       DenseIndex j = (i+1)%3;
00748       DenseIndex k = (j+1)%3;
00749 
00750       t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
00751       q.coeffs().coeffRef(i) = Scalar(0.5) * t;
00752       t = Scalar(0.5)/t;
00753       q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
00754       q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
00755       q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
00756     }
00757   }
00758 };
00759 
00760 // set from a vector of coefficients assumed to be a quaternion
00761 template<typename Other>
00762 struct quaternionbase_assign_impl<Other,4,1>
00763 {
00764   typedef typename Other::Scalar Scalar;
00765   template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec)
00766   {
00767     q.coeffs() = vec;
00768   }
00769 };
00770 
00771 } // end namespace internal
00772 
00773 } // end namespace Eigen
00774 
00775 #endif // EIGEN_QUATERNION_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:44