1// This file is part of Eigen, a lightweight C++ template library 2// for linear algebra. 3// 4// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr> 5// Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr> 6// 7// This Source Code Form is subject to the terms of the Mozilla 8// Public License v. 2.0. If a copy of the MPL was not distributed 9// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 10 11#ifndef EIGEN_QUATERNION_H 12#define EIGEN_QUATERNION_H 13namespace Eigen { 14 15 16/*************************************************************************** 17* Definition of QuaternionBase<Derived> 18* The implementation is at the end of the file 19***************************************************************************/ 20 21namespace internal { 22template<typename Other, 23 int OtherRows=Other::RowsAtCompileTime, 24 int OtherCols=Other::ColsAtCompileTime> 25struct quaternionbase_assign_impl; 26} 27 28/** \geometry_module \ingroup Geometry_Module 29 * \class QuaternionBase 30 * \brief Base class for quaternion expressions 31 * \tparam Derived derived type (CRTP) 32 * \sa class Quaternion 33 */ 34template<class Derived> 35class QuaternionBase : public RotationBase<Derived, 3> 36{ 37 typedef RotationBase<Derived, 3> Base; 38public: 39 using Base::operator*; 40 using Base::derived; 41 42 typedef typename internal::traits<Derived>::Scalar Scalar; 43 typedef typename NumTraits<Scalar>::Real RealScalar; 44 typedef typename internal::traits<Derived>::Coefficients Coefficients; 45 enum { 46 Flags = Eigen::internal::traits<Derived>::Flags 47 }; 48 49 // typedef typename Matrix<Scalar,4,1> Coefficients; 50 /** the type of a 3D vector */ 51 typedef Matrix<Scalar,3,1> Vector3; 52 /** the equivalent rotation matrix type */ 53 typedef Matrix<Scalar,3,3> Matrix3; 54 /** the equivalent angle-axis type */ 55 typedef AngleAxis<Scalar> AngleAxisType; 56 57 58 59 /** \returns the \c x coefficient */ 60 inline Scalar x() const { return this->derived().coeffs().coeff(0); } 61 /** \returns the \c y coefficient */ 62 inline Scalar y() const { return this->derived().coeffs().coeff(1); } 63 /** \returns the \c z coefficient */ 64 inline Scalar z() const { return this->derived().coeffs().coeff(2); } 65 /** \returns the \c w coefficient */ 66 inline Scalar w() const { return this->derived().coeffs().coeff(3); } 67 68 /** \returns a reference to the \c x coefficient */ 69 inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } 70 /** \returns a reference to the \c y coefficient */ 71 inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } 72 /** \returns a reference to the \c z coefficient */ 73 inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } 74 /** \returns a reference to the \c w coefficient */ 75 inline Scalar& w() { return this->derived().coeffs().coeffRef(3); } 76 77 /** \returns a read-only vector expression of the imaginary part (x,y,z) */ 78 inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); } 79 80 /** \returns a vector expression of the imaginary part (x,y,z) */ 81 inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); } 82 83 /** \returns a read-only vector expression of the coefficients (x,y,z,w) */ 84 inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); } 85 86 /** \returns a vector expression of the coefficients (x,y,z,w) */ 87 inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); } 88 89 EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other); 90 template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other); 91 92// disabled this copy operator as it is giving very strange compilation errors when compiling 93// test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's 94// useful; however notice that we already have the templated operator= above and e.g. in MatrixBase 95// we didn't have to add, in addition to templated operator=, such a non-templated copy operator. 96// Derived& operator=(const QuaternionBase& other) 97// { return operator=<Derived>(other); } 98 99 Derived& operator=(const AngleAxisType& aa); 100 template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m); 101 102 /** \returns a quaternion representing an identity rotation 103 * \sa MatrixBase::Identity() 104 */ 105 static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); } 106 107 /** \sa QuaternionBase::Identity(), MatrixBase::setIdentity() 108 */ 109 inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; } 110 111 /** \returns the squared norm of the quaternion's coefficients 112 * \sa QuaternionBase::norm(), MatrixBase::squaredNorm() 113 */ 114 inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } 115 116 /** \returns the norm of the quaternion's coefficients 117 * \sa QuaternionBase::squaredNorm(), MatrixBase::norm() 118 */ 119 inline Scalar norm() const { return coeffs().norm(); } 120 121 /** Normalizes the quaternion \c *this 122 * \sa normalized(), MatrixBase::normalize() */ 123 inline void normalize() { coeffs().normalize(); } 124 /** \returns a normalized copy of \c *this 125 * \sa normalize(), MatrixBase::normalized() */ 126 inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); } 127 128 /** \returns the dot product of \c *this and \a other 129 * Geometrically speaking, the dot product of two unit quaternions 130 * corresponds to the cosine of half the angle between the two rotations. 131 * \sa angularDistance() 132 */ 133 template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); } 134 135 template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const; 136 137 /** \returns an equivalent 3x3 rotation matrix */ 138 Matrix3 toRotationMatrix() const; 139 140 /** \returns the quaternion which transform \a a into \a b through a rotation */ 141 template<typename Derived1, typename Derived2> 142 Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); 143 144 template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const; 145 template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q); 146 147 /** \returns the quaternion describing the inverse rotation */ 148 Quaternion<Scalar> inverse() const; 149 150 /** \returns the conjugated quaternion */ 151 Quaternion<Scalar> conjugate() const; 152 153 template<class OtherDerived> Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const; 154 155 /** \returns \c true if \c *this is approximately equal to \a other, within the precision 156 * determined by \a prec. 157 * 158 * \sa MatrixBase::isApprox() */ 159 template<class OtherDerived> 160 bool isApprox(const QuaternionBase<OtherDerived>& other, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const 161 { return coeffs().isApprox(other.coeffs(), prec); } 162 163 /** return the result vector of \a v through the rotation*/ 164 EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const; 165 166 /** \returns \c *this with scalar type casted to \a NewScalarType 167 * 168 * Note that if \a NewScalarType is equal to the current scalar type of \c *this 169 * then this function smartly returns a const reference to \c *this. 170 */ 171 template<typename NewScalarType> 172 inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const 173 { 174 return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived()); 175 } 176 177#ifdef EIGEN_QUATERNIONBASE_PLUGIN 178# include EIGEN_QUATERNIONBASE_PLUGIN 179#endif 180}; 181 182/*************************************************************************** 183* Definition/implementation of Quaternion<Scalar> 184***************************************************************************/ 185 186/** \geometry_module \ingroup Geometry_Module 187 * 188 * \class Quaternion 189 * 190 * \brief The quaternion class used to represent 3D orientations and rotations 191 * 192 * \tparam _Scalar the scalar type, i.e., the type of the coefficients 193 * \tparam _Options controls the memory alignment of the coefficients. Can be \# AutoAlign or \# DontAlign. Default is AutoAlign. 194 * 195 * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of 196 * orientations and rotations of objects in three dimensions. Compared to other representations 197 * like Euler angles or 3x3 matrices, quaternions offer the following advantages: 198 * \li \b compact storage (4 scalars) 199 * \li \b efficient to compose (28 flops), 200 * \li \b stable spherical interpolation 201 * 202 * The following two typedefs are provided for convenience: 203 * \li \c Quaternionf for \c float 204 * \li \c Quaterniond for \c double 205 * 206 * \warning Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized. 207 * 208 * \sa class AngleAxis, class Transform 209 */ 210 211namespace internal { 212template<typename _Scalar,int _Options> 213struct traits<Quaternion<_Scalar,_Options> > 214{ 215 typedef Quaternion<_Scalar,_Options> PlainObject; 216 typedef _Scalar Scalar; 217 typedef Matrix<_Scalar,4,1,_Options> Coefficients; 218 enum{ 219 IsAligned = internal::traits<Coefficients>::Flags & AlignedBit, 220 Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit 221 }; 222}; 223} 224 225template<typename _Scalar, int _Options> 226class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> > 227{ 228 typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base; 229 enum { IsAligned = internal::traits<Quaternion>::IsAligned }; 230 231public: 232 typedef _Scalar Scalar; 233 234 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion) 235 using Base::operator*=; 236 237 typedef typename internal::traits<Quaternion>::Coefficients Coefficients; 238 typedef typename Base::AngleAxisType AngleAxisType; 239 240 /** Default constructor leaving the quaternion uninitialized. */ 241 inline Quaternion() {} 242 243 /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from 244 * its four coefficients \a w, \a x, \a y and \a z. 245 * 246 * \warning Note the order of the arguments: the real \a w coefficient first, 247 * while internally the coefficients are stored in the following order: 248 * [\c x, \c y, \c z, \c w] 249 */ 250 inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){} 251 252 /** Constructs and initialize a quaternion from the array data */ 253 inline Quaternion(const Scalar* data) : m_coeffs(data) {} 254 255 /** Copy constructor */ 256 template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); } 257 258 /** Constructs and initializes a quaternion from the angle-axis \a aa */ 259 explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; } 260 261 /** Constructs and initializes a quaternion from either: 262 * - a rotation matrix expression, 263 * - a 4D vector expression representing quaternion coefficients. 264 */ 265 template<typename Derived> 266 explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; } 267 268 /** Explicit copy constructor with scalar conversion */ 269 template<typename OtherScalar, int OtherOptions> 270 explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other) 271 { m_coeffs = other.coeffs().template cast<Scalar>(); } 272 273 template<typename Derived1, typename Derived2> 274 static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); 275 276 inline Coefficients& coeffs() { return m_coeffs;} 277 inline const Coefficients& coeffs() const { return m_coeffs;} 278 279 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned) 280 281protected: 282 Coefficients m_coeffs; 283 284#ifndef EIGEN_PARSED_BY_DOXYGEN 285 static EIGEN_STRONG_INLINE void _check_template_params() 286 { 287 EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options, 288 INVALID_MATRIX_TEMPLATE_PARAMETERS) 289 } 290#endif 291}; 292 293/** \ingroup Geometry_Module 294 * single precision quaternion type */ 295typedef Quaternion<float> Quaternionf; 296/** \ingroup Geometry_Module 297 * double precision quaternion type */ 298typedef Quaternion<double> Quaterniond; 299 300/*************************************************************************** 301* Specialization of Map<Quaternion<Scalar>> 302***************************************************************************/ 303 304namespace internal { 305 template<typename _Scalar, int _Options> 306 struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > 307 { 308 typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients; 309 }; 310} 311 312namespace internal { 313 template<typename _Scalar, int _Options> 314 struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > 315 { 316 typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients; 317 typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase; 318 enum { 319 Flags = TraitsBase::Flags & ~LvalueBit 320 }; 321 }; 322} 323 324/** \ingroup Geometry_Module 325 * \brief Quaternion expression mapping a constant memory buffer 326 * 327 * \tparam _Scalar the type of the Quaternion coefficients 328 * \tparam _Options see class Map 329 * 330 * This is a specialization of class Map for Quaternion. This class allows to view 331 * a 4 scalar memory buffer as an Eigen's Quaternion object. 332 * 333 * \sa class Map, class Quaternion, class QuaternionBase 334 */ 335template<typename _Scalar, int _Options> 336class Map<const Quaternion<_Scalar>, _Options > 337 : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > 338{ 339 typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base; 340 341 public: 342 typedef _Scalar Scalar; 343 typedef typename internal::traits<Map>::Coefficients Coefficients; 344 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) 345 using Base::operator*=; 346 347 /** Constructs a Mapped Quaternion object from the pointer \a coeffs 348 * 349 * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: 350 * \code *coeffs == {x, y, z, w} \endcode 351 * 352 * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ 353 EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {} 354 355 inline const Coefficients& coeffs() const { return m_coeffs;} 356 357 protected: 358 const Coefficients m_coeffs; 359}; 360 361/** \ingroup Geometry_Module 362 * \brief Expression of a quaternion from a memory buffer 363 * 364 * \tparam _Scalar the type of the Quaternion coefficients 365 * \tparam _Options see class Map 366 * 367 * This is a specialization of class Map for Quaternion. This class allows to view 368 * a 4 scalar memory buffer as an Eigen's Quaternion object. 369 * 370 * \sa class Map, class Quaternion, class QuaternionBase 371 */ 372template<typename _Scalar, int _Options> 373class Map<Quaternion<_Scalar>, _Options > 374 : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> > 375{ 376 typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base; 377 378 public: 379 typedef _Scalar Scalar; 380 typedef typename internal::traits<Map>::Coefficients Coefficients; 381 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) 382 using Base::operator*=; 383 384 /** Constructs a Mapped Quaternion object from the pointer \a coeffs 385 * 386 * The pointer \a coeffs must reference the four coefficients of Quaternion in the following order: 387 * \code *coeffs == {x, y, z, w} \endcode 388 * 389 * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ 390 EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {} 391 392 inline Coefficients& coeffs() { return m_coeffs; } 393 inline const Coefficients& coeffs() const { return m_coeffs; } 394 395 protected: 396 Coefficients m_coeffs; 397}; 398 399/** \ingroup Geometry_Module 400 * Map an unaligned array of single precision scalars as a quaternion */ 401typedef Map<Quaternion<float>, 0> QuaternionMapf; 402/** \ingroup Geometry_Module 403 * Map an unaligned array of double precision scalars as a quaternion */ 404typedef Map<Quaternion<double>, 0> QuaternionMapd; 405/** \ingroup Geometry_Module 406 * Map a 16-byte aligned array of single precision scalars as a quaternion */ 407typedef Map<Quaternion<float>, Aligned> QuaternionMapAlignedf; 408/** \ingroup Geometry_Module 409 * Map a 16-byte aligned array of double precision scalars as a quaternion */ 410typedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd; 411 412/*************************************************************************** 413* Implementation of QuaternionBase methods 414***************************************************************************/ 415 416// Generic Quaternion * Quaternion product 417// This product can be specialized for a given architecture via the Arch template argument. 418namespace internal { 419template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product 420{ 421 static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){ 422 return Quaternion<Scalar> 423 ( 424 a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), 425 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), 426 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(), 427 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x() 428 ); 429 } 430}; 431} 432 433/** \returns the concatenation of two rotations as a quaternion-quaternion product */ 434template <class Derived> 435template <class OtherDerived> 436EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar> 437QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const 438{ 439 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value), 440 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 441 return internal::quat_product<Architecture::Target, Derived, OtherDerived, 442 typename internal::traits<Derived>::Scalar, 443 internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other); 444} 445 446/** \sa operator*(Quaternion) */ 447template <class Derived> 448template <class OtherDerived> 449EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other) 450{ 451 derived() = derived() * other.derived(); 452 return derived(); 453} 454 455/** Rotation of a vector by a quaternion. 456 * \remarks If the quaternion is used to rotate several points (>1) 457 * then it is much more efficient to first convert it to a 3x3 Matrix. 458 * Comparison of the operation cost for n transformations: 459 * - Quaternion2: 30n 460 * - Via a Matrix3: 24 + 15n 461 */ 462template <class Derived> 463EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3 464QuaternionBase<Derived>::_transformVector(Vector3 v) const 465{ 466 // Note that this algorithm comes from the optimization by hand 467 // of the conversion to a Matrix followed by a Matrix/Vector product. 468 // It appears to be much faster than the common algorithm found 469 // in the literature (30 versus 39 flops). It also requires two 470 // Vector3 as temporaries. 471 Vector3 uv = this->vec().cross(v); 472 uv += uv; 473 return v + this->w() * uv + this->vec().cross(uv); 474} 475 476template<class Derived> 477EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other) 478{ 479 coeffs() = other.coeffs(); 480 return derived(); 481} 482 483template<class Derived> 484template<class OtherDerived> 485EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other) 486{ 487 coeffs() = other.coeffs(); 488 return derived(); 489} 490 491/** Set \c *this from an angle-axis \a aa and returns a reference to \c *this 492 */ 493template<class Derived> 494EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa) 495{ 496 using std::cos; 497 using std::sin; 498 Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings 499 this->w() = cos(ha); 500 this->vec() = sin(ha) * aa.axis(); 501 return derived(); 502} 503 504/** Set \c *this from the expression \a xpr: 505 * - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion 506 * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix 507 * and \a xpr is converted to a quaternion 508 */ 509 510template<class Derived> 511template<class MatrixDerived> 512inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr) 513{ 514 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value), 515 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 516 internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived()); 517 return derived(); 518} 519 520/** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to 521 * be normalized, otherwise the result is undefined. 522 */ 523template<class Derived> 524inline typename QuaternionBase<Derived>::Matrix3 525QuaternionBase<Derived>::toRotationMatrix(void) const 526{ 527 // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) 528 // if not inlined then the cost of the return by value is huge ~ +35%, 529 // however, not inlining this function is an order of magnitude slower, so 530 // it has to be inlined, and so the return by value is not an issue 531 Matrix3 res; 532 533 const Scalar tx = Scalar(2)*this->x(); 534 const Scalar ty = Scalar(2)*this->y(); 535 const Scalar tz = Scalar(2)*this->z(); 536 const Scalar twx = tx*this->w(); 537 const Scalar twy = ty*this->w(); 538 const Scalar twz = tz*this->w(); 539 const Scalar txx = tx*this->x(); 540 const Scalar txy = ty*this->x(); 541 const Scalar txz = tz*this->x(); 542 const Scalar tyy = ty*this->y(); 543 const Scalar tyz = tz*this->y(); 544 const Scalar tzz = tz*this->z(); 545 546 res.coeffRef(0,0) = Scalar(1)-(tyy+tzz); 547 res.coeffRef(0,1) = txy-twz; 548 res.coeffRef(0,2) = txz+twy; 549 res.coeffRef(1,0) = txy+twz; 550 res.coeffRef(1,1) = Scalar(1)-(txx+tzz); 551 res.coeffRef(1,2) = tyz-twx; 552 res.coeffRef(2,0) = txz-twy; 553 res.coeffRef(2,1) = tyz+twx; 554 res.coeffRef(2,2) = Scalar(1)-(txx+tyy); 555 556 return res; 557} 558 559/** Sets \c *this to be a quaternion representing a rotation between 560 * the two arbitrary vectors \a a and \a b. In other words, the built 561 * rotation represent a rotation sending the line of direction \a a 562 * to the line of direction \a b, both lines passing through the origin. 563 * 564 * \returns a reference to \c *this. 565 * 566 * Note that the two input vectors do \b not have to be normalized, and 567 * do not need to have the same norm. 568 */ 569template<class Derived> 570template<typename Derived1, typename Derived2> 571inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) 572{ 573 using std::max; 574 using std::sqrt; 575 Vector3 v0 = a.normalized(); 576 Vector3 v1 = b.normalized(); 577 Scalar c = v1.dot(v0); 578 579 // if dot == -1, vectors are nearly opposites 580 // => accurately compute the rotation axis by computing the 581 // intersection of the two planes. This is done by solving: 582 // x^T v0 = 0 583 // x^T v1 = 0 584 // under the constraint: 585 // ||x|| = 1 586 // which yields a singular value problem 587 if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision()) 588 { 589 c = (max)(c,Scalar(-1)); 590 Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose(); 591 JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV); 592 Vector3 axis = svd.matrixV().col(2); 593 594 Scalar w2 = (Scalar(1)+c)*Scalar(0.5); 595 this->w() = sqrt(w2); 596 this->vec() = axis * sqrt(Scalar(1) - w2); 597 return derived(); 598 } 599 Vector3 axis = v0.cross(v1); 600 Scalar s = sqrt((Scalar(1)+c)*Scalar(2)); 601 Scalar invs = Scalar(1)/s; 602 this->vec() = axis * invs; 603 this->w() = s * Scalar(0.5); 604 605 return derived(); 606} 607 608 609/** Returns a quaternion representing a rotation between 610 * the two arbitrary vectors \a a and \a b. In other words, the built 611 * rotation represent a rotation sending the line of direction \a a 612 * to the line of direction \a b, both lines passing through the origin. 613 * 614 * \returns resulting quaternion 615 * 616 * Note that the two input vectors do \b not have to be normalized, and 617 * do not need to have the same norm. 618 */ 619template<typename Scalar, int Options> 620template<typename Derived1, typename Derived2> 621Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) 622{ 623 Quaternion quat; 624 quat.setFromTwoVectors(a, b); 625 return quat; 626} 627 628 629/** \returns the multiplicative inverse of \c *this 630 * Note that in most cases, i.e., if you simply want the opposite rotation, 631 * and/or the quaternion is normalized, then it is enough to use the conjugate. 632 * 633 * \sa QuaternionBase::conjugate() 634 */ 635template <class Derived> 636inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const 637{ 638 // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ?? 639 Scalar n2 = this->squaredNorm(); 640 if (n2 > 0) 641 return Quaternion<Scalar>(conjugate().coeffs() / n2); 642 else 643 { 644 // return an invalid result to flag the error 645 return Quaternion<Scalar>(Coefficients::Zero()); 646 } 647} 648 649/** \returns the conjugate of the \c *this which is equal to the multiplicative inverse 650 * if the quaternion is normalized. 651 * The conjugate of a quaternion represents the opposite rotation. 652 * 653 * \sa Quaternion2::inverse() 654 */ 655template <class Derived> 656inline Quaternion<typename internal::traits<Derived>::Scalar> 657QuaternionBase<Derived>::conjugate() const 658{ 659 return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z()); 660} 661 662/** \returns the angle (in radian) between two rotations 663 * \sa dot() 664 */ 665template <class Derived> 666template <class OtherDerived> 667inline typename internal::traits<Derived>::Scalar 668QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const 669{ 670 using std::acos; 671 using std::abs; 672 Scalar d = abs(this->dot(other)); 673 if (d>=Scalar(1)) 674 return Scalar(0); 675 return Scalar(2) * acos(d); 676} 677 678 679 680/** \returns the spherical linear interpolation between the two quaternions 681 * \c *this and \a other at the parameter \a t in [0;1]. 682 * 683 * This represents an interpolation for a constant motion between \c *this and \a other, 684 * see also http://en.wikipedia.org/wiki/Slerp. 685 */ 686template <class Derived> 687template <class OtherDerived> 688Quaternion<typename internal::traits<Derived>::Scalar> 689QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const 690{ 691 using std::acos; 692 using std::sin; 693 using std::abs; 694 static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon(); 695 Scalar d = this->dot(other); 696 Scalar absD = abs(d); 697 698 Scalar scale0; 699 Scalar scale1; 700 701 if(absD>=one) 702 { 703 scale0 = Scalar(1) - t; 704 scale1 = t; 705 } 706 else 707 { 708 // theta is the angle between the 2 quaternions 709 Scalar theta = acos(absD); 710 Scalar sinTheta = sin(theta); 711 712 scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta; 713 scale1 = sin( ( t * theta) ) / sinTheta; 714 } 715 if(d<0) scale1 = -scale1; 716 717 return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs()); 718} 719 720namespace internal { 721 722// set from a rotation matrix 723template<typename Other> 724struct quaternionbase_assign_impl<Other,3,3> 725{ 726 typedef typename Other::Scalar Scalar; 727 typedef DenseIndex Index; 728 template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat) 729 { 730 using std::sqrt; 731 // This algorithm comes from "Quaternion Calculus and Fast Animation", 732 // Ken Shoemake, 1987 SIGGRAPH course notes 733 Scalar t = mat.trace(); 734 if (t > Scalar(0)) 735 { 736 t = sqrt(t + Scalar(1.0)); 737 q.w() = Scalar(0.5)*t; 738 t = Scalar(0.5)/t; 739 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t; 740 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t; 741 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t; 742 } 743 else 744 { 745 DenseIndex i = 0; 746 if (mat.coeff(1,1) > mat.coeff(0,0)) 747 i = 1; 748 if (mat.coeff(2,2) > mat.coeff(i,i)) 749 i = 2; 750 DenseIndex j = (i+1)%3; 751 DenseIndex k = (j+1)%3; 752 753 t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0)); 754 q.coeffs().coeffRef(i) = Scalar(0.5) * t; 755 t = Scalar(0.5)/t; 756 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t; 757 q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t; 758 q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t; 759 } 760 } 761}; 762 763// set from a vector of coefficients assumed to be a quaternion 764template<typename Other> 765struct quaternionbase_assign_impl<Other,4,1> 766{ 767 typedef typename Other::Scalar Scalar; 768 template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec) 769 { 770 q.coeffs() = vec; 771 } 772}; 773 774} // end namespace internal 775 776} // end namespace Eigen 777 778#endif // EIGEN_QUATERNION_H 779