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 (at) inria.fr> 5 // Copyright (C) 2009 Mathieu Gautier <mathieu.gautier (at) 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 13 namespace Eigen { 14 15 16 /*************************************************************************** 17 * Definition of QuaternionBase<Derived> 18 * The implementation is at the end of the file 19 ***************************************************************************/ 20 21 namespace internal { 22 template<typename Other, 23 int OtherRows=Other::RowsAtCompileTime, 24 int OtherCols=Other::ColsAtCompileTime> 25 struct 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 */ 34 template<class Derived> 35 class QuaternionBase : public RotationBase<Derived, 3> 36 { 37 typedef RotationBase<Derived, 3> Base; 38 public: 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 /** \returns an interpolation for a constant motion between \a other and \c *this 154 * \a t in [0;1] 155 * see http://en.wikipedia.org/wiki/Slerp 156 */ 157 template<class OtherDerived> Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const; 158 159 /** \returns \c true if \c *this is approximately equal to \a other, within the precision 160 * determined by \a prec. 161 * 162 * \sa MatrixBase::isApprox() */ 163 template<class OtherDerived> 164 bool isApprox(const QuaternionBase<OtherDerived>& other, RealScalar prec = NumTraits<Scalar>::dummy_precision()) const 165 { return coeffs().isApprox(other.coeffs(), prec); } 166 167 /** return the result vector of \a v through the rotation*/ 168 EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const; 169 170 /** \returns \c *this with scalar type casted to \a NewScalarType 171 * 172 * Note that if \a NewScalarType is equal to the current scalar type of \c *this 173 * then this function smartly returns a const reference to \c *this. 174 */ 175 template<typename NewScalarType> 176 inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const 177 { 178 return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived()); 179 } 180 181 #ifdef EIGEN_QUATERNIONBASE_PLUGIN 182 # include EIGEN_QUATERNIONBASE_PLUGIN 183 #endif 184 }; 185 186 /*************************************************************************** 187 * Definition/implementation of Quaternion<Scalar> 188 ***************************************************************************/ 189 190 /** \geometry_module \ingroup Geometry_Module 191 * 192 * \class Quaternion 193 * 194 * \brief The quaternion class used to represent 3D orientations and rotations 195 * 196 * \param _Scalar the scalar type, i.e., the type of the coefficients 197 * 198 * This class represents a quaternion \f$ w+xi+yj+zk \f$ that is a convenient representation of 199 * orientations and rotations of objects in three dimensions. Compared to other representations 200 * like Euler angles or 3x3 matrices, quatertions offer the following advantages: 201 * \li \b compact storage (4 scalars) 202 * \li \b efficient to compose (28 flops), 203 * \li \b stable spherical interpolation 204 * 205 * The following two typedefs are provided for convenience: 206 * \li \c Quaternionf for \c float 207 * \li \c Quaterniond for \c double 208 * 209 * \sa class AngleAxis, class Transform 210 */ 211 212 namespace internal { 213 template<typename _Scalar,int _Options> 214 struct traits<Quaternion<_Scalar,_Options> > 215 { 216 typedef Quaternion<_Scalar,_Options> PlainObject; 217 typedef _Scalar Scalar; 218 typedef Matrix<_Scalar,4,1,_Options> Coefficients; 219 enum{ 220 IsAligned = internal::traits<Coefficients>::Flags & AlignedBit, 221 Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit 222 }; 223 }; 224 } 225 226 template<typename _Scalar, int _Options> 227 class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> > 228 { 229 typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base; 230 enum { IsAligned = internal::traits<Quaternion>::IsAligned }; 231 232 public: 233 typedef _Scalar Scalar; 234 235 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion) 236 using Base::operator*=; 237 238 typedef typename internal::traits<Quaternion>::Coefficients Coefficients; 239 typedef typename Base::AngleAxisType AngleAxisType; 240 241 /** Default constructor leaving the quaternion uninitialized. */ 242 inline Quaternion() {} 243 244 /** Constructs and initializes the quaternion \f$ w+xi+yj+zk \f$ from 245 * its four coefficients \a w, \a x, \a y and \a z. 246 * 247 * \warning Note the order of the arguments: the real \a w coefficient first, 248 * while internally the coefficients are stored in the following order: 249 * [\c x, \c y, \c z, \c w] 250 */ 251 inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){} 252 253 /** Constructs and initialize a quaternion from the array data */ 254 inline Quaternion(const Scalar* data) : m_coeffs(data) {} 255 256 /** Copy constructor */ 257 template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); } 258 259 /** Constructs and initializes a quaternion from the angle-axis \a aa */ 260 explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; } 261 262 /** Constructs and initializes a quaternion from either: 263 * - a rotation matrix expression, 264 * - a 4D vector expression representing quaternion coefficients. 265 */ 266 template<typename Derived> 267 explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; } 268 269 /** Explicit copy constructor with scalar conversion */ 270 template<typename OtherScalar, int OtherOptions> 271 explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other) 272 { m_coeffs = other.coeffs().template cast<Scalar>(); } 273 274 template<typename Derived1, typename Derived2> 275 static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); 276 277 inline Coefficients& coeffs() { return m_coeffs;} 278 inline const Coefficients& coeffs() const { return m_coeffs;} 279 280 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned) 281 282 protected: 283 Coefficients m_coeffs; 284 285 #ifndef EIGEN_PARSED_BY_DOXYGEN 286 static EIGEN_STRONG_INLINE void _check_template_params() 287 { 288 EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options, 289 INVALID_MATRIX_TEMPLATE_PARAMETERS) 290 } 291 #endif 292 }; 293 294 /** \ingroup Geometry_Module 295 * single precision quaternion type */ 296 typedef Quaternion<float> Quaternionf; 297 /** \ingroup Geometry_Module 298 * double precision quaternion type */ 299 typedef Quaternion<double> Quaterniond; 300 301 /*************************************************************************** 302 * Specialization of Map<Quaternion<Scalar>> 303 ***************************************************************************/ 304 305 namespace internal { 306 template<typename _Scalar, int _Options> 307 struct traits<Map<Quaternion<_Scalar>, _Options> >: 308 traits<Quaternion<_Scalar, _Options> > 309 { 310 typedef _Scalar Scalar; 311 typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients; 312 313 typedef traits<Quaternion<_Scalar, _Options> > TraitsBase; 314 enum { 315 IsAligned = TraitsBase::IsAligned, 316 317 Flags = TraitsBase::Flags 318 }; 319 }; 320 } 321 322 namespace internal { 323 template<typename _Scalar, int _Options> 324 struct traits<Map<const Quaternion<_Scalar>, _Options> >: 325 traits<Quaternion<_Scalar> > 326 { 327 typedef _Scalar Scalar; 328 typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients; 329 330 typedef traits<Quaternion<_Scalar, _Options> > TraitsBase; 331 enum { 332 IsAligned = TraitsBase::IsAligned, 333 Flags = TraitsBase::Flags & ~LvalueBit 334 }; 335 }; 336 } 337 338 /** \brief Quaternion expression mapping a constant memory buffer 339 * 340 * \param _Scalar the type of the Quaternion coefficients 341 * \param _Options see class Map 342 * 343 * This is a specialization of class Map for Quaternion. This class allows to view 344 * a 4 scalar memory buffer as an Eigen's Quaternion object. 345 * 346 * \sa class Map, class Quaternion, class QuaternionBase 347 */ 348 template<typename _Scalar, int _Options> 349 class Map<const Quaternion<_Scalar>, _Options > 350 : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > 351 { 352 typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base; 353 354 public: 355 typedef _Scalar Scalar; 356 typedef typename internal::traits<Map>::Coefficients Coefficients; 357 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) 358 using Base::operator*=; 359 360 /** Constructs a Mapped Quaternion object from the pointer \a coeffs 361 * 362 * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order: 363 * \code *coeffs == {x, y, z, w} \endcode 364 * 365 * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ 366 EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {} 367 368 inline const Coefficients& coeffs() const { return m_coeffs;} 369 370 protected: 371 const Coefficients m_coeffs; 372 }; 373 374 /** \brief Expression of a quaternion from a memory buffer 375 * 376 * \param _Scalar the type of the Quaternion coefficients 377 * \param _Options see class Map 378 * 379 * This is a specialization of class Map for Quaternion. This class allows to view 380 * a 4 scalar memory buffer as an Eigen's Quaternion object. 381 * 382 * \sa class Map, class Quaternion, class QuaternionBase 383 */ 384 template<typename _Scalar, int _Options> 385 class Map<Quaternion<_Scalar>, _Options > 386 : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> > 387 { 388 typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base; 389 390 public: 391 typedef _Scalar Scalar; 392 typedef typename internal::traits<Map>::Coefficients Coefficients; 393 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) 394 using Base::operator*=; 395 396 /** Constructs a Mapped Quaternion object from the pointer \a coeffs 397 * 398 * The pointer \a coeffs must reference the four coeffecients of Quaternion in the following order: 399 * \code *coeffs == {x, y, z, w} \endcode 400 * 401 * If the template parameter _Options is set to #Aligned, then the pointer coeffs must be aligned. */ 402 EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {} 403 404 inline Coefficients& coeffs() { return m_coeffs; } 405 inline const Coefficients& coeffs() const { return m_coeffs; } 406 407 protected: 408 Coefficients m_coeffs; 409 }; 410 411 /** \ingroup Geometry_Module 412 * Map an unaligned array of single precision scalar as a quaternion */ 413 typedef Map<Quaternion<float>, 0> QuaternionMapf; 414 /** \ingroup Geometry_Module 415 * Map an unaligned array of double precision scalar as a quaternion */ 416 typedef Map<Quaternion<double>, 0> QuaternionMapd; 417 /** \ingroup Geometry_Module 418 * Map a 16-bits aligned array of double precision scalars as a quaternion */ 419 typedef Map<Quaternion<float>, Aligned> QuaternionMapAlignedf; 420 /** \ingroup Geometry_Module 421 * Map a 16-bits aligned array of double precision scalars as a quaternion */ 422 typedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd; 423 424 /*************************************************************************** 425 * Implementation of QuaternionBase methods 426 ***************************************************************************/ 427 428 // Generic Quaternion * Quaternion product 429 // This product can be specialized for a given architecture via the Arch template argument. 430 namespace internal { 431 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product 432 { 433 static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){ 434 return Quaternion<Scalar> 435 ( 436 a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), 437 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), 438 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(), 439 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x() 440 ); 441 } 442 }; 443 } 444 445 /** \returns the concatenation of two rotations as a quaternion-quaternion product */ 446 template <class Derived> 447 template <class OtherDerived> 448 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar> 449 QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const 450 { 451 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value), 452 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 453 return internal::quat_product<Architecture::Target, Derived, OtherDerived, 454 typename internal::traits<Derived>::Scalar, 455 internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other); 456 } 457 458 /** \sa operator*(Quaternion) */ 459 template <class Derived> 460 template <class OtherDerived> 461 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other) 462 { 463 derived() = derived() * other.derived(); 464 return derived(); 465 } 466 467 /** Rotation of a vector by a quaternion. 468 * \remarks If the quaternion is used to rotate several points (>1) 469 * then it is much more efficient to first convert it to a 3x3 Matrix. 470 * Comparison of the operation cost for n transformations: 471 * - Quaternion2: 30n 472 * - Via a Matrix3: 24 + 15n 473 */ 474 template <class Derived> 475 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3 476 QuaternionBase<Derived>::_transformVector(Vector3 v) const 477 { 478 // Note that this algorithm comes from the optimization by hand 479 // of the conversion to a Matrix followed by a Matrix/Vector product. 480 // It appears to be much faster than the common algorithm found 481 // in the litterature (30 versus 39 flops). It also requires two 482 // Vector3 as temporaries. 483 Vector3 uv = this->vec().cross(v); 484 uv += uv; 485 return v + this->w() * uv + this->vec().cross(uv); 486 } 487 488 template<class Derived> 489 EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other) 490 { 491 coeffs() = other.coeffs(); 492 return derived(); 493 } 494 495 template<class Derived> 496 template<class OtherDerived> 497 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other) 498 { 499 coeffs() = other.coeffs(); 500 return derived(); 501 } 502 503 /** Set \c *this from an angle-axis \a aa and returns a reference to \c *this 504 */ 505 template<class Derived> 506 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa) 507 { 508 Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings 509 this->w() = internal::cos(ha); 510 this->vec() = internal::sin(ha) * aa.axis(); 511 return derived(); 512 } 513 514 /** Set \c *this from the expression \a xpr: 515 * - if \a xpr is a 4x1 vector, then \a xpr is assumed to be a quaternion 516 * - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation matrix 517 * and \a xpr is converted to a quaternion 518 */ 519 520 template<class Derived> 521 template<class MatrixDerived> 522 inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr) 523 { 524 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value), 525 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 526 internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived()); 527 return derived(); 528 } 529 530 /** Convert the quaternion to a 3x3 rotation matrix. The quaternion is required to 531 * be normalized, otherwise the result is undefined. 532 */ 533 template<class Derived> 534 inline typename QuaternionBase<Derived>::Matrix3 535 QuaternionBase<Derived>::toRotationMatrix(void) const 536 { 537 // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) 538 // if not inlined then the cost of the return by value is huge ~ +35%, 539 // however, not inlining this function is an order of magnitude slower, so 540 // it has to be inlined, and so the return by value is not an issue 541 Matrix3 res; 542 543 const Scalar tx = Scalar(2)*this->x(); 544 const Scalar ty = Scalar(2)*this->y(); 545 const Scalar tz = Scalar(2)*this->z(); 546 const Scalar twx = tx*this->w(); 547 const Scalar twy = ty*this->w(); 548 const Scalar twz = tz*this->w(); 549 const Scalar txx = tx*this->x(); 550 const Scalar txy = ty*this->x(); 551 const Scalar txz = tz*this->x(); 552 const Scalar tyy = ty*this->y(); 553 const Scalar tyz = tz*this->y(); 554 const Scalar tzz = tz*this->z(); 555 556 res.coeffRef(0,0) = Scalar(1)-(tyy+tzz); 557 res.coeffRef(0,1) = txy-twz; 558 res.coeffRef(0,2) = txz+twy; 559 res.coeffRef(1,0) = txy+twz; 560 res.coeffRef(1,1) = Scalar(1)-(txx+tzz); 561 res.coeffRef(1,2) = tyz-twx; 562 res.coeffRef(2,0) = txz-twy; 563 res.coeffRef(2,1) = tyz+twx; 564 res.coeffRef(2,2) = Scalar(1)-(txx+tyy); 565 566 return res; 567 } 568 569 /** Sets \c *this to be a quaternion representing a rotation between 570 * the two arbitrary vectors \a a and \a b. In other words, the built 571 * rotation represent a rotation sending the line of direction \a a 572 * to the line of direction \a b, both lines passing through the origin. 573 * 574 * \returns a reference to \c *this. 575 * 576 * Note that the two input vectors do \b not have to be normalized, and 577 * do not need to have the same norm. 578 */ 579 template<class Derived> 580 template<typename Derived1, typename Derived2> 581 inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) 582 { 583 using std::max; 584 Vector3 v0 = a.normalized(); 585 Vector3 v1 = b.normalized(); 586 Scalar c = v1.dot(v0); 587 588 // if dot == -1, vectors are nearly opposites 589 // => accuraletly compute the rotation axis by computing the 590 // intersection of the two planes. This is done by solving: 591 // x^T v0 = 0 592 // x^T v1 = 0 593 // under the constraint: 594 // ||x|| = 1 595 // which yields a singular value problem 596 if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision()) 597 { 598 c = max<Scalar>(c,-1); 599 Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose(); 600 JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV); 601 Vector3 axis = svd.matrixV().col(2); 602 603 Scalar w2 = (Scalar(1)+c)*Scalar(0.5); 604 this->w() = internal::sqrt(w2); 605 this->vec() = axis * internal::sqrt(Scalar(1) - w2); 606 return derived(); 607 } 608 Vector3 axis = v0.cross(v1); 609 Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2)); 610 Scalar invs = Scalar(1)/s; 611 this->vec() = axis * invs; 612 this->w() = s * Scalar(0.5); 613 614 return derived(); 615 } 616 617 618 /** Returns a quaternion representing a rotation between 619 * the two arbitrary vectors \a a and \a b. In other words, the built 620 * rotation represent a rotation sending the line of direction \a a 621 * to the line of direction \a b, both lines passing through the origin. 622 * 623 * \returns resulting quaternion 624 * 625 * Note that the two input vectors do \b not have to be normalized, and 626 * do not need to have the same norm. 627 */ 628 template<typename Scalar, int Options> 629 template<typename Derived1, typename Derived2> 630 Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) 631 { 632 Quaternion quat; 633 quat.setFromTwoVectors(a, b); 634 return quat; 635 } 636 637 638 /** \returns the multiplicative inverse of \c *this 639 * Note that in most cases, i.e., if you simply want the opposite rotation, 640 * and/or the quaternion is normalized, then it is enough to use the conjugate. 641 * 642 * \sa QuaternionBase::conjugate() 643 */ 644 template <class Derived> 645 inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const 646 { 647 // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ?? 648 Scalar n2 = this->squaredNorm(); 649 if (n2 > 0) 650 return Quaternion<Scalar>(conjugate().coeffs() / n2); 651 else 652 { 653 // return an invalid result to flag the error 654 return Quaternion<Scalar>(Coefficients::Zero()); 655 } 656 } 657 658 /** \returns the conjugate of the \c *this which is equal to the multiplicative inverse 659 * if the quaternion is normalized. 660 * The conjugate of a quaternion represents the opposite rotation. 661 * 662 * \sa Quaternion2::inverse() 663 */ 664 template <class Derived> 665 inline Quaternion<typename internal::traits<Derived>::Scalar> 666 QuaternionBase<Derived>::conjugate() const 667 { 668 return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z()); 669 } 670 671 /** \returns the angle (in radian) between two rotations 672 * \sa dot() 673 */ 674 template <class Derived> 675 template <class OtherDerived> 676 inline typename internal::traits<Derived>::Scalar 677 QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const 678 { 679 using std::acos; 680 double d = internal::abs(this->dot(other)); 681 if (d>=1.0) 682 return Scalar(0); 683 return static_cast<Scalar>(2 * acos(d)); 684 } 685 686 /** \returns the spherical linear interpolation between the two quaternions 687 * \c *this and \a other at the parameter \a t 688 */ 689 template <class Derived> 690 template <class OtherDerived> 691 Quaternion<typename internal::traits<Derived>::Scalar> 692 QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const 693 { 694 using std::acos; 695 static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon(); 696 Scalar d = this->dot(other); 697 Scalar absD = internal::abs(d); 698 699 Scalar scale0; 700 Scalar scale1; 701 702 if(absD>=one) 703 { 704 scale0 = Scalar(1) - t; 705 scale1 = t; 706 } 707 else 708 { 709 // theta is the angle between the 2 quaternions 710 Scalar theta = acos(absD); 711 Scalar sinTheta = internal::sin(theta); 712 713 scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta; 714 scale1 = internal::sin( ( t * theta) ) / sinTheta; 715 } 716 if(d<0) scale1 = -scale1; 717 718 return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs()); 719 } 720 721 namespace internal { 722 723 // set from a rotation matrix 724 template<typename Other> 725 struct quaternionbase_assign_impl<Other,3,3> 726 { 727 typedef typename Other::Scalar Scalar; 728 typedef DenseIndex Index; 729 template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat) 730 { 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 764 template<typename Other> 765 struct 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