Quaternion.h

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 
00028 /** \geometry_module \ingroup Geometry_Module
00029   * \class QuaternionBase
00030   * \brief Base class for quaternion expressions
00031   * \tparam Derived derived type (CRTP)
00032   * \sa class Quaternion
00033   */
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;
00050   /** the type of a 3D vector */
00051   typedef Matrix<Scalar,3,1>  Vector3;
00052   /** the equivalent rotation matrix type */
00053   typedef Matrix<Scalar,3,3> Matrix3;
00054   /** the equivalent angle-axis type */
00055   typedef AngleAxis<Scalar>  AngleAxisType;
00056 
00057 
00058 
00059   /** \returns the \c x coefficient */
00060   inline Scalar x () const { return this->derived().coeffs().coeff(0); }
00061   /** \returns the \c y coefficient */
00062   inline Scalar y () const { return this->derived().coeffs().coeff(1); }
00063   /** \returns the \c z coefficient */
00064   inline Scalar z () const { return this->derived().coeffs().coeff(2); }
00065   /** \returns the \c w coefficient */
00066   inline Scalar w () const { return this->derived().coeffs().coeff(3); }
00067 
00068   /** \returns a reference to the \c x coefficient */
00069   inline Scalar& x () { return this->derived().coeffs().coeffRef(0); }
00070   /** \returns a reference to the \c y coefficient */
00071   inline Scalar& y () { return this->derived().coeffs().coeffRef(1); }
00072   /** \returns a reference to the \c z coefficient */
00073   inline Scalar& z () { return this->derived().coeffs().coeffRef(2); }
00074   /** \returns a reference to the \c w coefficient */
00075   inline Scalar& w () { return this->derived().coeffs().coeffRef(3); }
00076 
00077   /** \returns a read-only vector expression of the imaginary part (x,y,z) */
00078   inline const VectorBlock<const Coefficients,3> vec () const { return coeffs ().template head<3>(); }
00079 
00080   /** \returns a vector expression of the imaginary part (x,y,z) */
00081   inline VectorBlock<Coefficients,3> vec () { return coeffs ().template head<3>(); }
00082 
00083   /** \returns a read-only vector expression of the coefficients (x,y,z,w) */
00084   inline const typename internal::traits<Derived>::Coefficients& coeffs () const { return derived().coeffs(); }
00085 
00086   /** \returns a vector expression of the coefficients (x,y,z,w) */
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 
00102   /** \returns a quaternion representing an identity rotation
00103     * \sa MatrixBase::Identity()
00104     */
00105   static inline Quaternion<Scalar>  Identity () { return Quaternion<Scalar> (Scalar(1), Scalar(0), Scalar(0), Scalar(0)); }
00106 
00107   /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity()
00108     */
00109   inline QuaternionBase & setIdentity () { coeffs () << Scalar(0), Scalar(0), Scalar(0), Scalar(1); return *this; }
00110 
00111   /** \returns the squared norm of the quaternion's coefficients
00112     * \sa QuaternionBase::norm(), MatrixBase::squaredNorm()
00113     */
00114   inline Scalar squaredNorm () const { return coeffs ().squaredNorm(); }
00115 
00116   /** \returns the norm of the quaternion's coefficients
00117     * \sa QuaternionBase::squaredNorm(), MatrixBase::norm()
00118     */
00119   inline Scalar norm () const { return coeffs ().norm(); }
00120 
00121   /** Normalizes the quaternion \c *this
00122     * \sa normalized(), MatrixBase::normalize() */
00123   inline void normalize() { coeffs ().normalize(); }
00124   /** \returns a normalized copy of \c *this
00125     * \sa normalize(), MatrixBase::normalized() */
00126   inline Quaternion<Scalar>  normalized () const { return Quaternion<Scalar> (coeffs ().normalized()); }
00127 
00128     /** \returns the dot product of \c *this and \a other
00129     * Geometrically speaking, the dot product of two unit quaternions
00130     * corresponds to the cosine of half the angle between the two rotations.
00131     * \sa angularDistance()
00132     */
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 
00137   /** \returns an equivalent 3x3 rotation matrix */
00138   Matrix3 toRotationMatrix() const;
00139 
00140   /** \returns the quaternion which transform \a a into \a b through a rotation */
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 
00147   /** \returns the quaternion describing the inverse rotation */
00148   Quaternion<Scalar>  inverse () const;
00149 
00150   /** \returns the conjugated quaternion */
00151   Quaternion<Scalar>  conjugate () const;
00152 
00153   template<class OtherDerived> Quaternion<Scalar>  slerp (const Scalar& t, const QuaternionBase<OtherDerived> & other) const;
00154 
00155   /** \returns \c true if \c *this is approximately equal to \a other, within the precision
00156     * determined by \a prec.
00157     *
00158     * \sa MatrixBase::isApprox() */
00159   template<class OtherDerived>
00160   bool isApprox (const QuaternionBase<OtherDerived> & other, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
00161   { return coeffs ().isApprox(other.coeffs (), prec); }
00162 
00163     /** return the result vector of \a v through the rotation*/
00164   EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const;
00165 
00166   /** \returns \c *this with scalar type casted to \a NewScalarType
00167     *
00168     * Note that if \a NewScalarType is equal to the current scalar type of \c *this
00169     * then this function smartly returns a const reference to \c *this.
00170     */
00171   template<typename NewScalarType>
00172   inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast () const
00173   {
00174     return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived());
00175   }
00176 
00177 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
00178 # include EIGEN_QUATERNIONBASE_PLUGIN
00179 #endif
00180 };
00181 
00182 /***************************************************************************
00183 * Definition/implementation of Quaternion<Scalar>
00184 ***************************************************************************/
00185 
00186 /** \geometry_module \ingroup Geometry_Module
00187   *
00188   * \class Quaternion
00189   *
00190   * \brief The quaternion class used to represent 3D orientations and rotations
00191   *
00192   * \tparam _Scalar the scalar type, i.e., the type of the coefficients
00193   * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign.
00194   *
00195   * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of
00196   * orientations and rotations of objects in three dimensions. Compared to other representations
00197   * like Euler angles or 3x3 matrices, quaternions offer the following advantages:
00198   * \li \b compact storage (4 scalars)
00199   * \li \b efficient to compose (28 flops),
00200   * \li \b stable spherical interpolation
00201   *
00202   * The following two typedefs are provided for convenience:
00203   * \li \c Quaternionf for \c float
00204   * \li \c Quaterniond for \c double
00205   *
00206   * \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized.
00207   *
00208   * \sa  class AngleAxis, class Transform
00209   */
00210 
00211 namespace internal {
00212 template<typename _Scalar,int _Options>
00213 struct traits<Quaternion<_Scalar,_Options> >
00214 {
00215   typedef Quaternion<_Scalar,_Options> PlainObject;
00216   typedef _Scalar Scalar;
00217   typedef Matrix<_Scalar,4,1,_Options> Coefficients;
00218   enum{
00219     IsAligned = internal::traits<Coefficients>::Flags & AlignedBit,
00220     Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit
00221   };
00222 };
00223 }
00224 
00225 template<typename _Scalar, int _Options>
00226 class Quaternion  : public QuaternionBase <Quaternion<_Scalar,_Options> >
00227 {
00228   typedef QuaternionBase<Quaternion<_Scalar,_Options>  > Base ;
00229   enum { IsAligned = internal::traits<Quaternion>::IsAligned };
00230 
00231 public:
00232   typedef _Scalar Scalar;
00233 
00234   EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion )
00235   using Base ::operator*=;
00236 
00237   typedef typename internal::traits<Quaternion >::Coefficients Coefficients;
00238   typedef typename Base ::AngleAxisType  AngleAxisType ;
00239 
00240   /** Default constructor leaving the quaternion uninitialized. */
00241   inline Quaternion () {}
00242 
00243   /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from
00244     * its four coefficients \a w, \a x, \a y and \a z.
00245     *
00246     * \warning Note the order of the arguments: the real \a w coefficient first,
00247     * while internally the coefficients are stored in the following order:
00248     * [\c x, \c y, \c z, \c w]
00249     */
00250   inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){}
00251 
00252   /** Constructs and initialize a quaternion from the array data */
00253   inline Quaternion(const Scalar* data) : m_coeffs(data) {}
00254 
00255   /** Copy constructor */
00256   template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived> & other) { this->Base::operator=(other); }
00257 
00258   /** Constructs and initializes a quaternion from the angle-axis \a aa */
00259   explicit inline Quaternion(const AngleAxisType & aa) { *this = aa; }
00260 
00261   /** Constructs and initializes a quaternion from either:
00262     *  - a rotation matrix expression,
00263     *  - a 4D vector expression representing quaternion coefficients.
00264     */
00265   template<typename Derived>
00266   explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
00267 
00268   /** Explicit copy constructor with scalar conversion */
00269   template<typename OtherScalar, int OtherOptions>
00270   explicit inline Quaternion (const Quaternion<OtherScalar, OtherOptions> & other)
00271   { m_coeffs = other.coeffs ().template cast<Scalar>(); }
00272 
00273   template<typename Derived1, typename Derived2>
00274   static Quaternion  FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00275 
00276   inline Coefficients& coeffs () { return m_coeffs;}
00277   inline const Coefficients& coeffs () const { return m_coeffs;}
00278 
00279   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
00280 
00281 protected:
00282   Coefficients m_coeffs;
00283   
00284 #ifndef EIGEN_PARSED_BY_DOXYGEN
00285     static EIGEN_STRONG_INLINE void _check_template_params()
00286     {
00287       EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
00288         INVALID_MATRIX_TEMPLATE_PARAMETERS)
00289     }
00290 #endif
00291 };
00292 
00293 /** \ingroup Geometry_Module
00294   * single precision quaternion type */
00295 typedef Quaternion<float>  Quaternionf;
00296 /** \ingroup Geometry_Module
00297   * double precision quaternion type */
00298 typedef Quaternion<double>  Quaterniond;
00299 
00300 /***************************************************************************
00301 * Specialization of Map<Quaternion<Scalar>>
00302 ***************************************************************************/
00303 
00304 namespace internal {
00305   template<typename _Scalar, int _Options>
00306   struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
00307   {
00308     typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients;
00309   };
00310 }
00311 
00312 namespace internal {
00313   template<typename _Scalar, int _Options>
00314   struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
00315   {
00316     typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
00317     typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase;
00318     enum {
00319       Flags = TraitsBase::Flags & ~LvalueBit
00320     };
00321   };
00322 }
00323 
00324 /** \ingroup Geometry_Module
00325   * \brief Quaternion expression mapping a constant memory buffer
00326   *
00327   * \tparam _Scalar the type of the Quaternion coefficients
00328   * \tparam _Options see class Map
00329   *
00330   * This is a specialization of class Map for Quaternion. This class allows to view
00331   * a 4 scalar memory buffer as an Eigen's Quaternion object.
00332   *
00333   * \sa class Map, class Quaternion, class QuaternionBase
00334   */
00335 template<typename _Scalar, int _Options>
00336 class Map<const Quaternion <_Scalar>, _Options >
00337   : public QuaternionBase <Map<const Quaternion<_Scalar>, _Options> >
00338 {
00339     typedef QuaternionBase<Map<const Quaternion<_Scalar> , _Options> > Base ;
00340 
00341   public:
00342     typedef _Scalar Scalar;
00343     typedef typename internal::traits<Map>::Coefficients Coefficients;
00344     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
00345     using Base ::operator*=;
00346 
00347     /** Constructs a Mapped Quaternion object from the pointer \a coeffs
00348       *
00349       * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
00350       * \code *coeffs == {x, y, z, w} \endcode
00351       *
00352       * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
00353     EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
00354 
00355     inline const Coefficients& coeffs () const { return m_coeffs;}
00356 
00357   protected:
00358     const Coefficients m_coeffs;
00359 };
00360 
00361 /** \ingroup Geometry_Module
00362   * \brief Expression of a quaternion from a memory buffer
00363   *
00364   * \tparam _Scalar the type of the Quaternion coefficients
00365   * \tparam _Options see class Map
00366   *
00367   * This is a specialization of class Map for Quaternion. This class allows to view
00368   * a 4 scalar memory buffer as an Eigen's  Quaternion object.
00369   *
00370   * \sa class Map, class Quaternion, class QuaternionBase
00371   */
00372 template<typename _Scalar, int _Options>
00373 class Map<Quaternion <_Scalar>, _Options >
00374   : public QuaternionBase <Map<Quaternion<_Scalar>, _Options> >
00375 {
00376     typedef QuaternionBase<Map<Quaternion<_Scalar> , _Options> > Base ;
00377 
00378   public:
00379     typedef _Scalar Scalar;
00380     typedef typename internal::traits<Map>::Coefficients Coefficients;
00381     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
00382     using Base ::operator*=;
00383 
00384     /** Constructs a Mapped Quaternion object from the pointer \a coeffs
00385       *
00386       * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order:
00387       * \code *coeffs == {x, y, z, w} \endcode
00388       *
00389       * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */
00390     EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
00391 
00392     inline Coefficients& coeffs () { return m_coeffs; }
00393     inline const Coefficients& coeffs () const { return m_coeffs; }
00394 
00395   protected:
00396     Coefficients m_coeffs;
00397 };
00398 
00399 /** \ingroup Geometry_Module
00400   * Map an unaligned array of single precision scalars as a quaternion */
00401 typedef Map<Quaternion<float>, 0>         QuaternionMapf;
00402 /** \ingroup Geometry_Module
00403   * Map an unaligned array of double precision scalars as a quaternion */
00404 typedef Map<Quaternion<double>, 0>        QuaternionMapd;
00405 /** \ingroup Geometry_Module
00406   * Map a 16-byte aligned array of single precision scalars as a quaternion */
00407 typedef Map<Quaternion<float>, Aligned>   QuaternionMapAlignedf;
00408 /** \ingroup Geometry_Module
00409   * Map a 16-byte aligned array of double precision scalars as a quaternion */
00410 typedef Map<Quaternion<double>, Aligned>  QuaternionMapAlignedd;
00411 
00412 /***************************************************************************
00413 * Implementation of QuaternionBase methods
00414 ***************************************************************************/
00415 
00416 // Generic Quaternion * Quaternion product
00417 // This product can be specialized for a given architecture via the Arch template argument.
00418 namespace internal {
00419 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product
00420 {
00421   static EIGEN_STRONG_INLINE Quaternion<Scalar>  run(const QuaternionBase<Derived1> & a, const QuaternionBase<Derived2> & b){
00422     return Quaternion<Scalar> 
00423     (
00424       a.w () * b.w () - a.x () * b.x () - a.y () * b.y () - a.z () * b.z (),
00425       a.w () * b.x () + a.x () * b.w () + a.y () * b.z () - a.z () * b.y (),
00426       a.w () * b.y () + a.y () * b.w () + a.z () * b.x () - a.x () * b.z (),
00427       a.w () * b.z () + a.z () * b.w () + a.x () * b.y () - a.y () * b.x ()
00428     );
00429   }
00430 };
00431 }
00432 
00433 /** \returns the concatenation of two rotations as a quaternion-quaternion product */
00434 template <class Derived>
00435 template <class OtherDerived>
00436 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
00437 QuaternionBase<Derived>::operator*  (const QuaternionBase<OtherDerived> & other) const
00438 {
00439   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
00440    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00441   return internal::quat_product<Architecture::Target, Derived, OtherDerived,
00442                          typename internal::traits<Derived>::Scalar,
00443                          internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other);
00444 }
00445 
00446 /** \sa operator*(Quaternion) */
00447 template <class Derived>
00448 template <class OtherDerived>
00449 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*=  (const QuaternionBase<OtherDerived> & other)
00450 {
00451   derived() = derived() * other.derived();
00452   return derived();
00453 }
00454 
00455 /** Rotation of a vector by a quaternion.
00456   * \remarks If the quaternion is used to rotate several points (>1)
00457   * then it is much more efficient to first convert it to a 3x3 Matrix.
00458   * Comparison of the operation cost for n transformations:
00459   *   - Quaternion2:    30n
00460   *   - Via a Matrix3: 24 + 15n
00461   */
00462 template <class Derived>
00463 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3 
00464 QuaternionBase<Derived>::_transformVector (const Vector3 & v) const
00465 {
00466     // Note that this algorithm comes from the optimization by hand
00467     // of the conversion to a Matrix followed by a Matrix/Vector product.
00468     // It appears to be much faster than the common algorithm found
00469     // in the literature (30 versus 39 flops). It also requires two
00470     // Vector3 as temporaries.
00471     Vector3  uv = this->vec().cross(v);
00472     uv += uv;
00473     return v + this->w() * uv + this->vec().cross(uv);
00474 }
00475 
00476 template<class Derived>
00477 EIGEN_STRONG_INLINE QuaternionBase<Derived> & QuaternionBase<Derived>::operator= (const QuaternionBase<Derived> & other)
00478 {
00479   coeffs() = other.coeffs ();
00480   return derived();
00481 }
00482 
00483 template<class Derived>
00484 template<class OtherDerived>
00485 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
00486 {
00487   coeffs() = other.coeffs();
00488   return derived();
00489 }
00490 
00491 /** Set \c *this from an angle-axis \a aa and returns a reference to \c *this
00492   */
00493 template<class Derived>
00494 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator= (const AngleAxisType & aa)
00495 {
00496   using std::cos;
00497   using std::sin;
00498   Scalar ha = Scalar(0.5)*aa.angle (); // Scalar(0.5) to suppress precision loss warnings
00499   this->w() = cos(ha);
00500   this->vec() = sin(ha) * aa.axis ();
00501   return derived();
00502 }
00503 
00504 /** Set \c *this from the expression \a xpr:
00505   *   - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion
00506   *   - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix
00507   *     and \a xpr is converted to a quaternion
00508   */
00509 
00510 template<class Derived>
00511 template<class MatrixDerived>
00512 inline Derived& QuaternionBase<Derived>::operator= (const MatrixBase<MatrixDerived>& xpr)
00513 {
00514   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
00515    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00516   internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
00517   return derived();
00518 }
00519 
00520 /** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to
00521   * be normalized, otherwise the result is undefined.
00522   */
00523 template<class Derived>
00524 inline typename QuaternionBase<Derived>::Matrix3
00525 QuaternionBase<Derived>::toRotationMatrix (void) const
00526 {
00527   // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
00528   // if not inlined then the cost of the return by value is huge ~ +35%,
00529   // however, not inlining this function is an order of magnitude slower, so
00530   // it has to be inlined, and so the return by value is not an issue
00531   Matrix3 res;
00532 
00533   const Scalar tx  = Scalar(2)*this->x();
00534   const Scalar ty  = Scalar(2)*this->y();
00535   const Scalar tz  = Scalar(2)*this->z();
00536   const Scalar twx = tx*this->w();
00537   const Scalar twy = ty*this->w();
00538   const Scalar twz = tz*this->w();
00539   const Scalar txx = tx*this->x();
00540   const Scalar txy = ty*this->x();
00541   const Scalar txz = tz*this->x();
00542   const Scalar tyy = ty*this->y();
00543   const Scalar tyz = tz*this->y();
00544   const Scalar tzz = tz*this->z();
00545 
00546   res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
00547   res.coeffRef(0,1) = txy-twz;
00548   res.coeffRef(0,2) = txz+twy;
00549   res.coeffRef(1,0) = txy+twz;
00550   res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
00551   res.coeffRef(1,2) = tyz-twx;
00552   res.coeffRef(2,0) = txz-twy;
00553   res.coeffRef(2,1) = tyz+twx;
00554   res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
00555 
00556   return res;
00557 }
00558 
00559 /** Sets \c *this to be a quaternion representing a rotation between
00560   * the two arbitrary vectors \a a and \a b. In other words, the built
00561   * rotation represent a rotation sending the line of direction \a a
00562   * to the line of direction \a b, both lines passing through the origin.
00563   *
00564   * \returns a reference to \c *this.
00565   *
00566   * Note that the two input vectors do \b not have to be normalized, and
00567   * do not need to have the same norm.
00568   */
00569 template<class Derived>
00570 template<typename Derived1, typename Derived2>
00571 inline Derived& QuaternionBase<Derived>::setFromTwoVectors (const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00572 {
00573   using std::max;
00574   using std::sqrt;
00575   Vector3  v0 = a.normalized ();
00576   Vector3  v1 = b.normalized ();
00577   Scalar c = v1.dot(v0);
00578 
00579   // if dot == -1, vectors are nearly opposites
00580   // => accurately compute the rotation axis by computing the
00581   //    intersection of the two planes. This is done by solving:
00582   //       x^T v0 = 0
00583   //       x^T v1 = 0
00584   //    under the constraint:
00585   //       ||x|| = 1
00586   //    which yields a singular value problem
00587   if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
00588   {
00589     c = (max)(c,Scalar(-1));
00590     Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
00591     JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
00592     Vector3  axis = svd.matrixV ().col(2);
00593 
00594     Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
00595     this->w() = sqrt(w2);
00596     this->vec() = axis * sqrt(Scalar(1) - w2);
00597     return derived();
00598   }
00599   Vector3  axis = v0.cross(v1);
00600   Scalar s = sqrt((Scalar(1)+c)*Scalar(2));
00601   Scalar invs = Scalar(1)/s;
00602   this->vec() = axis * invs;
00603   this->w() = s * Scalar(0.5);
00604 
00605   return derived();
00606 }
00607 
00608 
00609 /** Returns a quaternion representing a rotation between
00610   * the two arbitrary vectors \a a and \a b. In other words, the built
00611   * rotation represent a rotation sending the line of direction \a a
00612   * to the line of direction \a b, both lines passing through the origin.
00613   *
00614   * \returns resulting quaternion
00615   *
00616   * Note that the two input vectors do \b not have to be normalized, and
00617   * do not need to have the same norm.
00618   */
00619 template<typename Scalar, int Options>
00620 template<typename Derived1, typename Derived2>
00621 Quaternion<Scalar,Options>  Quaternion<Scalar,Options>::FromTwoVectors (const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00622 {
00623     Quaternion  quat;
00624     quat.setFromTwoVectors(a, b);
00625     return quat;
00626 }
00627 
00628 
00629 /** \returns the multiplicative inverse of \c *this
00630   * Note that in most cases, i.e., if you simply want the opposite rotation,
00631   * and/or the quaternion is normalized, then it is enough to use the conjugate.
00632   *
00633   * \sa QuaternionBase::conjugate()
00634   */
00635 template <class Derived>
00636 inline Quaternion<typename internal::traits<Derived>::Scalar > QuaternionBase<Derived>::inverse () const
00637 {
00638   // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite()  ??
00639   Scalar n2 = this->squaredNorm();
00640   if (n2 > Scalar(0))
00641     return Quaternion<Scalar> (conjugate().coeffs() / n2);
00642   else
00643   {
00644     // return an invalid result to flag the error
00645     return Quaternion<Scalar> (Coefficients::Zero());
00646   }
00647 }
00648 
00649 /** \returns the conjugate of the \c *this which is equal to the multiplicative inverse
00650   * if the quaternion is normalized.
00651   * The conjugate of a quaternion represents the opposite rotation.
00652   *
00653   * \sa Quaternion2::inverse()
00654   */
00655 template <class Derived>
00656 inline Quaternion<typename internal::traits<Derived>::Scalar >
00657 QuaternionBase<Derived>::conjugate () const
00658 {
00659   return Quaternion<Scalar> (this->w(),-this->x(),-this->y(),-this->z());
00660 }
00661 
00662 /** \returns the angle (in radian) between two rotations
00663   * \sa dot()
00664   */
00665 template <class Derived>
00666 template <class OtherDerived>
00667 inline typename internal::traits<Derived>::Scalar
00668 QuaternionBase<Derived>::angularDistance (const QuaternionBase<OtherDerived> & other) const
00669 {
00670   using std::atan2;
00671   using std::abs;
00672   Quaternion<Scalar>  d = (*this) * other.conjugate ();
00673   return Scalar(2) * atan2( d.vec ().norm(), abs(d.w ()) );
00674 }
00675 
00676  
00677     
00678 /** \returns the spherical linear interpolation between the two quaternions
00679   * \c *this and \a other at the parameter \a t in [0;1].
00680   * 
00681   * This represents an interpolation for a constant motion between \c *this and \a other,
00682   * see also http://en.wikipedia.org/wiki/Slerp.
00683   */
00684 template <class Derived>
00685 template <class OtherDerived>
00686 Quaternion<typename internal::traits<Derived>::Scalar >
00687 QuaternionBase<Derived>::slerp (const Scalar& t, const QuaternionBase<OtherDerived> & other) const
00688 {
00689   using std::acos;
00690   using std::sin;
00691   using std::abs;
00692   static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
00693   Scalar d = this->dot(other);
00694   Scalar absD = abs(d);
00695 
00696   Scalar scale0;
00697   Scalar scale1;
00698 
00699   if(absD>=one)
00700   {
00701     scale0 = Scalar(1) - t;
00702     scale1 = t;
00703   }
00704   else
00705   {
00706     // theta is the angle between the 2 quaternions
00707     Scalar theta = acos(absD);
00708     Scalar sinTheta = sin(theta);
00709 
00710     scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;
00711     scale1 = sin( ( t * theta) ) / sinTheta;
00712   }
00713   if(d<Scalar(0)) scale1 = -scale1;
00714 
00715   return Quaternion<Scalar> (scale0 * coeffs() + scale1 * other.coeffs ());
00716 }
00717 
00718 namespace internal {
00719 
00720 // set from a rotation matrix
00721 template<typename Other>
00722 struct quaternionbase_assign_impl<Other,3,3>
00723 {
00724   typedef typename Other::Scalar Scalar;
00725   typedef DenseIndex Index;
00726   template<class Derived> static inline void run(QuaternionBase<Derived> & q, const Other& mat)
00727   {
00728     using std::sqrt;
00729     // This algorithm comes from  "Quaternion Calculus and Fast Animation",
00730     // Ken Shoemake, 1987 SIGGRAPH course notes
00731     Scalar t = mat.trace();
00732     if (t > Scalar(0))
00733     {
00734       t = sqrt(t + Scalar(1.0));
00735       q.w () = Scalar(0.5)*t;
00736       t = Scalar(0.5)/t;
00737       q.x () = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
00738       q.y () = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
00739       q.z () = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
00740     }
00741     else
00742     {
00743       DenseIndex i = 0;
00744       if (mat.coeff(1,1) > mat.coeff(0,0))
00745         i = 1;
00746       if (mat.coeff(2,2) > mat.coeff(i,i))
00747         i = 2;
00748       DenseIndex j = (i+1)%3;
00749       DenseIndex k = (j+1)%3;
00750 
00751       t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
00752       q.coeffs ().coeffRef(i) = Scalar(0.5) * t;
00753       t = Scalar(0.5)/t;
00754       q.w () = (mat.coeff(k,j)-mat.coeff(j,k))*t;
00755       q.coeffs ().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
00756       q.coeffs ().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
00757     }
00758   }
00759 };
00760 
00761 // set from a vector of coefficients assumed to be a quaternion
00762 template<typename Other>
00763 struct quaternionbase_assign_impl<Other,4,1>
00764 {
00765   typedef typename Other::Scalar Scalar;
00766   template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec)
00767   {
00768     q.coeffs() = vec;
00769   }
00770 };
00771 
00772 } // end namespace internal
00773 
00774 } // end namespace Eigen
00775 
00776 #endif // EIGEN_QUATERNION_H