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 #ifndef EIGEN_QUATERNION_H
00026 #define EIGEN_QUATERNION_H
00027
00028 template<typename Other,
00029 int OtherRows=Other::RowsAtCompileTime,
00030 int OtherCols=Other::ColsAtCompileTime>
00031 struct ei_quaternion_assign_impl;
00032
00055 template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> >
00056 {
00057 typedef _Scalar Scalar;
00058 };
00059
00060 template<typename _Scalar>
00061 class Quaternion : public RotationBase<Quaternion<_Scalar>,3>
00062 {
00063 typedef RotationBase<Quaternion<_Scalar>,3> Base;
00064
00065 public:
00066 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,4)
00067
00068 using Base::operator*;
00069
00071 typedef _Scalar Scalar;
00072
00074 typedef Matrix<Scalar, 4, 1> Coefficients;
00076 typedef Matrix<Scalar,3,1> Vector3;
00078 typedef Matrix<Scalar,3,3> Matrix3;
00080 typedef AngleAxis<Scalar> AngleAxisType;
00081
00083 inline Scalar x() const { return m_coeffs.coeff(0); }
00085 inline Scalar y() const { return m_coeffs.coeff(1); }
00087 inline Scalar z() const { return m_coeffs.coeff(2); }
00089 inline Scalar w() const { return m_coeffs.coeff(3); }
00090
00092 inline Scalar& x() { return m_coeffs.coeffRef(0); }
00094 inline Scalar& y() { return m_coeffs.coeffRef(1); }
00096 inline Scalar& z() { return m_coeffs.coeffRef(2); }
00098 inline Scalar& w() { return m_coeffs.coeffRef(3); }
00099
00101 inline const Block<Coefficients,3,1> vec() const { return m_coeffs.template start<3>(); }
00102
00104 inline Block<Coefficients,3,1> vec() { return m_coeffs.template start<3>(); }
00105
00107 inline const Coefficients& coeffs() const { return m_coeffs; }
00108
00110 inline Coefficients& coeffs() { return m_coeffs; }
00111
00113 inline Quaternion() {}
00114
00122 inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z)
00123 { m_coeffs << x, y, z, w; }
00124
00126 inline Quaternion(const Quaternion& other) { m_coeffs = other.m_coeffs; }
00127
00129 explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
00130
00136 template<typename Derived>
00137 explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
00138
00139 Quaternion& operator=(const Quaternion& other);
00140 Quaternion& operator=(const AngleAxisType& aa);
00141 template<typename Derived>
00142 Quaternion& operator=(const MatrixBase<Derived>& m);
00143
00147 inline static Quaternion Identity() { return Quaternion(1, 0, 0, 0); }
00148
00151 inline Quaternion& setIdentity() { m_coeffs << 0, 0, 0, 1; return *this; }
00152
00156 inline Scalar squaredNorm() const { return m_coeffs.squaredNorm(); }
00157
00161 inline Scalar norm() const { return m_coeffs.norm(); }
00162
00165 inline void normalize() { m_coeffs.normalize(); }
00168 inline Quaternion normalized() const { return Quaternion(m_coeffs.normalized()); }
00169
00175 inline Scalar dot(const Quaternion& other) const { return m_coeffs.dot(other.m_coeffs); }
00176
00177 inline Scalar angularDistance(const Quaternion& other) const;
00178
00179 Matrix3 toRotationMatrix(void) const;
00180
00181 template<typename Derived1, typename Derived2>
00182 Quaternion& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
00183
00184 inline Quaternion operator* (const Quaternion& q) const;
00185 inline Quaternion& operator*= (const Quaternion& q);
00186
00187 Quaternion inverse(void) const;
00188 Quaternion conjugate(void) const;
00189
00190 Quaternion slerp(Scalar t, const Quaternion& other) const;
00191
00192 template<typename Derived>
00193 Vector3 operator* (const MatrixBase<Derived>& vec) const;
00194
00200 template<typename NewScalarType>
00201 inline typename ei_cast_return_type<Quaternion,Quaternion<NewScalarType> >::type cast() const
00202 { return typename ei_cast_return_type<Quaternion,Quaternion<NewScalarType> >::type(*this); }
00203
00205 template<typename OtherScalarType>
00206 inline explicit Quaternion(const Quaternion<OtherScalarType>& other)
00207 { m_coeffs = other.coeffs().template cast<Scalar>(); }
00208
00213 bool isApprox(const Quaternion& other, typename NumTraits<Scalar>::Real prec = precision<Scalar>()) const
00214 { return m_coeffs.isApprox(other.m_coeffs, prec); }
00215
00216 protected:
00217 Coefficients m_coeffs;
00218 };
00219
00222 typedef Quaternion<float> Quaternionf;
00225 typedef Quaternion<double> Quaterniond;
00226
00227
00228 template<int Arch,typename Scalar> inline Quaternion<Scalar>
00229 ei_quaternion_product(const Quaternion<Scalar>& a, const Quaternion<Scalar>& b)
00230 {
00231 return Quaternion<Scalar>
00232 (
00233 a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
00234 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
00235 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
00236 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
00237 );
00238 }
00239
00240 #ifdef EIGEN_VECTORIZE_SSE
00241 template<> inline Quaternion<float>
00242 ei_quaternion_product<EiArch_SSE,float>(const Quaternion<float>& _a, const Quaternion<float>& _b)
00243 {
00244 const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0,0,0,0x80000000));
00245 Quaternion<float> res;
00246 __m128 a = _a.coeffs().packet<Aligned>(0);
00247 __m128 b = _b.coeffs().packet<Aligned>(0);
00248 __m128 flip1 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,1,2,0,2),
00249 ei_vec4f_swizzle1(b,2,0,1,2)),mask);
00250 __m128 flip2 = _mm_xor_ps(_mm_mul_ps(ei_vec4f_swizzle1(a,3,3,3,1),
00251 ei_vec4f_swizzle1(b,0,1,2,1)),mask);
00252 ei_pstore(&res.x(),
00253 _mm_add_ps(_mm_sub_ps(_mm_mul_ps(a,ei_vec4f_swizzle1(b,3,3,3,3)),
00254 _mm_mul_ps(ei_vec4f_swizzle1(a,2,0,1,0),
00255 ei_vec4f_swizzle1(b,1,2,0,0))),
00256 _mm_add_ps(flip1,flip2)));
00257 return res;
00258 }
00259 #endif
00260
00262 template <typename Scalar>
00263 inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion& other) const
00264 {
00265 return ei_quaternion_product<EiArch>(*this,other);
00266 }
00267
00269 template <typename Scalar>
00270 inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const Quaternion& other)
00271 {
00272 return (*this = *this * other);
00273 }
00274
00282 template <typename Scalar>
00283 template<typename Derived>
00284 inline typename Quaternion<Scalar>::Vector3
00285 Quaternion<Scalar>::operator* (const MatrixBase<Derived>& v) const
00286 {
00287
00288
00289
00290
00291
00292 Vector3 uv;
00293 uv = 2 * this->vec().cross(v);
00294 return v + this->w() * uv + this->vec().cross(uv);
00295 }
00296
00297 template<typename Scalar>
00298 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion& other)
00299 {
00300 m_coeffs = other.m_coeffs;
00301 return *this;
00302 }
00303
00306 template<typename Scalar>
00307 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const AngleAxisType& aa)
00308 {
00309 Scalar ha = Scalar(0.5)*aa.angle();
00310 this->w() = ei_cos(ha);
00311 this->vec() = ei_sin(ha) * aa.axis();
00312 return *this;
00313 }
00314
00320 template<typename Scalar>
00321 template<typename Derived>
00322 inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const MatrixBase<Derived>& xpr)
00323 {
00324 ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived());
00325 return *this;
00326 }
00327
00329 template<typename Scalar>
00330 inline typename Quaternion<Scalar>::Matrix3
00331 Quaternion<Scalar>::toRotationMatrix(void) const
00332 {
00333
00334
00335
00336
00337 Matrix3 res;
00338
00339 const Scalar tx = 2*this->x();
00340 const Scalar ty = 2*this->y();
00341 const Scalar tz = 2*this->z();
00342 const Scalar twx = tx*this->w();
00343 const Scalar twy = ty*this->w();
00344 const Scalar twz = tz*this->w();
00345 const Scalar txx = tx*this->x();
00346 const Scalar txy = ty*this->x();
00347 const Scalar txz = tz*this->x();
00348 const Scalar tyy = ty*this->y();
00349 const Scalar tyz = tz*this->y();
00350 const Scalar tzz = tz*this->z();
00351
00352 res.coeffRef(0,0) = 1-(tyy+tzz);
00353 res.coeffRef(0,1) = txy-twz;
00354 res.coeffRef(0,2) = txz+twy;
00355 res.coeffRef(1,0) = txy+twz;
00356 res.coeffRef(1,1) = 1-(txx+tzz);
00357 res.coeffRef(1,2) = tyz-twx;
00358 res.coeffRef(2,0) = txz-twy;
00359 res.coeffRef(2,1) = tyz+twx;
00360 res.coeffRef(2,2) = 1-(txx+tyy);
00361
00362 return res;
00363 }
00364
00371 template<typename Scalar>
00372 template<typename Derived1, typename Derived2>
00373 inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
00374 {
00375 Vector3 v0 = a.normalized();
00376 Vector3 v1 = b.normalized();
00377 Scalar c = v0.dot(v1);
00378
00379
00380 if (ei_isApprox(c,Scalar(1)))
00381 {
00382
00383 this->w() = 1; this->vec().setZero();
00384 return *this;
00385 }
00386
00387 if (ei_isApprox(c,Scalar(-1)))
00388 {
00389 this->vec() = v0.unitOrthogonal();
00390 this->w() = 0;
00391 return *this;
00392 }
00393
00394 Vector3 axis = v0.cross(v1);
00395 Scalar s = ei_sqrt((Scalar(1)+c)*Scalar(2));
00396 Scalar invs = Scalar(1)/s;
00397 this->vec() = axis * invs;
00398 this->w() = s * Scalar(0.5);
00399
00400 return *this;
00401 }
00402
00409 template <typename Scalar>
00410 inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
00411 {
00412
00413 Scalar n2 = this->squaredNorm();
00414 if (n2 > 0)
00415 return Quaternion(conjugate().coeffs() / n2);
00416 else
00417 {
00418
00419 return Quaternion(Coefficients::Zero());
00420 }
00421 }
00422
00429 template <typename Scalar>
00430 inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const
00431 {
00432 return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
00433 }
00434
00438 template <typename Scalar>
00439 inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other) const
00440 {
00441 double d = ei_abs(this->dot(other));
00442 if (d>=1.0)
00443 return 0;
00444 return Scalar(2) * std::acos(d);
00445 }
00446
00450 template <typename Scalar>
00451 Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion& other) const
00452 {
00453 static const Scalar one = Scalar(1) - machine_epsilon<Scalar>();
00454 Scalar d = this->dot(other);
00455 Scalar absD = ei_abs(d);
00456
00457 Scalar scale0;
00458 Scalar scale1;
00459
00460 if (absD>=one)
00461 {
00462 scale0 = Scalar(1) - t;
00463 scale1 = t;
00464 }
00465 else
00466 {
00467
00468 Scalar theta = std::acos(absD);
00469 Scalar sinTheta = ei_sin(theta);
00470
00471 scale0 = ei_sin( ( Scalar(1) - t ) * theta) / sinTheta;
00472 scale1 = ei_sin( ( t * theta) ) / sinTheta;
00473 if (d<0)
00474 scale1 = -scale1;
00475 }
00476
00477 return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
00478 }
00479
00480
00481 template<typename Other>
00482 struct ei_quaternion_assign_impl<Other,3,3>
00483 {
00484 typedef typename Other::Scalar Scalar;
00485 inline static void run(Quaternion<Scalar>& q, const Other& mat)
00486 {
00487
00488
00489 Scalar t = mat.trace();
00490 if (t > 0)
00491 {
00492 t = ei_sqrt(t + Scalar(1.0));
00493 q.w() = Scalar(0.5)*t;
00494 t = Scalar(0.5)/t;
00495 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
00496 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
00497 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
00498 }
00499 else
00500 {
00501 int i = 0;
00502 if (mat.coeff(1,1) > mat.coeff(0,0))
00503 i = 1;
00504 if (mat.coeff(2,2) > mat.coeff(i,i))
00505 i = 2;
00506 int j = (i+1)%3;
00507 int k = (j+1)%3;
00508
00509 t = ei_sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
00510 q.coeffs().coeffRef(i) = Scalar(0.5) * t;
00511 t = Scalar(0.5)/t;
00512 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
00513 q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
00514 q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
00515 }
00516 }
00517 };
00518
00519
00520 template<typename Other>
00521 struct ei_quaternion_assign_impl<Other,4,1>
00522 {
00523 typedef typename Other::Scalar Scalar;
00524 inline static void run(Quaternion<Scalar>& q, const Other& vec)
00525 {
00526 q.coeffs() = vec;
00527 }
00528 };
00529
00530 #endif // EIGEN_QUATERNION_H