11 #ifndef EIGEN_QUATERNION_H
12 #define EIGEN_QUATERNION_H
22 template<
typename Other,
23 int OtherRows=Other::RowsAtCompileTime,
24 int OtherCols=Other::ColsAtCompileTime>
25 struct quaternionbase_assign_impl;
34 template<
class Derived>
35 class QuaternionBase :
public RotationBase<Derived, 3>
37 typedef RotationBase<Derived, 3> Base;
39 using Base::operator*;
42 typedef typename internal::traits<Derived>::Scalar Scalar;
43 typedef typename NumTraits<Scalar>::Real RealScalar;
44 typedef typename internal::traits<Derived>::Coefficients Coefficients;
46 Flags = Eigen::internal::traits<Derived>::Flags
60 inline Scalar x()
const {
return this->derived().coeffs().coeff(0); }
62 inline Scalar y()
const {
return this->derived().coeffs().coeff(1); }
64 inline Scalar z()
const {
return this->derived().coeffs().coeff(2); }
66 inline Scalar w()
const {
return this->derived().coeffs().coeff(3); }
69 inline Scalar&
x() {
return this->derived().coeffs().coeffRef(0); }
71 inline Scalar&
y() {
return this->derived().coeffs().coeffRef(1); }
73 inline Scalar&
z() {
return this->derived().coeffs().coeffRef(2); }
75 inline Scalar&
w() {
return this->derived().coeffs().coeffRef(3); }
84 inline const typename internal::traits<Derived>::Coefficients&
coeffs()
const {
return derived().coeffs(); }
87 inline typename internal::traits<Derived>::Coefficients&
coeffs() {
return derived().coeffs(); }
141 template<
typename Derived1,
typename Derived2>
163 template<
class OtherDerived>
165 {
return coeffs().isApprox(other.coeffs(), prec); }
175 template<
typename NewScalarType>
176 inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type
cast()
const
178 return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived());
181 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
182 # include EIGEN_QUATERNIONBASE_PLUGIN
214 template<
typename _Scalar,
int _Options>
215 struct traits<Quaternion<_Scalar,_Options> >
217 typedef Quaternion<_Scalar,_Options> PlainObject;
218 typedef _Scalar Scalar;
219 typedef Matrix<_Scalar,4,1,_Options> Coefficients;
221 IsAligned = internal::traits<Coefficients>::Flags &
AlignedBit,
227 template<
typename _Scalar,
int _Options>
228 class Quaternion :
public QuaternionBase<Quaternion<_Scalar,_Options> >
230 typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base;
231 enum { IsAligned = internal::traits<Quaternion>::IsAligned };
234 typedef _Scalar Scalar;
236 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion)
237 using Base::operator*=;
239 typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
240 typedef typename Base::AngleAxisType AngleAxisType;
252 inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){}
267 template<
typename Derived>
271 template<
typename OtherScalar,
int OtherOptions>
273 { m_coeffs = other.coeffs().template cast<Scalar>(); }
275 template<
typename Derived1,
typename Derived2>
278 inline Coefficients& coeffs() {
return m_coeffs;}
279 inline const Coefficients& coeffs()
const {
return m_coeffs;}
281 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
284 Coefficients m_coeffs;
286 #ifndef EIGEN_PARSED_BY_DOXYGEN
287 static EIGEN_STRONG_INLINE
void _check_template_params()
289 EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
290 INVALID_MATRIX_TEMPLATE_PARAMETERS)
307 template<
typename _Scalar,
int _Options>
308 struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
315 template<
typename _Scalar,
int _Options>
316 struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
318 typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
319 typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase;
337 template<
typename _Scalar,
int _Options>
339 :
public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
344 typedef _Scalar Scalar;
345 typedef typename internal::traits<Map>::Coefficients Coefficients;
346 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(
Map)
347 using Base::operator*=;
355 EIGEN_STRONG_INLINE
Map(
const Scalar* coeffs) : m_coeffs(coeffs) {}
357 inline const Coefficients& coeffs()
const {
return m_coeffs;}
360 const Coefficients m_coeffs;
374 template<
typename _Scalar,
int _Options>
381 typedef _Scalar Scalar;
382 typedef typename internal::traits<Map>::Coefficients Coefficients;
383 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(
Map)
384 using Base::operator*=;
392 EIGEN_STRONG_INLINE
Map(Scalar* coeffs) : m_coeffs(coeffs) {}
394 inline Coefficients& coeffs() {
return m_coeffs; }
395 inline const Coefficients& coeffs()
const {
return m_coeffs; }
398 Coefficients m_coeffs;
421 template<
int Arch,
class Derived1,
class Derived2,
typename Scalar,
int _Options>
struct quat_product
426 a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
427 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
428 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
429 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
436 template <
class Derived>
437 template <
class OtherDerived>
438 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
441 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
442 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
443 return internal::quat_product<Architecture::Target, Derived, OtherDerived,
444 typename internal::traits<Derived>::Scalar,
445 internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*
this, other);
449 template <
class Derived>
450 template <
class OtherDerived>
453 derived() = derived() * other.derived();
464 template <
class Derived>
473 Vector3 uv = this->vec().cross(v);
475 return v + this->w() * uv + this->vec().cross(uv);
478 template<
class Derived>
481 coeffs() = other.coeffs();
485 template<
class Derived>
486 template<
class OtherDerived>
487 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(
const QuaternionBase<OtherDerived>& other)
489 coeffs() = other.coeffs();
495 template<
class Derived>
498 Scalar ha = Scalar(0.5)*aa.angle();
499 this->w() = internal::cos(ha);
500 this->vec() = internal::sin(ha) * aa.axis();
510 template<
class Derived>
511 template<
class MatrixDerived>
514 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
515 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
516 internal::quaternionbase_assign_impl<MatrixDerived>::run(*
this, xpr.derived());
523 template<
class Derived>
533 const Scalar tx = Scalar(2)*this->x();
534 const Scalar ty = Scalar(2)*this->y();
535 const Scalar tz = Scalar(2)*this->z();
536 const Scalar twx = tx*this->w();
537 const Scalar twy = ty*this->w();
538 const Scalar twz = tz*this->w();
539 const Scalar txx = tx*this->x();
540 const Scalar txy = ty*this->x();
541 const Scalar txz = tz*this->x();
542 const Scalar tyy = ty*this->y();
543 const Scalar tyz = tz*this->y();
544 const Scalar tzz = tz*this->z();
546 res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
547 res.coeffRef(0,1) = txy-twz;
548 res.coeffRef(0,2) = txz+twy;
549 res.coeffRef(1,0) = txy+twz;
550 res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
551 res.coeffRef(1,2) = tyz-twx;
552 res.coeffRef(2,0) = txz-twy;
553 res.coeffRef(2,1) = tyz+twx;
554 res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
569 template<
class Derived>
570 template<
typename Derived1,
typename Derived2>
576 Scalar c = v1.dot(v0);
588 c = max<Scalar>(c,-1);
593 Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
594 this->w() = internal::sqrt(w2);
595 this->vec() = axis * internal::sqrt(Scalar(1) - w2);
599 Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2));
600 Scalar invs = Scalar(1)/s;
601 this->vec() = axis * invs;
602 this->w() = s * Scalar(0.5);
618 template<
typename Scalar,
int Options>
619 template<
typename Derived1,
typename Derived2>
623 quat.setFromTwoVectors(a, b);
634 template <
class Derived>
638 Scalar n2 = this->squaredNorm();
654 template <
class Derived>
664 template <
class Derived>
665 template <
class OtherDerived>
666 inline typename internal::traits<Derived>::Scalar
670 double d = internal::abs(this->dot(other));
673 return static_cast<Scalar
>(2 * acos(d));
679 template <
class Derived>
680 template <
class OtherDerived>
686 Scalar d = this->dot(other);
687 Scalar absD = internal::abs(d);
694 scale0 = Scalar(1) - t;
700 Scalar theta = acos(absD);
701 Scalar sinTheta = internal::sin(theta);
703 scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
704 scale1 = internal::sin( ( t * theta) ) / sinTheta;
706 if(d<0) scale1 = -scale1;
714 template<
typename Other>
715 struct quaternionbase_assign_impl<Other,3,3>
717 typedef typename Other::Scalar Scalar;
718 typedef DenseIndex Index;
723 Scalar t = mat.trace();
726 t = sqrt(t + Scalar(1.0));
727 q.w() = Scalar(0.5)*t;
729 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
730 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
731 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
736 if (mat.coeff(1,1) > mat.coeff(0,0))
738 if (mat.coeff(2,2) > mat.coeff(i,i))
740 DenseIndex j = (i+1)%3;
741 DenseIndex k = (j+1)%3;
743 t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
744 q.coeffs().coeffRef(i) = Scalar(0.5) * t;
746 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
747 q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
748 q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
754 template<
typename Other>
755 struct quaternionbase_assign_impl<Other,4,1>
757 typedef typename Other::Scalar Scalar;
758 template<
class Derived>
static inline void run(QuaternionBase<Derived>& q,
const Other& vec)
768 #endif // EIGEN_QUATERNION_H