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