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>
39 using Base::operator*;
42 typedef typename internal::traits<Derived>::Scalar Scalar;
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>
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
213 template<
typename _Scalar,
int _Options>
214 struct traits<Quaternion<_Scalar,_Options> >
216 typedef Quaternion<_Scalar,_Options> PlainObject;
217 typedef _Scalar Scalar;
218 typedef Matrix<_Scalar,4,1,_Options> Coefficients;
220 IsAligned = internal::traits<Coefficients>::Flags &
AlignedBit,
226 template<
typename _Scalar,
int _Options>
230 enum { IsAligned = internal::traits<Quaternion>::IsAligned };
233 typedef _Scalar Scalar;
235 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(
Quaternion)
236 using Base::operator*=;
238 typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
251 inline Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z, w){}
266 template<
typename Derived>
270 template<
typename OtherScalar,
int OtherOptions>
272 { m_coeffs = other.coeffs().template cast<Scalar>(); }
274 template<
typename Derived1,
typename Derived2>
277 inline Coefficients& coeffs() {
return m_coeffs;}
278 inline const Coefficients& coeffs()
const {
return m_coeffs;}
280 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
283 Coefficients m_coeffs;
285 #ifndef EIGEN_PARSED_BY_DOXYGEN
286 static EIGEN_STRONG_INLINE
void _check_template_params()
288 EIGEN_STATIC_ASSERT( (_Options & DontAlign) == _Options,
289 INVALID_MATRIX_TEMPLATE_PARAMETERS)
306 template<
typename _Scalar,
int _Options>
307 struct traits<Map<Quaternion<_Scalar>, _Options> >:
308 traits<Quaternion<_Scalar, _Options> >
310 typedef _Scalar Scalar;
313 typedef traits<Quaternion<_Scalar, _Options> > TraitsBase;
315 IsAligned = TraitsBase::IsAligned,
317 Flags = TraitsBase::Flags
323 template<
typename _Scalar,
int _Options>
324 struct traits<Map<const Quaternion<_Scalar>, _Options> >:
325 traits<Quaternion<_Scalar> >
327 typedef _Scalar Scalar;
328 typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
330 typedef traits<Quaternion<_Scalar, _Options> > TraitsBase;
332 IsAligned = TraitsBase::IsAligned,
348 template<
typename _Scalar,
int _Options>
350 :
public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
355 typedef _Scalar Scalar;
356 typedef typename internal::traits<Map>::Coefficients Coefficients;
357 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(
Map)
358 using Base::operator*=;
366 EIGEN_STRONG_INLINE
Map(
const Scalar* coeffs) : m_coeffs(coeffs) {}
368 inline const Coefficients& coeffs()
const {
return m_coeffs;}
371 const Coefficients m_coeffs;
384 template<
typename _Scalar,
int _Options>
391 typedef _Scalar Scalar;
392 typedef typename internal::traits<Map>::Coefficients Coefficients;
393 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(
Map)
394 using Base::operator*=;
402 EIGEN_STRONG_INLINE
Map(Scalar* coeffs) : m_coeffs(coeffs) {}
404 inline Coefficients& coeffs() {
return m_coeffs; }
405 inline const Coefficients& coeffs()
const {
return m_coeffs; }
408 Coefficients m_coeffs;
431 template<
int Arch,
class Derived1,
class Derived2,
typename Scalar,
int _Options>
struct quat_product
436 a.
w() * b.
w() - a.
x() * b.
x() - a.
y() * b.
y() - a.
z() * b.
z(),
437 a.
w() * b.
x() + a.
x() * b.
w() + a.
y() * b.
z() - a.
z() * b.
y(),
438 a.
w() * b.
y() + a.
y() * b.
w() + a.
z() * b.
x() - a.
x() * b.
z(),
439 a.
w() * b.
z() + a.
z() * b.
w() + a.
x() * b.
y() - a.
y() * b.
x()
446 template <
class Derived>
447 template <
class OtherDerived>
448 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
451 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
452 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
453 return internal::quat_product<Architecture::Target, Derived, OtherDerived,
454 typename internal::traits<Derived>::Scalar,
455 internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*
this, other);
459 template <
class Derived>
460 template <
class OtherDerived>
463 derived() = derived() * other.derived();
474 template <
class Derived>
483 Vector3 uv = this->vec().cross(v);
485 return v + this->w() * uv + this->vec().cross(uv);
488 template<
class Derived>
491 coeffs() = other.
coeffs();
495 template<
class Derived>
496 template<
class OtherDerived>
497 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(
const QuaternionBase<OtherDerived>& other)
499 coeffs() = other.coeffs();
505 template<
class Derived>
508 Scalar ha = Scalar(0.5)*aa.angle();
509 this->w() = internal::cos(ha);
510 this->vec() = internal::sin(ha) * aa.axis();
520 template<
class Derived>
521 template<
class MatrixDerived>
524 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
525 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
526 internal::quaternionbase_assign_impl<MatrixDerived>::run(*
this, xpr.derived());
533 template<
class Derived>
543 const Scalar tx = Scalar(2)*this->x();
544 const Scalar ty = Scalar(2)*this->y();
545 const Scalar tz = Scalar(2)*this->z();
546 const Scalar twx = tx*this->w();
547 const Scalar twy = ty*this->w();
548 const Scalar twz = tz*this->w();
549 const Scalar txx = tx*this->x();
550 const Scalar txy = ty*this->x();
551 const Scalar txz = tz*this->x();
552 const Scalar tyy = ty*this->y();
553 const Scalar tyz = tz*this->y();
554 const Scalar tzz = tz*this->z();
556 res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
557 res.coeffRef(0,1) = txy-twz;
558 res.coeffRef(0,2) = txz+twy;
559 res.coeffRef(1,0) = txy+twz;
560 res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
561 res.coeffRef(1,2) = tyz-twx;
562 res.coeffRef(2,0) = txz-twy;
563 res.coeffRef(2,1) = tyz+twx;
564 res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
579 template<
class Derived>
580 template<
typename Derived1,
typename Derived2>
586 Scalar c = v1.dot(v0);
598 c = max<Scalar>(c,-1);
603 Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
604 this->w() = internal::sqrt(w2);
605 this->vec() = axis * internal::sqrt(Scalar(1) - w2);
609 Scalar s = internal::sqrt((Scalar(1)+c)*Scalar(2));
610 Scalar invs = Scalar(1)/s;
611 this->vec() = axis * invs;
612 this->w() = s * Scalar(0.5);
628 template<
typename Scalar,
int Options>
629 template<
typename Derived1,
typename Derived2>
644 template <
class Derived>
648 Scalar n2 = this->squaredNorm();
664 template <
class Derived>
674 template <
class Derived>
675 template <
class OtherDerived>
676 inline typename internal::traits<Derived>::Scalar
683 return static_cast<Scalar
>(2 * acos(d));
689 template <
class Derived>
690 template <
class OtherDerived>
696 Scalar d = this->dot(other);
704 scale0 = Scalar(1) - t;
710 Scalar theta = acos(absD);
711 Scalar sinTheta = internal::sin(theta);
713 scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
714 scale1 = internal::sin( ( t * theta) ) / sinTheta;
716 if(d<0) scale1 = -scale1;
724 template<
typename Other>
725 struct quaternionbase_assign_impl<Other,3,3>
727 typedef typename Other::Scalar Scalar;
728 typedef DenseIndex Index;
733 Scalar t = mat.trace();
736 t = sqrt(t + Scalar(1.0));
737 q.
w() = Scalar(0.5)*t;
739 q.
x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
740 q.
y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
741 q.
z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
746 if (mat.coeff(1,1) > mat.coeff(0,0))
748 if (mat.coeff(2,2) > mat.coeff(i,i))
750 DenseIndex j = (i+1)%3;
751 DenseIndex k = (j+1)%3;
753 t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
754 q.
coeffs().coeffRef(i) = Scalar(0.5) * t;
756 q.
w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
757 q.
coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
758 q.
coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
764 template<
typename Other>
765 struct quaternionbase_assign_impl<Other,4,1>
767 typedef typename Other::Scalar Scalar;
768 template<
class Derived>
static inline void run(QuaternionBase<Derived>& q,
const Other& vec)
778 #endif // EIGEN_QUATERNION_H