Eigne Matrix Class Library
Dependents: Eigen_test Odometry_test AttitudeEstimation_usingTicker MPU9250_Quaternion_Binary_Serial ... more
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
Generated on Tue Jul 12 2022 17:46:59 by 1.7.2