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(
const Scalar& w,
const Scalar& x,
const Scalar& y,
const 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>
500 Scalar ha = Scalar(0.5)*aa.angle();
502 this->vec() = sin(ha) * aa.axis();
512 template<
class Derived>
513 template<
class MatrixDerived>
516 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
517 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
518 internal::quaternionbase_assign_impl<MatrixDerived>::run(*
this, xpr.derived());
525 template<
class Derived>
535 const Scalar tx = Scalar(2)*this->x();
536 const Scalar ty = Scalar(2)*this->y();
537 const Scalar tz = Scalar(2)*this->z();
538 const Scalar twx = tx*this->w();
539 const Scalar twy = ty*this->w();
540 const Scalar twz = tz*this->w();
541 const Scalar txx = tx*this->x();
542 const Scalar txy = ty*this->x();
543 const Scalar txz = tz*this->x();
544 const Scalar tyy = ty*this->y();
545 const Scalar tyz = tz*this->y();
546 const Scalar tzz = tz*this->z();
548 res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
549 res.coeffRef(0,1) = txy-twz;
550 res.coeffRef(0,2) = txz+twy;
551 res.coeffRef(1,0) = txy+twz;
552 res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
553 res.coeffRef(1,2) = tyz-twx;
554 res.coeffRef(2,0) = txz-twy;
555 res.coeffRef(2,1) = tyz+twx;
556 res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
571 template<
class Derived>
572 template<
typename Derived1,
typename Derived2>
579 Scalar c = v1.dot(v0);
591 c = max<Scalar>(c,-1);
596 Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
597 this->w() = sqrt(w2);
598 this->vec() = axis * sqrt(Scalar(1) - w2);
602 Scalar s = sqrt((Scalar(1)+c)*Scalar(2));
603 Scalar invs = Scalar(1)/s;
604 this->vec() = axis * invs;
605 this->w() = s * Scalar(0.5);
621 template<
typename Scalar,
int Options>
622 template<
typename Derived1,
typename Derived2>
626 quat.setFromTwoVectors(a, b);
637 template <
class Derived>
641 Scalar n2 = this->squaredNorm();
657 template <
class Derived>
667 template <
class Derived>
668 template <
class OtherDerived>
669 inline typename internal::traits<Derived>::Scalar
674 double d = abs(this->dot(other));
677 return static_cast<Scalar
>(2 * acos(d));
683 template <
class Derived>
684 template <
class OtherDerived>
692 Scalar d = this->dot(other);
693 Scalar absD = abs(d);
700 scale0 = Scalar(1) - t;
706 Scalar theta = acos(absD);
707 Scalar sinTheta = sin(theta);
709 scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;
710 scale1 = sin( ( t * theta) ) / sinTheta;
712 if(d<0) scale1 = -scale1;
720 template<
typename Other>
721 struct quaternionbase_assign_impl<Other,3,3>
723 typedef typename Other::Scalar Scalar;
724 typedef DenseIndex Index;
730 Scalar t = mat.trace();
733 t = sqrt(t + Scalar(1.0));
734 q.w() = Scalar(0.5)*t;
736 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
737 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
738 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
743 if (mat.coeff(1,1) > mat.coeff(0,0))
745 if (mat.coeff(2,2) > mat.coeff(i,i))
747 DenseIndex j = (i+1)%3;
748 DenseIndex k = (j+1)%3;
750 t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
751 q.coeffs().coeffRef(i) = Scalar(0.5) * t;
753 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
754 q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
755 q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
761 template<
typename Other>
762 struct quaternionbase_assign_impl<Other,4,1>
764 typedef typename Other::Scalar Scalar;
765 template<
class Derived>
static inline void run(QuaternionBase<Derived>& q,
const Other& vec)
775 #endif // EIGEN_QUATERNION_H
Map(const Scalar *coeffs)
Definition: Quaternion.h:355
Scalar x() const
Definition: Quaternion.h:60
Quaternion< float > Quaternionf
Definition: Quaternion.h:297
Scalar norm() const
Definition: Quaternion.h:119
Scalar dot(const QuaternionBase< OtherDerived > &other) const
Definition: Quaternion.h:133
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:104
Scalar squaredNorm() const
Definition: Quaternion.h:114
internal::traits< Quaternion< _Scalar, _Options > >::Scalar Scalar
Definition: RotationBase.h:34
const internal::traits< Derived >::Coefficients & coeffs() const
Definition: Quaternion.h:84
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:88
const PlainObject normalized() const
Definition: Dot.h:139
Scalar & w()
Definition: Quaternion.h:75
Definition: Constants.h:331
Quaternion< Scalar > inverse() const
Definition: Quaternion.h:638
VectorBlock< Coefficients, 3 > vec()
Definition: Quaternion.h:81
Map(Scalar *coeffs)
Definition: Quaternion.h:392
Expression of a fixed-size or dynamic-size sub-vector.
Definition: ForwardDeclarations.h:83
Quaternion(const AngleAxisType &aa)
Definition: Quaternion.h:261
Scalar y() const
Definition: Quaternion.h:62
Scalar & z()
Definition: Quaternion.h:73
internal::traits< Derived >::Coefficients & coeffs()
Definition: Quaternion.h:87
const unsigned int LvalueBit
Definition: Constants.h:131
Quaternion(const Scalar *data)
Definition: Quaternion.h:255
Quaternion< Scalar > conjugate() const
Definition: Quaternion.h:659
Definition: Constants.h:270
Quaternion< double > Quaterniond
Definition: Quaternion.h:300
Derived & setFromTwoVectors(const MatrixBase< Derived1 > &a, const MatrixBase< Derived2 > &b)
Definition: Quaternion.h:573
Quaternion< Scalar > normalized() const
Definition: Quaternion.h:126
internal::cast_return_type< Derived, Quaternion< NewScalarType > >::type cast() const
Definition: Quaternion.h:176
Quaternion(const Quaternion< OtherScalar, OtherOptions > &other)
Definition: Quaternion.h:272
Base class for quaternion expressions.
Definition: ForwardDeclarations.h:233
Matrix< Scalar, 3, 1 > Vector3
Definition: Quaternion.h:51
Quaternion< Scalar > slerp(const Scalar &t, const QuaternionBase< OtherDerived > &other) const
Quaternion(const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)
Definition: Quaternion.h:252
AngleAxis< Scalar > AngleAxisType
Definition: Quaternion.h:55
Scalar & x()
Definition: Quaternion.h:69
Matrix3 toRotationMatrix() const
Definition: Quaternion.h:527
const MatrixVType & matrixV() const
Definition: JacobiSVD.h:618
Scalar z() const
Definition: Quaternion.h:64
Map< Quaternion< double >, Aligned > QuaternionMapAlignedd
Definition: Quaternion.h:412
bool isApprox(const QuaternionBase< OtherDerived > &other, const RealScalar &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: Quaternion.h:164
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:260
Vector3 _transformVector(Vector3 v) const
Definition: Quaternion.h:466
Definition: Constants.h:194
Quaternion(const MatrixBase< Derived > &other)
Definition: Quaternion.h:268
Two-sided Jacobi SVD decomposition of a rectangular matrix.
Definition: ForwardDeclarations.h:224
Map< Quaternion< float >, Aligned > QuaternionMapAlignedf
Definition: Quaternion.h:409
Scalar & y()
Definition: Quaternion.h:71
void normalize()
Definition: Quaternion.h:123
Scalar w() const
Definition: Quaternion.h:66
Matrix< Scalar, 3, 3 > Matrix3
Definition: Quaternion.h:53
Map< Quaternion< float >, 0 > QuaternionMapf
Definition: Quaternion.h:403
Map< Quaternion< double >, 0 > QuaternionMapd
Definition: Quaternion.h:406
const unsigned int AlignedBit
Definition: Constants.h:147
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:48
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition: ForwardDeclarations.h:235
Quaternion(const QuaternionBase< Derived > &other)
Definition: Quaternion.h:258
Derived & operator*=(const QuaternionBase< OtherDerived > &q)
Definition: Quaternion.h:451
static Quaternion< Scalar > Identity()
Definition: Quaternion.h:105
QuaternionBase & setIdentity()
Definition: Quaternion.h:109
const VectorBlock< const Coefficients, 3 > vec() const
Definition: Quaternion.h:78