$treeview $search $mathjax
Eigen
3.2.5
$projectbrief
|
$projectbrief
|
$searchbox |
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 typedef RotationBase<Derived, 3> Base; 00038 public: 00039 using Base::operator*; 00040 using Base::derived; 00041 00042 typedef typename internal::traits<Derived>::Scalar Scalar; 00043 typedef typename NumTraits<Scalar>::Real RealScalar; 00044 typedef typename internal::traits<Derived>::Coefficients Coefficients; 00045 enum { 00046 Flags = Eigen::internal::traits<Derived>::Flags 00047 }; 00048 00049 // typedef typename Matrix<Scalar,4,1> Coefficients; 00051 typedef Matrix<Scalar,3,1> Vector3; 00053 typedef Matrix<Scalar,3,3> Matrix3; 00055 typedef AngleAxis<Scalar> AngleAxisType; 00056 00057 00058 00060 inline Scalar x() const { return this->derived().coeffs().coeff(0); } 00062 inline Scalar y() const { return this->derived().coeffs().coeff(1); } 00064 inline Scalar z() const { return this->derived().coeffs().coeff(2); } 00066 inline Scalar w() const { return this->derived().coeffs().coeff(3); } 00067 00069 inline Scalar& x() { return this->derived().coeffs().coeffRef(0); } 00071 inline Scalar& y() { return this->derived().coeffs().coeffRef(1); } 00073 inline Scalar& z() { return this->derived().coeffs().coeffRef(2); } 00075 inline Scalar& w() { return this->derived().coeffs().coeffRef(3); } 00076 00078 inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); } 00079 00081 inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); } 00082 00084 inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); } 00085 00087 inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); } 00088 00089 EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other); 00090 template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other); 00091 00092 // disabled this copy operator as it is giving very strange compilation errors when compiling 00093 // test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's 00094 // useful; however notice that we already have the templated operator= above and e.g. in MatrixBase 00095 // we didn't have to add, in addition to templated operator=, such a non-templated copy operator. 00096 // Derived& operator=(const QuaternionBase& other) 00097 // { return operator=<Derived>(other); } 00098 00099 Derived& operator=(const AngleAxisType& aa); 00100 template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m); 00101 00105 static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); } 00106 00109 inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; } 00110 00114 inline Scalar squaredNorm() const { return coeffs().squaredNorm(); } 00115 00119 inline Scalar norm() const { return coeffs().norm(); } 00120 00123 inline void normalize() { coeffs().normalize(); } 00126 inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); } 00127 00133 template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); } 00134 00135 template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const; 00136 00138 Matrix3 toRotationMatrix() const; 00139 00141 template<typename Derived1, typename Derived2> 00142 Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); 00143 00144 template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const; 00145 template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q); 00146 00148 Quaternion<Scalar> inverse() const; 00149 00151 Quaternion<Scalar> conjugate() const; 00152 00153 template<class OtherDerived> Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const; 00154 00159 template<class OtherDerived> 00160 bool isApprox(const QuaternionBase<OtherDerived>& other, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const 00161 { return coeffs().isApprox(other.coeffs(), prec); } 00162 00164 EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const; 00165 00171 template<typename NewScalarType> 00172 inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const 00173 { 00174 return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived()); 00175 } 00176 00177 #ifdef EIGEN_QUATERNIONBASE_PLUGIN 00178 # include EIGEN_QUATERNIONBASE_PLUGIN 00179 #endif 00180 }; 00181 00182 /*************************************************************************** 00183 * Definition/implementation of Quaternion<Scalar> 00184 ***************************************************************************/ 00185 00211 namespace internal { 00212 template<typename _Scalar,int _Options> 00213 struct traits<Quaternion<_Scalar,_Options> > 00214 { 00215 typedef Quaternion<_Scalar,_Options> PlainObject; 00216 typedef _Scalar Scalar; 00217 typedef Matrix<_Scalar,4,1,_Options> Coefficients; 00218 enum{ 00219 IsAligned = internal::traits<Coefficients>::Flags & AlignedBit, 00220 Flags = IsAligned ? (AlignedBit | LvalueBit) : LvalueBit 00221 }; 00222 }; 00223 } 00224 00225 template<typename _Scalar, int _Options> 00226 class Quaternion : public QuaternionBase<Quaternion<_Scalar,_Options> > 00227 { 00228 typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base; 00229 enum { IsAligned = internal::traits<Quaternion>::IsAligned }; 00230 00231 public: 00232 typedef _Scalar Scalar; 00233 00234 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion) 00235 using Base::operator*=; 00236 00237 typedef typename internal::traits<Quaternion>::Coefficients Coefficients; 00238 typedef typename Base::AngleAxisType AngleAxisType; 00239 00241 inline Quaternion() {} 00242 00250 inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){} 00251 00253 inline Quaternion(const Scalar* data) : m_coeffs(data) {} 00254 00256 template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); } 00257 00259 explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; } 00260 00265 template<typename Derived> 00266 explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; } 00267 00269 template<typename OtherScalar, int OtherOptions> 00270 explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other) 00271 { m_coeffs = other.coeffs().template cast<Scalar>(); } 00272 00273 template<typename Derived1, typename Derived2> 00274 static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b); 00275 00276 inline Coefficients& coeffs() { return m_coeffs;} 00277 inline const Coefficients& coeffs() const { return m_coeffs;} 00278 00279 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned) 00280 00281 protected: 00282 Coefficients m_coeffs; 00283 00284 #ifndef EIGEN_PARSED_BY_DOXYGEN 00285 static EIGEN_STRONG_INLINE void _check_template_params() 00286 { 00287 EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options, 00288 INVALID_MATRIX_TEMPLATE_PARAMETERS) 00289 } 00290 #endif 00291 }; 00292 00295 typedef Quaternion<float> Quaternionf; 00298 typedef Quaternion<double> Quaterniond; 00299 00300 /*************************************************************************** 00301 * Specialization of Map<Quaternion<Scalar>> 00302 ***************************************************************************/ 00303 00304 namespace internal { 00305 template<typename _Scalar, int _Options> 00306 struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > 00307 { 00308 typedef Map<Matrix<_Scalar,4,1>, _Options> Coefficients; 00309 }; 00310 } 00311 00312 namespace internal { 00313 template<typename _Scalar, int _Options> 00314 struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > 00315 { 00316 typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients; 00317 typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase; 00318 enum { 00319 Flags = TraitsBase::Flags & ~LvalueBit 00320 }; 00321 }; 00322 } 00323 00335 template<typename _Scalar, int _Options> 00336 class Map<const Quaternion<_Scalar>, _Options > 00337 : public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > 00338 { 00339 typedef QuaternionBase<Map<const Quaternion<_Scalar>, _Options> > Base; 00340 00341 public: 00342 typedef _Scalar Scalar; 00343 typedef typename internal::traits<Map>::Coefficients Coefficients; 00344 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) 00345 using Base::operator*=; 00346 00353 EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {} 00354 00355 inline const Coefficients& coeffs() const { return m_coeffs;} 00356 00357 protected: 00358 const Coefficients m_coeffs; 00359 }; 00360 00372 template<typename _Scalar, int _Options> 00373 class Map<Quaternion<_Scalar>, _Options > 00374 : public QuaternionBase<Map<Quaternion<_Scalar>, _Options> > 00375 { 00376 typedef QuaternionBase<Map<Quaternion<_Scalar>, _Options> > Base; 00377 00378 public: 00379 typedef _Scalar Scalar; 00380 typedef typename internal::traits<Map>::Coefficients Coefficients; 00381 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map) 00382 using Base::operator*=; 00383 00390 EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {} 00391 00392 inline Coefficients& coeffs() { return m_coeffs; } 00393 inline const Coefficients& coeffs() const { return m_coeffs; } 00394 00395 protected: 00396 Coefficients m_coeffs; 00397 }; 00398 00401 typedef Map<Quaternion<float>, 0> QuaternionMapf; 00404 typedef Map<Quaternion<double>, 0> QuaternionMapd; 00407 typedef Map<Quaternion<float>, Aligned> QuaternionMapAlignedf; 00410 typedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd; 00411 00412 /*************************************************************************** 00413 * Implementation of QuaternionBase methods 00414 ***************************************************************************/ 00415 00416 // Generic Quaternion * Quaternion product 00417 // This product can be specialized for a given architecture via the Arch template argument. 00418 namespace internal { 00419 template<int Arch, class Derived1, class Derived2, typename Scalar, int _Options> struct quat_product 00420 { 00421 static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){ 00422 return Quaternion<Scalar> 00423 ( 00424 a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(), 00425 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(), 00426 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(), 00427 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x() 00428 ); 00429 } 00430 }; 00431 } 00432 00434 template <class Derived> 00435 template <class OtherDerived> 00436 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar> 00437 QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const 00438 { 00439 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value), 00440 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 00441 return internal::quat_product<Architecture::Target, Derived, OtherDerived, 00442 typename internal::traits<Derived>::Scalar, 00443 internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*this, other); 00444 } 00445 00447 template <class Derived> 00448 template <class OtherDerived> 00449 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other) 00450 { 00451 derived() = derived() * other.derived(); 00452 return derived(); 00453 } 00454 00462 template <class Derived> 00463 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3 00464 QuaternionBase<Derived>::_transformVector(const Vector3& v) const 00465 { 00466 // Note that this algorithm comes from the optimization by hand 00467 // of the conversion to a Matrix followed by a Matrix/Vector product. 00468 // It appears to be much faster than the common algorithm found 00469 // in the literature (30 versus 39 flops). It also requires two 00470 // Vector3 as temporaries. 00471 Vector3 uv = this->vec().cross(v); 00472 uv += uv; 00473 return v + this->w() * uv + this->vec().cross(uv); 00474 } 00475 00476 template<class Derived> 00477 EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other) 00478 { 00479 coeffs() = other.coeffs(); 00480 return derived(); 00481 } 00482 00483 template<class Derived> 00484 template<class OtherDerived> 00485 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other) 00486 { 00487 coeffs() = other.coeffs(); 00488 return derived(); 00489 } 00490 00493 template<class Derived> 00494 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa) 00495 { 00496 using std::cos; 00497 using std::sin; 00498 Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings 00499 this->w() = cos(ha); 00500 this->vec() = sin(ha) * aa.axis(); 00501 return derived(); 00502 } 00503 00510 template<class Derived> 00511 template<class MatrixDerived> 00512 inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr) 00513 { 00514 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value), 00515 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY) 00516 internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived()); 00517 return derived(); 00518 } 00519 00523 template<class Derived> 00524 inline typename QuaternionBase<Derived>::Matrix3 00525 QuaternionBase<Derived>::toRotationMatrix(void) const 00526 { 00527 // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!) 00528 // if not inlined then the cost of the return by value is huge ~ +35%, 00529 // however, not inlining this function is an order of magnitude slower, so 00530 // it has to be inlined, and so the return by value is not an issue 00531 Matrix3 res; 00532 00533 const Scalar tx = Scalar(2)*this->x(); 00534 const Scalar ty = Scalar(2)*this->y(); 00535 const Scalar tz = Scalar(2)*this->z(); 00536 const Scalar twx = tx*this->w(); 00537 const Scalar twy = ty*this->w(); 00538 const Scalar twz = tz*this->w(); 00539 const Scalar txx = tx*this->x(); 00540 const Scalar txy = ty*this->x(); 00541 const Scalar txz = tz*this->x(); 00542 const Scalar tyy = ty*this->y(); 00543 const Scalar tyz = tz*this->y(); 00544 const Scalar tzz = tz*this->z(); 00545 00546 res.coeffRef(0,0) = Scalar(1)-(tyy+tzz); 00547 res.coeffRef(0,1) = txy-twz; 00548 res.coeffRef(0,2) = txz+twy; 00549 res.coeffRef(1,0) = txy+twz; 00550 res.coeffRef(1,1) = Scalar(1)-(txx+tzz); 00551 res.coeffRef(1,2) = tyz-twx; 00552 res.coeffRef(2,0) = txz-twy; 00553 res.coeffRef(2,1) = tyz+twx; 00554 res.coeffRef(2,2) = Scalar(1)-(txx+tyy); 00555 00556 return res; 00557 } 00558 00569 template<class Derived> 00570 template<typename Derived1, typename Derived2> 00571 inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) 00572 { 00573 using std::max; 00574 using std::sqrt; 00575 Vector3 v0 = a.normalized(); 00576 Vector3 v1 = b.normalized(); 00577 Scalar c = v1.dot(v0); 00578 00579 // if dot == -1, vectors are nearly opposites 00580 // => accurately compute the rotation axis by computing the 00581 // intersection of the two planes. This is done by solving: 00582 // x^T v0 = 0 00583 // x^T v1 = 0 00584 // under the constraint: 00585 // ||x|| = 1 00586 // which yields a singular value problem 00587 if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision()) 00588 { 00589 c = (max)(c,Scalar(-1)); 00590 Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose(); 00591 JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV); 00592 Vector3 axis = svd.matrixV().col(2); 00593 00594 Scalar w2 = (Scalar(1)+c)*Scalar(0.5); 00595 this->w() = sqrt(w2); 00596 this->vec() = axis * sqrt(Scalar(1) - w2); 00597 return derived(); 00598 } 00599 Vector3 axis = v0.cross(v1); 00600 Scalar s = sqrt((Scalar(1)+c)*Scalar(2)); 00601 Scalar invs = Scalar(1)/s; 00602 this->vec() = axis * invs; 00603 this->w() = s * Scalar(0.5); 00604 00605 return derived(); 00606 } 00607 00608 00619 template<typename Scalar, int Options> 00620 template<typename Derived1, typename Derived2> 00621 Quaternion<Scalar,Options> Quaternion<Scalar,Options>::FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) 00622 { 00623 Quaternion quat; 00624 quat.setFromTwoVectors(a, b); 00625 return quat; 00626 } 00627 00628 00635 template <class Derived> 00636 inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const 00637 { 00638 // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ?? 00639 Scalar n2 = this->squaredNorm(); 00640 if (n2 > Scalar(0)) 00641 return Quaternion<Scalar>(conjugate().coeffs() / n2); 00642 else 00643 { 00644 // return an invalid result to flag the error 00645 return Quaternion<Scalar>(Coefficients::Zero()); 00646 } 00647 } 00648 00655 template <class Derived> 00656 inline Quaternion<typename internal::traits<Derived>::Scalar> 00657 QuaternionBase<Derived>::conjugate() const 00658 { 00659 return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z()); 00660 } 00661 00665 template <class Derived> 00666 template <class OtherDerived> 00667 inline typename internal::traits<Derived>::Scalar 00668 QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const 00669 { 00670 using std::atan2; 00671 using std::abs; 00672 Quaternion<Scalar> d = (*this) * other.conjugate(); 00673 return Scalar(2) * atan2( d.vec().norm(), abs(d.w()) ); 00674 } 00675 00676 00677 00684 template <class Derived> 00685 template <class OtherDerived> 00686 Quaternion<typename internal::traits<Derived>::Scalar> 00687 QuaternionBase<Derived>::slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const 00688 { 00689 using std::acos; 00690 using std::sin; 00691 using std::abs; 00692 static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon(); 00693 Scalar d = this->dot(other); 00694 Scalar absD = abs(d); 00695 00696 Scalar scale0; 00697 Scalar scale1; 00698 00699 if(absD>=one) 00700 { 00701 scale0 = Scalar(1) - t; 00702 scale1 = t; 00703 } 00704 else 00705 { 00706 // theta is the angle between the 2 quaternions 00707 Scalar theta = acos(absD); 00708 Scalar sinTheta = sin(theta); 00709 00710 scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta; 00711 scale1 = sin( ( t * theta) ) / sinTheta; 00712 } 00713 if(d<Scalar(0)) scale1 = -scale1; 00714 00715 return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs()); 00716 } 00717 00718 namespace internal { 00719 00720 // set from a rotation matrix 00721 template<typename Other> 00722 struct quaternionbase_assign_impl<Other,3,3> 00723 { 00724 typedef typename Other::Scalar Scalar; 00725 typedef DenseIndex Index; 00726 template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& mat) 00727 { 00728 using std::sqrt; 00729 // This algorithm comes from "Quaternion Calculus and Fast Animation", 00730 // Ken Shoemake, 1987 SIGGRAPH course notes 00731 Scalar t = mat.trace(); 00732 if (t > Scalar(0)) 00733 { 00734 t = sqrt(t + Scalar(1.0)); 00735 q.w() = Scalar(0.5)*t; 00736 t = Scalar(0.5)/t; 00737 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t; 00738 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t; 00739 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t; 00740 } 00741 else 00742 { 00743 DenseIndex i = 0; 00744 if (mat.coeff(1,1) > mat.coeff(0,0)) 00745 i = 1; 00746 if (mat.coeff(2,2) > mat.coeff(i,i)) 00747 i = 2; 00748 DenseIndex j = (i+1)%3; 00749 DenseIndex k = (j+1)%3; 00750 00751 t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0)); 00752 q.coeffs().coeffRef(i) = Scalar(0.5) * t; 00753 t = Scalar(0.5)/t; 00754 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t; 00755 q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t; 00756 q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t; 00757 } 00758 } 00759 }; 00760 00761 // set from a vector of coefficients assumed to be a quaternion 00762 template<typename Other> 00763 struct quaternionbase_assign_impl<Other,4,1> 00764 { 00765 typedef typename Other::Scalar Scalar; 00766 template<class Derived> static inline void run(QuaternionBase<Derived>& q, const Other& vec) 00767 { 00768 q.coeffs() = vec; 00769 } 00770 }; 00771 00772 } // end namespace internal 00773 00774 } // end namespace Eigen 00775 00776 #endif // EIGEN_QUATERNION_H