Eigen  3.3.3
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 
00034 template<class Derived>
00035 class QuaternionBase : public RotationBase<Derived, 3>
00036 {
00037  public:
00038   typedef RotationBase<Derived, 3> Base;
00039 
00040   using Base::operator*;
00041   using Base::derived;
00042 
00043   typedef typename internal::traits<Derived>::Scalar Scalar;
00044   typedef typename NumTraits<Scalar>::Real RealScalar;
00045   typedef typename internal::traits<Derived>::Coefficients Coefficients;
00046   enum {
00047     Flags = Eigen::internal::traits<Derived>::Flags
00048   };
00049 
00050  // typedef typename Matrix<Scalar,4,1> Coefficients;
00052   typedef Matrix<Scalar,3,1> Vector3;
00054   typedef Matrix<Scalar,3,3> Matrix3;
00056   typedef AngleAxis<Scalar> AngleAxisType;
00057 
00058 
00059 
00061   EIGEN_DEVICE_FUNC inline Scalar x() const { return this->derived().coeffs().coeff(0); }
00063   EIGEN_DEVICE_FUNC inline Scalar y() const { return this->derived().coeffs().coeff(1); }
00065   EIGEN_DEVICE_FUNC inline Scalar z() const { return this->derived().coeffs().coeff(2); }
00067   EIGEN_DEVICE_FUNC inline Scalar w() const { return this->derived().coeffs().coeff(3); }
00068 
00070   EIGEN_DEVICE_FUNC inline Scalar& x() { return this->derived().coeffs().coeffRef(0); }
00072   EIGEN_DEVICE_FUNC inline Scalar& y() { return this->derived().coeffs().coeffRef(1); }
00074   EIGEN_DEVICE_FUNC inline Scalar& z() { return this->derived().coeffs().coeffRef(2); }
00076   EIGEN_DEVICE_FUNC inline Scalar& w() { return this->derived().coeffs().coeffRef(3); }
00077 
00079   EIGEN_DEVICE_FUNC inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
00080 
00082   EIGEN_DEVICE_FUNC inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); }
00083 
00085   EIGEN_DEVICE_FUNC inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); }
00086 
00088   EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
00089 
00090   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
00091   template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
00092 
00093 // disabled this copy operator as it is giving very strange compilation errors when compiling
00094 // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's
00095 // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase
00096 // we didn't have to add, in addition to templated operator=, such a non-templated copy operator.
00097 //  Derived& operator=(const QuaternionBase& other)
00098 //  { return operator=<Derived>(other); }
00099 
00100   EIGEN_DEVICE_FUNC Derived& operator=(const AngleAxisType& aa);
00101   template<class OtherDerived> EIGEN_DEVICE_FUNC Derived& operator=(const MatrixBase<OtherDerived>& m);
00102 
00106   EIGEN_DEVICE_FUNC static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); }
00107 
00110   EIGEN_DEVICE_FUNC inline QuaternionBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1); return *this; }
00111 
00115   EIGEN_DEVICE_FUNC inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
00116 
00120   EIGEN_DEVICE_FUNC inline Scalar norm() const { return coeffs().norm(); }
00121 
00124   EIGEN_DEVICE_FUNC inline void normalize() { coeffs().normalize(); }
00127   EIGEN_DEVICE_FUNC inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
00128 
00134   template<class OtherDerived> EIGEN_DEVICE_FUNC inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
00135 
00136   template<class OtherDerived> EIGEN_DEVICE_FUNC Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
00137 
00139   EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix() const;
00140 
00142   template<typename Derived1, typename Derived2>
00143   EIGEN_DEVICE_FUNC Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00144 
00145   template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
00146   template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
00147 
00149   EIGEN_DEVICE_FUNC Quaternion<Scalar> inverse() const;
00150 
00152   EIGEN_DEVICE_FUNC Quaternion<Scalar> conjugate() const;
00153 
00154   template<class OtherDerived> EIGEN_DEVICE_FUNC Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const;
00155 
00160   template<class OtherDerived>
00161   EIGEN_DEVICE_FUNC bool isApprox(const QuaternionBase<OtherDerived>& other, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
00162   { return coeffs().isApprox(other.coeffs(), prec); }
00163 
00165   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const;
00166 
00172   template<typename NewScalarType>
00173   EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const
00174   {
00175     return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived());
00176   }
00177 
00178 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
00179 # include EIGEN_QUATERNIONBASE_PLUGIN
00180 #endif
00181 };
00182 
00183 /***************************************************************************
00184 * Definition/implementation of Quaternion<Scalar>
00185 ***************************************************************************/
00186 
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     Alignment = internal::traits<Coefficients>::Alignment,
00221     Flags = LvalueBit
00222   };
00223 };
00224 }
00225 
00226 template<typename _Scalar, int _Options>
00227 class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> >
00228 {
00229 public:
00230   typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base;
00231   enum { NeedsAlignment = internal::traits<Quaternion>::Alignment>0 };
00232 
00233   typedef _Scalar Scalar;
00234 
00235   EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion)
00236   using Base::operator*=;
00237 
00238   typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
00239   typedef typename Base::AngleAxisType AngleAxisType;
00240 
00242   EIGEN_DEVICE_FUNC inline Quaternion() {}
00243 
00251   EIGEN_DEVICE_FUNC inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){}
00252 
00254   EIGEN_DEVICE_FUNC explicit inline Quaternion(const Scalar* data) : m_coeffs(data) {}
00255 
00257   template<class Derived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
00258 
00260   EIGEN_DEVICE_FUNC explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
00261 
00266   template<typename Derived>
00267   EIGEN_DEVICE_FUNC explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
00268 
00270   template<typename OtherScalar, int OtherOptions>
00271   EIGEN_DEVICE_FUNC explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
00272   { m_coeffs = other.coeffs().template cast<Scalar>(); }
00273 
00274   EIGEN_DEVICE_FUNC static Quaternion UnitRandom();
00275 
00276   template<typename Derived1, typename Derived2>
00277   EIGEN_DEVICE_FUNC static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00278 
00279   EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs;}
00280   EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs;}
00281 
00282   EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(NeedsAlignment))
00283   
00284 #ifdef EIGEN_QUATERNION_PLUGIN
00285 # include EIGEN_QUATERNION_PLUGIN
00286 #endif
00287 
00288 protected:
00289   Coefficients m_coeffs;
00290   
00291 #ifndef EIGEN_PARSED_BY_DOXYGEN
00292     static EIGEN_STRONG_INLINE void _check_template_params()
00293     {
00294       EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
00295         INVALID_MATRIX_TEMPLATE_PARAMETERS)
00296     }
00297 #endif
00298 };
00299 
00302 typedef Quaternion<float> Quaternionf;
00305 typedef Quaternion<double> Quaterniond;
00306 
00307 /***************************************************************************
00308 * Specialization of Map<Quaternion<Scalar>>
00309 ***************************************************************************/
00310 
00311 namespace internal {
00312   template<typename _Scalar, int _Options>
00313   struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
00314   {
00315     typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients;
00316   };
00317 }
00318 
00319 namespace internal {
00320   template<typename _Scalar, int _Options>
00321   struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
00322   {
00323     typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
00324     typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase;
00325     enum {
00326       Flags = TraitsBase::Flags & ~LvalueBit
00327     };
00328   };
00329 }
00330 
00342 template<typename _Scalar, int _Options>
00343 class Map<const Quaternion<_Scalar>, _Options >
00344   : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
00345 {
00346   public:
00347     typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base;
00348 
00349     typedef _Scalar Scalar;
00350     typedef typename internal::traits<Map>::Coefficients Coefficients;
00351     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
00352     using Base::operator*=;
00353 
00360     EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
00361 
00362     EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs;}
00363 
00364   protected:
00365     const Coefficients m_coeffs;
00366 };
00367 
00379 template<typename _Scalar, int _Options>
00380 class Map<Quaternion<_Scalar>, _Options >
00381   : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> >
00382 {
00383   public:
00384     typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base;
00385 
00386     typedef _Scalar Scalar;
00387     typedef typename internal::traits<Map>::Coefficients Coefficients;
00388     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
00389     using Base::operator*=;
00390 
00397     EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
00398 
00399     EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; }
00400     EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; }
00401 
00402   protected:
00403     Coefficients m_coeffs;
00404 };
00405 
00408 typedef Map<Quaternion<float>, 0>         QuaternionMapf;
00411 typedef Map<Quaternion<double>, 0>        QuaternionMapd;
00414 typedef Map<Quaternion<float>, Aligned>   QuaternionMapAlignedf;
00417 typedef Map<Quaternion<double>, Aligned>  QuaternionMapAlignedd;
00418 
00419 /***************************************************************************
00420 * Implementation of QuaternionBase methods
00421 ***************************************************************************/
00422 
00423 // Generic Quaternion * Quaternion product
00424 // This product can be specialized for a given architecture via the Arch template argument.
00425 namespace internal {
00426 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product
00427 {
00428   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
00429     return Quaternion<Scalar>
00430     (
00431       a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
00432       a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
00433       a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
00434       a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
00435     );
00436   }
00437 };
00438 }
00439 
00441 template <class Derived>
00442 template <class OtherDerived>
00443 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
00444 QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const
00445 {
00446   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
00447    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00448   return internal::quat_product<Architecture::Target, Derived, OtherDerived,
00449                          typename internal::traits<Derived>::Scalar,
00450                          EIGEN_PLAIN_ENUM_MIN(internal::traits<Derived>::Alignment, internal::traits<OtherDerived>::Alignment)>::run(*this, other);
00451 }
00452 
00454 template <class Derived>
00455 template <class OtherDerived>
00456 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
00457 {
00458   derived() = derived() * other.derived();
00459   return derived();
00460 }
00461 
00469 template <class Derived>
00470 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
00471 QuaternionBase<Derived>::_transformVector(const Vector3& v) const
00472 {
00473     // Note that this algorithm comes from the optimization by hand
00474     // of the conversion to a Matrix followed by a Matrix/Vector product.
00475     // It appears to be much faster than the common algorithm found
00476     // in the literature (30 versus 39 flops). It also requires two
00477     // Vector3 as temporaries.
00478     Vector3 uv = this->vec().cross(v);
00479     uv += uv;
00480     return v + this->w() * uv + this->vec().cross(uv);
00481 }
00482 
00483 template<class Derived>
00484 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other)
00485 {
00486   coeffs() = other.coeffs();
00487   return derived();
00488 }
00489 
00490 template<class Derived>
00491 template<class OtherDerived>
00492 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
00493 {
00494   coeffs() = other.coeffs();
00495   return derived();
00496 }
00497 
00500 template<class Derived>
00501 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
00502 {
00503   EIGEN_USING_STD_MATH(cos)
00504   EIGEN_USING_STD_MATH(sin)
00505   Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
00506   this->w() = cos(ha);
00507   this->vec() = sin(ha) * aa.axis();
00508   return derived();
00509 }
00510 
00517 template<class Derived>
00518 template<class MatrixDerived>
00519 EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr)
00520 {
00521   EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
00522    YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00523   internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
00524   return derived();
00525 }
00526 
00530 template<class Derived>
00531 EIGEN_DEVICE_FUNC inline typename QuaternionBase<Derived>::Matrix3
00532 QuaternionBase<Derived>::toRotationMatrix(void) const
00533 {
00534   // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
00535   // if not inlined then the cost of the return by value is huge ~ +35%,
00536   // however, not inlining this function is an order of magnitude slower, so
00537   // it has to be inlined, and so the return by value is not an issue
00538   Matrix3 res;
00539 
00540   const Scalar tx  = Scalar(2)*this->x();
00541   const Scalar ty  = Scalar(2)*this->y();
00542   const Scalar tz  = Scalar(2)*this->z();
00543   const Scalar twx = tx*this->w();
00544   const Scalar twy = ty*this->w();
00545   const Scalar twz = tz*this->w();
00546   const Scalar txx = tx*this->x();
00547   const Scalar txy = ty*this->x();
00548   const Scalar txz = tz*this->x();
00549   const Scalar tyy = ty*this->y();
00550   const Scalar tyz = tz*this->y();
00551   const Scalar tzz = tz*this->z();
00552 
00553   res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
00554   res.coeffRef(0,1) = txy-twz;
00555   res.coeffRef(0,2) = txz+twy;
00556   res.coeffRef(1,0) = txy+twz;
00557   res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
00558   res.coeffRef(1,2) = tyz-twx;
00559   res.coeffRef(2,0) = txz-twy;
00560   res.coeffRef(2,1) = tyz+twx;
00561   res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
00562 
00563   return res;
00564 }
00565 
00576 template<class Derived>
00577 template<typename Derived1, typename Derived2>
00578 EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00579 {
00580   EIGEN_USING_STD_MATH(sqrt)
00581   Vector3 v0 = a.normalized();
00582   Vector3 v1 = b.normalized();
00583   Scalar c = v1.dot(v0);
00584 
00585   // if dot == -1, vectors are nearly opposites
00586   // => accurately compute the rotation axis by computing the
00587   //    intersection of the two planes. This is done by solving:
00588   //       x^T v0 = 0
00589   //       x^T v1 = 0
00590   //    under the constraint:
00591   //       ||x|| = 1
00592   //    which yields a singular value problem
00593   if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
00594   {
00595     c = numext::maxi(c,Scalar(-1));
00596     Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
00597     JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
00598     Vector3 axis = svd.matrixV().col(2);
00599 
00600     Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
00601     this->w() = sqrt(w2);
00602     this->vec() = axis * sqrt(Scalar(1) - w2);
00603     return derived();
00604   }
00605   Vector3 axis = v0.cross(v1);
00606   Scalar s = sqrt((Scalar(1)+c)*Scalar(2));
00607   Scalar invs = Scalar(1)/s;
00608   this->vec() = axis * invs;
00609   this->w() = s * Scalar(0.5);
00610 
00611   return derived();
00612 }
00613 
00618 template<typename Scalar, int Options>
00619 EIGEN_DEVICE_FUNC Quaternion<Scalar,Options> Quaternion<Scalar,Options>::UnitRandom()
00620 {
00621   EIGEN_USING_STD_MATH(sqrt)
00622   EIGEN_USING_STD_MATH(sin)
00623   EIGEN_USING_STD_MATH(cos)
00624   const Scalar u1 = internal::random<Scalar>(0, 1),
00625                u2 = internal::random<Scalar>(0, 2*EIGEN_PI),
00626                u3 = internal::random<Scalar>(0, 2*EIGEN_PI);
00627   const Scalar a = sqrt(1 - u1),
00628                b = sqrt(u1);
00629   return Quaternion (a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3));
00630 }
00631 
00632 
00643 template<typename Scalar, int Options>
00644 template<typename Derived1, typename Derived2>
00645 EIGEN_DEVICE_FUNC 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 EIGEN_DEVICE_FUNC 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 > Scalar(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 
00673 // Generic conjugate of a Quaternion
00674 namespace internal {
00675 template<int Arch, class Derived, typename Scalar, int _Options> struct quat_conj
00676 {
00677   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived>& q){
00678     return Quaternion<Scalar>(q.w(),-q.x(),-q.y(),-q.z());
00679   }
00680 };
00681 }
00682                          
00689 template <class Derived>
00690 EIGEN_DEVICE_FUNC inline Quaternion<typename internal::traits<Derived>::Scalar>
00691 QuaternionBase<Derived>::conjugate() const
00692 {
00693   return internal::quat_conj<Architecture::Target, Derived,
00694                          typename internal::traits<Derived>::Scalar,
00695                          internal::traits<Derived>::Alignment>::run(*this);
00696                          
00697 }
00698 
00702 template <class Derived>
00703 template <class OtherDerived>
00704 EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar
00705 QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const
00706 {
00707   EIGEN_USING_STD_MATH(atan2)
00708   Quaternion<Scalar> d = (*this) * other.conjugate();
00709   return Scalar(2) * atan2( d.vec().norm(), numext::abs(d.w()) );
00710 }
00711 
00712  
00713     
00720 template <class Derived>
00721 template <class OtherDerived>
00722 EIGEN_DEVICE_FUNC Quaternion<typename internal::traits<Derived>::Scalar>
00723 QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const
00724 {
00725   EIGEN_USING_STD_MATH(acos)
00726   EIGEN_USING_STD_MATH(sin)
00727   const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
00728   Scalar d = this->dot(other);
00729   Scalar absD = numext::abs(d);
00730 
00731   Scalar scale0;
00732   Scalar scale1;
00733 
00734   if(absD>=one)
00735   {
00736     scale0 = Scalar(1) - t;
00737     scale1 = t;
00738   }
00739   else
00740   {
00741     // theta is the angle between the 2 quaternions
00742     Scalar theta = acos(absD);
00743     Scalar sinTheta = sin(theta);
00744 
00745     scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;
00746     scale1 = sin( ( t * theta) ) / sinTheta;
00747   }
00748   if(d<Scalar(0)) scale1 = -scale1;
00749 
00750   return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
00751 }
00752 
00753 namespace internal {
00754 
00755 // set from a rotation matrix
00756 template<typename Other>
00757 struct quaternionbase_assign_impl<Other,3,3>
00758 {
00759   typedef typename Other::Scalar Scalar;
00760   template<class Derived> EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& a_mat)
00761   {
00762     const typename internal::nested_eval<Other,2>::type mat(a_mat);
00763     EIGEN_USING_STD_MATH(sqrt)
00764     // This algorithm comes from  "Quaternion Calculus and Fast Animation",
00765     // Ken Shoemake, 1987 SIGGRAPH course notes
00766     Scalar t = mat.trace();
00767     if (t > Scalar(0))
00768     {
00769       t = sqrt(t + Scalar(1.0));
00770       q.w() = Scalar(0.5)*t;
00771       t = Scalar(0.5)/t;
00772       q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
00773       q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
00774       q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
00775     }
00776     else
00777     {
00778       Index i = 0;
00779       if (mat.coeff(1,1) > mat.coeff(0,0))
00780         i = 1;
00781       if (mat.coeff(2,2) > mat.coeff(i,i))
00782         i = 2;
00783       Index j = (i+1)%3;
00784       Index k = (j+1)%3;
00785 
00786       t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
00787       q.coeffs().coeffRef(i) = Scalar(0.5) * t;
00788       t = Scalar(0.5)/t;
00789       q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
00790       q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
00791       q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
00792     }
00793   }
00794 };
00795 
00796 // set from a vector of coefficients assumed to be a quaternion
00797 template<typename Other>
00798 struct quaternionbase_assign_impl<Other,4,1>
00799 {
00800   typedef typename Other::Scalar Scalar;
00801   template<class Derived> EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& vec)
00802   {
00803     q.coeffs() = vec;
00804   }
00805 };
00806 
00807 } // end namespace internal
00808 
00809 } // end namespace Eigen
00810 
00811 #endif // EIGEN_QUATERNION_H
 All Classes Functions Variables Typedefs Enumerations Enumerator Friends