00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef EIGEN_QUATERNION_H
00027 #define EIGEN_QUATERNION_H
00028
00029
00030
00031
00032
00033
00034 namespace internal {
00035 template<typename Other,
00036 int OtherRows=Other::RowsAtCompileTime,
00037 int OtherCols=Other::ColsAtCompileTime>
00038 struct quaternionbase_assign_impl;
00039 }
00040
00041 template<class Derived>
00042 class QuaternionBase : public RotationBase<Derived, 3>
00043 {
00044 typedef RotationBase<Derived, 3> Base;
00045 public:
00046 using Base::operator*;
00047 using Base::derived;
00048
00049 typedef typename internal::traits<Derived>::Scalar Scalar;
00050 typedef typename NumTraits<Scalar>::Real RealScalar;
00051 typedef typename internal::traits<Derived>::Coefficients Coefficients;
00052
00053
00054
00055 typedef Matrix<Scalar,3,1> Vector3;
00056
00057 typedef Matrix<Scalar,3,3> Matrix3;
00058
00059 typedef AngleAxis<Scalar> AngleAxisType;
00060
00061
00062
00063
00064 inline Scalar x() const { return this->derived().coeffs().coeff(0); }
00065
00066 inline Scalar y() const { return this->derived().coeffs().coeff(1); }
00067
00068 inline Scalar z() const { return this->derived().coeffs().coeff(2); }
00069
00070 inline Scalar w() const { return this->derived().coeffs().coeff(3); }
00071
00072
00073 inline Scalar& x() { return this->derived().coeffs().coeffRef(0); }
00074
00075 inline Scalar& y() { return this->derived().coeffs().coeffRef(1); }
00076
00077 inline Scalar& z() { return this->derived().coeffs().coeffRef(2); }
00078
00079 inline Scalar& w() { return this->derived().coeffs().coeffRef(3); }
00080
00081
00082 inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
00083
00084
00085 inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); }
00086
00087
00088 inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); }
00089
00090
00091 inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
00092
00093 EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
00094 template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
00095
00096
00097
00098
00099
00100
00101
00102
00103 Derived& operator=(const AngleAxisType& aa);
00104 template<class OtherDerived> Derived& operator=(const MatrixBase<OtherDerived>& m);
00105
00106
00107
00108
00109 inline static Quaternion<Scalar> Identity() { return Quaternion<Scalar>(1, 0, 0, 0); }
00110
00111
00112
00113 inline QuaternionBase& setIdentity() { coeffs() << 0, 0, 0, 1; return *this; }
00114
00115
00116
00117
00118 inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
00119
00120
00121
00122
00123 inline Scalar norm() const { return coeffs().norm(); }
00124
00125
00126
00127 inline void normalize() { coeffs().normalize(); }
00128
00129
00130 inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
00131
00132
00133
00134
00135
00136
00137 template<class OtherDerived> inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
00138
00139 template<class OtherDerived> Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
00140
00141
00142 Matrix3 toRotationMatrix() const;
00143
00144
00145 template<typename Derived1, typename Derived2>
00146 Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00147
00148 template<class OtherDerived> EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
00149 template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
00150
00151
00152 Quaternion<Scalar> inverse() const;
00153
00154
00155 Quaternion<Scalar> conjugate() const;
00156
00157
00158
00159
00160
00161 template<class OtherDerived> Quaternion<Scalar> slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const;
00162
00163
00164
00165
00166
00167 template<class OtherDerived>
00168 bool isApprox(const QuaternionBase<OtherDerived>& other, RealScalar prec = NumTraits<Scalar>::dummy_precision()) const
00169 { return coeffs().isApprox(other.coeffs(), prec); }
00170
00171
00172 EIGEN_STRONG_INLINE Vector3 _transformVector(Vector3 v) const;
00173
00174
00175
00176
00177
00178
00179 template<typename NewScalarType>
00180 inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const
00181 {
00182 return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(
00183 coeffs().template cast<NewScalarType>());
00184 }
00185
00186 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
00187 # include EIGEN_QUATERNIONBASE_PLUGIN
00188 #endif
00189 };
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217 namespace internal {
00218 template<typename _Scalar>
00219 struct traits<Quaternion<_Scalar> >
00220 {
00221 typedef Quaternion<_Scalar> PlainObject;
00222 typedef _Scalar Scalar;
00223 typedef Matrix<_Scalar,4,1> Coefficients;
00224 enum{
00225 PacketAccess = Aligned
00226 };
00227 };
00228 }
00229
00230 template<typename _Scalar>
00231 class Quaternion : public QuaternionBase<Quaternion<_Scalar> >{
00232 typedef QuaternionBase<Quaternion<_Scalar> > Base;
00233 public:
00234 typedef _Scalar Scalar;
00235
00236 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion<Scalar>)
00237 using Base::operator*=;
00238
00239 typedef typename internal::traits<Quaternion<Scalar> >::Coefficients Coefficients;
00240 typedef typename Base::AngleAxisType AngleAxisType;
00241
00242
00243 inline Quaternion() {}
00244
00245
00246
00247
00248
00249
00250
00251
00252 inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){}
00253
00254
00255 inline Quaternion(const Scalar* data) : m_coeffs(data) {}
00256
00257
00258 template<class Derived> EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
00259
00260
00261 explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
00262
00263
00264
00265
00266
00267 template<typename Derived>
00268 explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
00269
00270 inline Coefficients& coeffs() { return m_coeffs;}
00271 inline const Coefficients& coeffs() const { return m_coeffs;}
00272
00273 protected:
00274 Coefficients m_coeffs;
00275 };
00276
00277
00278
00279 typedef Quaternion<float> Quaternionf;
00280
00281
00282 typedef Quaternion<double> Quaterniond;
00283
00284
00285
00286
00287
00288 namespace internal {
00289 template<typename _Scalar, int _PacketAccess>
00290 struct traits<Map<Quaternion<_Scalar>, _PacketAccess> >:
00291 traits<Quaternion<_Scalar> >
00292 {
00293 typedef _Scalar Scalar;
00294 typedef Map<Matrix<_Scalar,4,1>, _PacketAccess> Coefficients;
00295 enum {
00296 PacketAccess = _PacketAccess
00297 };
00298 };
00299 }
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311 template<typename _Scalar, int PacketAccess>
00312 class Map<const Quaternion<_Scalar>, PacketAccess >
00313 : public QuaternionBase<Map<const Quaternion<_Scalar>, PacketAccess> >
00314 {
00315 typedef QuaternionBase<Map<Quaternion<_Scalar>, PacketAccess> > Base;
00316
00317 public:
00318 typedef _Scalar Scalar;
00319 typedef typename internal::traits<Map>::Coefficients Coefficients;
00320 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00321 using Base::operator*=;
00322
00323
00324
00325
00326
00327
00328
00329 EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
00330
00331 inline const Coefficients& coeffs() const { return m_coeffs;}
00332
00333 protected:
00334 const Coefficients m_coeffs;
00335 };
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347 template<typename _Scalar, int PacketAccess>
00348 class Map<Quaternion<_Scalar>, PacketAccess >
00349 : public QuaternionBase<Map<Quaternion<_Scalar>, PacketAccess> >
00350 {
00351 typedef QuaternionBase<Map<Quaternion<_Scalar>, PacketAccess> > Base;
00352
00353 public:
00354 typedef _Scalar Scalar;
00355 typedef typename internal::traits<Map>::Coefficients Coefficients;
00356 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
00357 using Base::operator*=;
00358
00359
00360
00361
00362
00363
00364
00365 EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
00366
00367 inline Coefficients& coeffs() { return m_coeffs; }
00368 inline const Coefficients& coeffs() const { return m_coeffs; }
00369
00370 protected:
00371 Coefficients m_coeffs;
00372 };
00373
00374
00375
00376 typedef Map<Quaternion<float>, 0> QuaternionMapf;
00377
00378
00379 typedef Map<Quaternion<double>, 0> QuaternionMapd;
00380
00381
00382 typedef Map<Quaternion<float>, Aligned> QuaternionMapAlignedf;
00383
00384
00385 typedef Map<Quaternion<double>, Aligned> QuaternionMapAlignedd;
00386
00387
00388
00389
00390
00391
00392
00393 namespace internal {
00394 template<int Arch, class Derived1, class Derived2, typename Scalar, int PacketAccess> struct quat_product
00395 {
00396 EIGEN_STRONG_INLINE static Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
00397 return Quaternion<Scalar>
00398 (
00399 a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
00400 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
00401 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
00402 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
00403 );
00404 }
00405 };
00406 }
00407
00408
00409 template <class Derived>
00410 template <class OtherDerived>
00411 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
00412 QuaternionBase<Derived>::operator* (const QuaternionBase<OtherDerived>& other) const
00413 {
00414 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
00415 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00416 return internal::quat_product<Architecture::Target, Derived, OtherDerived,
00417 typename internal::traits<Derived>::Scalar,
00418 internal::traits<Derived>::PacketAccess && internal::traits<OtherDerived>::PacketAccess>::run(*this, other);
00419 }
00420
00421
00422 template <class Derived>
00423 template <class OtherDerived>
00424 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
00425 {
00426 derived() = derived() * other.derived();
00427 return derived();
00428 }
00429
00430
00431
00432
00433
00434
00435
00436
00437 template <class Derived>
00438 EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
00439 QuaternionBase<Derived>::_transformVector(Vector3 v) const
00440 {
00441
00442
00443
00444
00445
00446 Vector3 uv = this->vec().cross(v);
00447 uv += uv;
00448 return v + this->w() * uv + this->vec().cross(uv);
00449 }
00450
00451 template<class Derived>
00452 EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other)
00453 {
00454 coeffs() = other.coeffs();
00455 return derived();
00456 }
00457
00458 template<class Derived>
00459 template<class OtherDerived>
00460 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
00461 {
00462 coeffs() = other.coeffs();
00463 return derived();
00464 }
00465
00466
00467
00468 template<class Derived>
00469 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
00470 {
00471 Scalar ha = Scalar(0.5)*aa.angle();
00472 this->w() = internal::cos(ha);
00473 this->vec() = internal::sin(ha) * aa.axis();
00474 return derived();
00475 }
00476
00477
00478
00479
00480
00481
00482
00483 template<class Derived>
00484 template<class MatrixDerived>
00485 inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr)
00486 {
00487 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
00488 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
00489 internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
00490 return derived();
00491 }
00492
00493
00494
00495
00496 template<class Derived>
00497 inline typename QuaternionBase<Derived>::Matrix3
00498 QuaternionBase<Derived>::toRotationMatrix(void) const
00499 {
00500
00501
00502
00503
00504 Matrix3 res;
00505
00506 const Scalar tx = 2*this->x();
00507 const Scalar ty = 2*this->y();
00508 const Scalar tz = 2*this->z();
00509 const Scalar twx = tx*this->w();
00510 const Scalar twy = ty*this->w();
00511 const Scalar twz = tz*this->w();
00512 const Scalar txx = tx*this->x();
00513 const Scalar txy = ty*this->x();
00514 const Scalar txz = tz*this->x();
00515 const Scalar tyy = ty*this->y();
00516 const Scalar tyz = tz*this->y();
00517 const Scalar tzz = tz*this->z();
00518
00519 res.coeffRef(0,0) = 1-(tyy+tzz);
00520 res.coeffRef(0,1) = txy-twz;
00521 res.coeffRef(0,2) = txz+twy;
00522 res.coeffRef(1,0) = txy+twz;
00523 res.coeffRef(1,1) = 1-(txx+tzz);
00524 res.coeffRef(1,2) = tyz-twx;
00525 res.coeffRef(2,0) = txz-twy;
00526 res.coeffRef(2,1) = tyz+twx;
00527 res.coeffRef(2,2) = 1-(txx+tyy);
00528
00529 return res;
00530 }
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542 template<class Derived>
00543 template<typename Derived1, typename Derived2>
00544 inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00545 {
00546 Vector3 v0 = a.normalized();
00547 Vector3 v1 = b.normalized();
00548 Scalar c = v1.dot(v0);
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558 if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
00559 {
00560 c = std::max<Scalar>(c,-1);
00561 Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
00562 JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
00563 Vector3 axis = svd.matrixV().col(2);
00564
00565 Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
00566 this->w() = internal::sqrt(w2);
00567 this->vec() = axis * internal::sqrt(Scalar(1) - w2);
00568 return derived();
00569 }
00570 Vector3 axis = v0.cross(v1);
00571 Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2));
00572 Scalar invs = Scalar(1)/s;
00573 this->vec() = axis * invs;
00574 this->w() = s * Scalar(0.5);
00575
00576 return derived();
00577 }
00578
00579
00580
00581
00582
00583
00584
00585 template <class Derived>
00586 inline Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::inverse() const
00587 {
00588
00589 Scalar n2 = this->squaredNorm();
00590 if (n2 > 0)
00591 return Quaternion<Scalar>(conjugate().coeffs() / n2);
00592 else
00593 {
00594
00595 return Quaternion<Scalar>(Coefficients::Zero());
00596 }
00597 }
00598
00599
00600
00601
00602
00603
00604
00605 template <class Derived>
00606 inline Quaternion<typename internal::traits<Derived>::Scalar>
00607 QuaternionBase<Derived>::conjugate() const
00608 {
00609 return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
00610 }
00611
00612
00613
00614
00615 template <class Derived>
00616 template <class OtherDerived>
00617 inline typename internal::traits<Derived>::Scalar
00618 QuaternionBase<Derived>::angularDistance(const QuaternionBase<OtherDerived>& other) const
00619 {
00620 double d = internal::abs(this->dot(other));
00621 if (d>=1.0)
00622 return Scalar(0);
00623 return static_cast<Scalar>(2 * std::acos(d));
00624 }
00625
00626
00627
00628
00629 template <class Derived>
00630 template <class OtherDerived>
00631 Quaternion<typename internal::traits<Derived>::Scalar>
00632 QuaternionBase<Derived>::slerp(Scalar t, const QuaternionBase<OtherDerived>& other) const
00633 {
00634 static const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
00635 Scalar d = this->dot(other);
00636 Scalar absD = internal::abs(d);
00637
00638 Scalar scale0;
00639 Scalar scale1;
00640
00641 if (absD>=one)
00642 {
00643 scale0 = Scalar(1) - t;
00644 scale1 = t;
00645 }
00646 else
00647 {
00648
00649 Scalar theta = std::acos(absD);
00650 Scalar sinTheta = internal::sin(theta);
00651
00652 scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
00653 scale1 = internal::sin( ( t * theta) ) / sinTheta;
00654 if (d<0)
00655 scale1 = -scale1;
00656 }
00657
00658 return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
00659 }
00660
00661 namespace internal {
00662
00663
00664 template<typename Other>
00665 struct quaternionbase_assign_impl<Other,3,3>
00666 {
00667 typedef typename Other::Scalar Scalar;
00668 typedef DenseIndex Index;
00669 template<class Derived> inline static void run(QuaternionBase<Derived>& q, const Other& mat)
00670 {
00671
00672
00673 Scalar t = mat.trace();
00674 if (t > Scalar(0))
00675 {
00676 t = sqrt(t + Scalar(1.0));
00677 q.w() = Scalar(0.5)*t;
00678 t = Scalar(0.5)/t;
00679 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
00680 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
00681 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
00682 }
00683 else
00684 {
00685 DenseIndex i = 0;
00686 if (mat.coeff(1,1) > mat.coeff(0,0))
00687 i = 1;
00688 if (mat.coeff(2,2) > mat.coeff(i,i))
00689 i = 2;
00690 DenseIndex j = (i+1)%3;
00691 DenseIndex k = (j+1)%3;
00692
00693 t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
00694 q.coeffs().coeffRef(i) = Scalar(0.5) * t;
00695 t = Scalar(0.5)/t;
00696 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
00697 q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
00698 q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
00699 }
00700 }
00701 };
00702
00703
00704 template<typename Other>
00705 struct quaternionbase_assign_impl<Other,4,1>
00706 {
00707 typedef typename Other::Scalar Scalar;
00708 template<class Derived> inline static void run(QuaternionBase<Derived>& q, const Other& vec)
00709 {
00710 q.coeffs() = vec;
00711 }
00712 };
00713
00714 }
00715
00716 #endif // EIGEN_QUATERNION_H