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