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 // Eigen is free software; you can redistribute it and/or
00008 // modify it under the terms of the GNU Lesser General Public
00009 // License as published by the Free Software Foundation; either
00010 // version 3 of the License, or (at your option) any later version.
00011 //
00012 // Alternatively, you can redistribute it and/or
00013 // modify it under the terms of the GNU General Public License as
00014 // published by the Free Software Foundation; either version 2 of
00015 // the License, or (at your option) any later version.
00016 //
00017 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00018 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00019 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00020 // GNU General Public License for more details.
00021 //
00022 // You should have received a copy of the GNU Lesser General Public
00023 // License and a copy of the GNU General Public License along with
00024 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00025 
00026 #ifndef EIGEN_QUATERNION_H
00027 #define EIGEN_QUATERNION_H
00028 namespace Eigen { 
00029 
00030 
00031 /***************************************************************************
00032 * Definition of QuaternionBase<Derived>
00033 * The implementation is at the end of the file
00034 ***************************************************************************/
00035 
00036 namespace internal {
00037 template<typename Other,
00038          int OtherRows=Other::RowsAtCompileTime,
00039          int OtherCols=Other::ColsAtCompileTime>
00040 struct quaternionbase_assign_impl;
00041 }
00042 
00049 template<class Derived>
00050 class QuaternionBase : public RotationBase<Derived, 3>
00051 {
00052   typedef RotationBase<Derived, 3> Base;
00053 public:
00054   using Base::operator*;
00055   using Base::derived;
00056 
00057   typedef typename internal::traits<Derived>::Scalar Scalar;
00058   typedef typename NumTraits<Scalar>::Real RealScalar;
00059   typedef typename internal::traits<Derived>::Coefficients Coefficients;
00060   enum {
00061     Flags = Eigen::internal::traits<Derived>::Flags
00062   };
00063 
00064  // typedef typename Matrix<Scalar,4,1> Coefficients;
00066   typedef Matrix<Scalar,3,1> Vector3;
00068   typedef Matrix<Scalar,3,3> Matrix3;
00070   typedef AngleAxis<Scalar> AngleAxisType;
00071 
00072 
00073 
00075   inline Scalar x() const { return this->derived().coeffs().coeff(0); }
00077   inline Scalar y() const { return this->derived().coeffs().coeff(1); }
00079   inline Scalar z() const { return this->derived().coeffs().coeff(2); }
00081   inline Scalar w() const { return this->derived().coeffs().coeff(3); }
00082 
00084   inline Scalar& x() { return this->derived().coeffs().coeffRef(0); }
00086   inline Scalar& y() { return this->derived().coeffs().coeffRef(1); }
00088   inline Scalar& z() { return this->derived().coeffs().coeffRef(2); }
00090   inline Scalar& w() { return this->derived().coeffs().coeffRef(3); }
00091 
00093   inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
00094 
00096   inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); }
00097 
00099   inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); }
00100 
00102   inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
00103 
00104   EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
00105   template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
00106 
00107 // disabled this copy operator as it is giving very strange compilation errors when compiling
00108 // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's
00109 // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase
00110 // we didn't have to add, in addition to templated operator=, such a non-templated copy operator.
00111 //  Derived& operator=(const QuaternionBase& other)
00112 //  { return operator=<Derived>(other); }
00113 
00114   Derived& operator=(const AngleAxisType& aa);
00115   template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m);
00116 
00120   static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); }
00121 
00124   inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; }
00125 
00129   inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
00130 
00134   inline Scalar norm() const { return coeffs().norm(); }
00135 
00138   inline void normalize() { coeffs().normalize(); }
00141   inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
00142 
00148   template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
00149 
00150   template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
00151 
00153   Matrix3 toRotationMatrix() const;
00154 
00156   template<typename Derived1, typename Derived2>
00157   Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00158 
00159   template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
00160   template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
00161 
00163   Quaternion<Scalar> inverse() const;
00164 
00166   Quaternion<Scalar> conjugate() const;
00167 
00172   template<class OtherDerived> Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const;
00173 
00178   template<class OtherDerived>
00179   bool isApprox(const QuaternionBase<OtherDerived>& other, RealScalar prec = NumTraits<Scalar>::dummy_precision()) const
00180   { return coeffs().isApprox(other.coeffs(), prec); }
00181 
00183   EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const;
00184 
00190   template<typename NewScalarType>
00191   inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const
00192   {
00193     return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived());
00194   }
00195 
00196 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
00197 # include EIGEN_QUATERNIONBASE_PLUGIN
00198 #endif
00199 };
00200 
00201 /***************************************************************************
00202 * Definition/implementation of Quaternion<Scalar>
00203 ***************************************************************************/
00204 
00227 namespace internal {
00228 template<typename _Scalar,int _Options>
00229 struct traits<Quaternion<_Scalar,_Options> >
00230 {
00231   typedef Quaternion<_Scalar,_Options> PlainObject;
00232   typedef _Scalar Scalar;
00233   typedef Matrix<_Scalar,4,1,_Options> Coefficients;
00234   enum{
00235     IsAligned = internal::traits<Coefficients>::Flags & AlignedBit,
00236     Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit
00237   };
00238 };
00239 }
00240 
00241 template<typename _Scalar, int _Options>
00242 class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> >
00243 {
00244   typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base;
00245   enum { IsAligned = internal::traits<Quaternion>::IsAligned };
00246 
00247 public:
00248   typedef _Scalar Scalar;
00249 
00250   EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion)
00251   using Base::operator*=;
00252 
00253   typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
00254   typedef typename Base::AngleAxisType AngleAxisType;
00255 
00257   inline Quaternion() {}
00258 
00266   inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){}
00267 
00269   inline Quaternion(const Scalar* data) : m_coeffs(data) {}
00270 
00272   template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
00273 
00275   explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
00276 
00281   template<typename Derived>
00282   explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
00283 
00285   template<typename OtherScalar, int OtherOptions>
00286   explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
00287   { m_coeffs = other.coeffs().template cast<Scalar>(); }
00288 
00289   template<typename Derived1, typename Derived2>
00290   static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00291 
00292   inline Coefficients& coeffs() { return m_coeffs;}
00293   inline const Coefficients& coeffs() const { return m_coeffs;}
00294 
00295   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
00296 
00297 protected:
00298   Coefficients m_coeffs;
00299   
00300 #ifndef EIGEN_PARSED_BY_DOXYGEN
00301     static EIGEN_STRONG_INLINE void _check_template_params()
00302     {
00303       EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
00304         INVALID_MATRIX_TEMPLATE_PARAMETERS)
00305     }
00306 #endif
00307 };
00308 
00311 typedef Quaternion<float> Quaternionf;
00314 typedef Quaternion<double> Quaterniond;
00315 
00316 /***************************************************************************
00317 * Specialization of Map<Quaternion<Scalar>>
00318 ***************************************************************************/
00319 
00320 namespace internal {
00321   template<typename _Scalar, int _Options>
00322   struct traits<Map<Quaternion<_Scalar>, _Options> >:
00323   traits<Quaternion<_Scalar, _Options> >
00324   {
00325     typedef _Scalar Scalar;
00326     typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients;
00327 
00328     typedef traits<Quaternion<_Scalar, _Options> > TraitsBase;
00329     enum {
00330       IsAligned = TraitsBase::IsAligned,
00331 
00332       Flags = TraitsBase::Flags
00333     };
00334   };
00335 }
00336 
00337 namespace internal {
00338   template<typename _Scalar, int _Options>
00339   struct traits<Map<const Quaternion<_Scalar>, _Options> >:
00340   traits<Quaternion<_Scalar> >
00341   {
00342     typedef _Scalar Scalar;
00343     typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
00344 
00345     typedef traits<Quaternion<_Scalar, _Options> > TraitsBase;
00346     enum {
00347       IsAligned = TraitsBase::IsAligned,
00348       Flags = TraitsBase::Flags & ~LvalueBit
00349     };
00350   };
00351 }
00352 
00363 template<typename _Scalar, int _Options>
00364 class Map<const Quaternion<_Scalar>, _Options >
00365   : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
00366 {
00367     typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base;
00368 
00369   public:
00370     typedef _Scalar Scalar;
00371     typedef typename internal::traits<Map>::Coefficients Coefficients;
00372     EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00373     using Base::operator*=;
00374 
00381     EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
00382 
00383     inline const Coefficients& coeffs() const { return m_coeffs;}
00384 
00385   protected:
00386     const Coefficients m_coeffs;
00387 };
00388 
00399 template<typename _Scalar, int _Options>
00400 class Map<Quaternion<_Scalar>, _Options >
00401   : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> >
00402 {
00403     typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base;
00404 
00405   public:
00406     typedef _Scalar Scalar;
00407     typedef typename internal::traits<Map>::Coefficients Coefficients;
00408     EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00409     using Base::operator*=;
00410 
00417     EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
00418 
00419     inline Coefficients& coeffs() { return m_coeffs; }
00420     inline const Coefficients& coeffs() const { return m_coeffs; }
00421 
00422   protected:
00423     Coefficients m_coeffs;
00424 };
00425 
00428 typedef Map<Quaternion<float>, 0>         QuaternionMapf;
00431 typedef Map<Quaternion<double>, 0>        QuaternionMapd;
00434 typedef Map<Quaternion<float>, Aligned>   QuaternionMapAlignedf;
00437 typedef Map<Quaternion<double>, Aligned>  QuaternionMapAlignedd;
00438 
00439 /***************************************************************************
00440 * Implementation of QuaternionBase methods
00441 ***************************************************************************/
00442 
00443 // Generic Quaternion * Quaternion product
00444 // This product can be specialized for a given architecture via the Arch template argument.
00445 namespace internal {
00446 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product
00447 {
00448   static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
00449     return Quaternion<Scalar>
00450     (
00451       a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
00452       a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
00453       a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
00454       a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
00455     );
00456   }
00457 };
00458 }
00459 
00461 template <class Derived>
00462 template <class OtherDerived>
00463 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
00464 QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const
00465 {
00466   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
00467    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00468   return internal::quat_product<Architecture::Target, Derived, OtherDerived,
00469                          typename internal::traits<Derived>::Scalar,
00470                          internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other);
00471 }
00472 
00474 template <class Derived>
00475 template <class OtherDerived>
00476 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
00477 {
00478   derived() = derived() * other.derived();
00479   return derived();
00480 }
00481 
00489 template <class Derived>
00490 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
00491 QuaternionBase<Derived>::_transformVector(Vector3 v) const
00492 {
00493     // Note that this algorithm comes from the optimization by hand
00494     // of the conversion to a Matrix followed by a Matrix/Vector product.
00495     // It appears to be much faster than the common algorithm found
00496     // in the litterature (30 versus 39 flops). It also requires two
00497     // Vector3 as temporaries.
00498     Vector3 uv = this->vec().cross(v);
00499     uv += uv;
00500     return v + this->w() * uv + this->vec().cross(uv);
00501 }
00502 
00503 template<class Derived>
00504 EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other)
00505 {
00506   coeffs() = other.coeffs();
00507   return derived();
00508 }
00509 
00510 template<class Derived>
00511 template<class OtherDerived>
00512 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
00513 {
00514   coeffs() = other.coeffs();
00515   return derived();
00516 }
00517 
00520 template<class Derived>
00521 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
00522 {
00523   Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
00524   this->w() = internal::cos(ha);
00525   this->vec() = internal::sin(ha) * aa.axis();
00526   return derived();
00527 }
00528 
00535 template<class Derived>
00536 template<class MatrixDerived>
00537 inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr)
00538 {
00539   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
00540    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00541   internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
00542   return derived();
00543 }
00544 
00548 template<class Derived>
00549 inline typename QuaternionBase<Derived>::Matrix3
00550 QuaternionBase<Derived>::toRotationMatrix(void) const
00551 {
00552   // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
00553   // if not inlined then the cost of the return by value is huge ~ +35%,
00554   // however, not inlining this function is an order of magnitude slower, so
00555   // it has to be inlined, and so the return by value is not an issue
00556   Matrix3 res;
00557 
00558   const Scalar tx  = Scalar(2)*this->x();
00559   const Scalar ty  = Scalar(2)*this->y();
00560   const Scalar tz  = Scalar(2)*this->z();
00561   const Scalar twx = tx*this->w();
00562   const Scalar twy = ty*this->w();
00563   const Scalar twz = tz*this->w();
00564   const Scalar txx = tx*this->x();
00565   const Scalar txy = ty*this->x();
00566   const Scalar txz = tz*this->x();
00567   const Scalar tyy = ty*this->y();
00568   const Scalar tyz = tz*this->y();
00569   const Scalar tzz = tz*this->z();
00570 
00571   res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
00572   res.coeffRef(0,1) = txy-twz;
00573   res.coeffRef(0,2) = txz+twy;
00574   res.coeffRef(1,0) = txy+twz;
00575   res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
00576   res.coeffRef(1,2) = tyz-twx;
00577   res.coeffRef(2,0) = txz-twy;
00578   res.coeffRef(2,1) = tyz+twx;
00579   res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
00580 
00581   return res;
00582 }
00583 
00594 template<class Derived>
00595 template<typename Derived1, typename Derived2>
00596 inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00597 {
00598   using std::max;
00599   Vector3 v0 = a.normalized();
00600   Vector3 v1 = b.normalized();
00601   Scalar c = v1.dot(v0);
00602 
00603   // if dot == -1, vectors are nearly opposites
00604   // => accuraletly compute the rotation axis by computing the
00605   //    intersection of the two planes. This is done by solving:
00606   //       x^T v0 = 0
00607   //       x^T v1 = 0
00608   //    under the constraint:
00609   //       ||x|| = 1
00610   //    which yields a singular value problem
00611   if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
00612   {
00613     c = max<Scalar>(c,-1);
00614     Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
00615     JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
00616     Vector3 axis = svd.matrixV().col(2);
00617 
00618     Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
00619     this->w() = internal::sqrt(w2);
00620     this->vec() = axis * internal::sqrt(Scalar(1) - w2);
00621     return derived();
00622   }
00623   Vector3 axis = v0.cross(v1);
00624   Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2));
00625   Scalar invs = Scalar(1)/s;
00626   this->vec() = axis * invs;
00627   this->w() = s * Scalar(0.5);
00628 
00629   return derived();
00630 }
00631 
00632 
00643 template<typename Scalar, int Options>
00644 template<typename Derived1, typename Derived2>
00645 Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00646 {
00647     Quaternion quat;
00648     quat.setFromTwoVectors(a, b);
00649     return quat;
00650 }
00651 
00652 
00659 template <class Derived>
00660 inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const
00661 {
00662   // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
00663   Scalar n2 = this->squaredNorm();
00664   if (n2 > 0)
00665     return Quaternion<Scalar>(conjugate().coeffs() / n2);
00666   else
00667   {
00668     // return an invalid result to flag the error
00669     return Quaternion<Scalar>(Coefficients::Zero());
00670   }
00671 }
00672 
00679 template <class Derived>
00680 inline Quaternion<typename internal::traits<Derived>::Scalar>
00681 QuaternionBase<Derived>::conjugate() const
00682 {
00683   return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
00684 }
00685 
00689 template <class Derived>
00690 template <class OtherDerived>
00691 inline typename internal::traits<Derived>::Scalar
00692 QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const
00693 {
00694   using std::acos;
00695   double d = internal::abs(this->dot(other));
00696   if (d>=1.0)
00697     return Scalar(0);
00698   return static_cast<Scalar>(2 * acos(d));
00699 }
00700 
00704 template <class Derived>
00705 template <class OtherDerived>
00706 Quaternion<typename internal::traits<Derived>::Scalar>
00707 QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const
00708 {
00709   using std::acos;
00710   static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
00711   Scalar d = this->dot(other);
00712   Scalar absD = internal::abs(d);
00713 
00714   Scalar scale0;
00715   Scalar scale1;
00716 
00717   if(absD>=one)
00718   {
00719     scale0 = Scalar(1) - t;
00720     scale1 = t;
00721   }
00722   else
00723   {
00724     // theta is the angle between the 2 quaternions
00725     Scalar theta = acos(absD);
00726     Scalar sinTheta = internal::sin(theta);
00727 
00728     scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
00729     scale1 = internal::sin( ( t * theta) ) / sinTheta;
00730   }
00731   if(d<0) scale1 = -scale1;
00732 
00733   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
00734 }
00735 
00736 namespace internal {
00737 
00738 // set from a rotation matrix
00739 template<typename Other>
00740 struct quaternionbase_assign_impl<Other,3,3>
00741 {
00742   typedef typename Other::Scalar Scalar;
00743   typedef DenseIndex Index;
00744   template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat)
00745   {
00746     // This algorithm comes from  "Quaternion Calculus and Fast Animation",
00747     // Ken Shoemake, 1987 SIGGRAPH course notes
00748     Scalar t = mat.trace();
00749     if (t > Scalar(0))
00750     {
00751       t = sqrt(t + Scalar(1.0));
00752       q.w() = Scalar(0.5)*t;
00753       t = Scalar(0.5)/t;
00754       q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
00755       q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
00756       q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
00757     }
00758     else
00759     {
00760       DenseIndex i = 0;
00761       if (mat.coeff(1,1) > mat.coeff(0,0))
00762         i = 1;
00763       if (mat.coeff(2,2) > mat.coeff(i,i))
00764         i = 2;
00765       DenseIndex j = (i+1)%3;
00766       DenseIndex k = (j+1)%3;
00767 
00768       t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
00769       q.coeffs().coeffRef(i) = Scalar(0.5) * t;
00770       t = Scalar(0.5)/t;
00771       q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
00772       q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
00773       q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
00774     }
00775   }
00776 };
00777 
00778 // set from a vector of coefficients assumed to be a quaternion
00779 template<typename Other>
00780 struct quaternionbase_assign_impl<Other,4,1>
00781 {
00782   typedef typename Other::Scalar Scalar;
00783   template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec)
00784   {
00785     q.coeffs() = vec;
00786   }
00787 };
00788 
00789 } // end namespace internal
00790 
00791 } // end namespace Eigen
00792 
00793 #endif // EIGEN_QUATERNION_H