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>
159 template<
class OtherDerived>
161 {
return coeffs().isApprox(other.coeffs(), prec); }
171 template<
typename NewScalarType>
172 inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type
cast()
const
174 return typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type(derived());
177 #ifdef EIGEN_QUATERNIONBASE_PLUGIN
178 # include EIGEN_QUATERNIONBASE_PLUGIN
210 template<
typename _Scalar,
int _Options>
211 struct traits<Quaternion<_Scalar,_Options> >
213 typedef Quaternion<_Scalar,_Options> PlainObject;
214 typedef _Scalar Scalar;
215 typedef Matrix<_Scalar,4,1,_Options> Coefficients;
217 IsAligned = internal::traits<Coefficients>::Flags &
AlignedBit,
223 template<
typename _Scalar,
int _Options>
224 class Quaternion :
public QuaternionBase<Quaternion<_Scalar,_Options> >
226 typedef QuaternionBase<Quaternion<_Scalar,_Options> > Base;
227 enum { IsAligned = internal::traits<Quaternion>::IsAligned };
230 typedef _Scalar Scalar;
232 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Quaternion)
233 using Base::operator*=;
235 typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
236 typedef typename Base::AngleAxisType AngleAxisType;
248 inline Quaternion(
const Scalar& w,
const Scalar& x,
const Scalar& y,
const Scalar& z) : m_coeffs(x, y, z, w){}
263 template<
typename Derived>
267 template<
typename OtherScalar,
int OtherOptions>
269 { m_coeffs = other.coeffs().template cast<Scalar>(); }
271 template<
typename Derived1,
typename Derived2>
274 inline Coefficients& coeffs() {
return m_coeffs;}
275 inline const Coefficients& coeffs()
const {
return m_coeffs;}
277 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(IsAligned)
280 Coefficients m_coeffs;
282 #ifndef EIGEN_PARSED_BY_DOXYGEN
283 static EIGEN_STRONG_INLINE
void _check_template_params()
285 EIGEN_STATIC_ASSERT( (_Options &
DontAlign) == _Options,
286 INVALID_MATRIX_TEMPLATE_PARAMETERS)
303 template<
typename _Scalar,
int _Options>
304 struct traits<Map<Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
311 template<
typename _Scalar,
int _Options>
312 struct traits<Map<const Quaternion<_Scalar>, _Options> > : traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> >
314 typedef Map<const Matrix<_Scalar,4,1>, _Options> Coefficients;
315 typedef traits<Quaternion<_Scalar, (int(_Options)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase;
333 template<
typename _Scalar,
int _Options>
335 :
public QuaternionBase<Map<const Quaternion<_Scalar>, _Options> >
340 typedef _Scalar Scalar;
341 typedef typename internal::traits<Map>::Coefficients Coefficients;
342 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(
Map)
343 using Base::operator*=;
351 EIGEN_STRONG_INLINE
Map(
const Scalar* coeffs) : m_coeffs(coeffs) {}
353 inline const Coefficients& coeffs()
const {
return m_coeffs;}
356 const Coefficients m_coeffs;
370 template<
typename _Scalar,
int _Options>
377 typedef _Scalar Scalar;
378 typedef typename internal::traits<Map>::Coefficients Coefficients;
379 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(
Map)
380 using Base::operator*=;
388 EIGEN_STRONG_INLINE
Map(Scalar* coeffs) : m_coeffs(coeffs) {}
390 inline Coefficients& coeffs() {
return m_coeffs; }
391 inline const Coefficients& coeffs()
const {
return m_coeffs; }
394 Coefficients m_coeffs;
417 template<
int Arch,
class Derived1,
class Derived2,
typename Scalar,
int _Options>
struct quat_product
422 a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
423 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
424 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
425 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
432 template <
class Derived>
433 template <
class OtherDerived>
434 EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
437 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
438 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
439 return internal::quat_product<Architecture::Target, Derived, OtherDerived,
440 typename internal::traits<Derived>::Scalar,
441 internal::traits<Derived>::IsAligned && internal::traits<OtherDerived>::IsAligned>::run(*
this, other);
445 template <
class Derived>
446 template <
class OtherDerived>
449 derived() = derived() * other.derived();
460 template <
class Derived>
469 Vector3 uv = this->vec().cross(v);
471 return v + this->w() * uv + this->vec().cross(uv);
474 template<
class Derived>
477 coeffs() = other.coeffs();
481 template<
class Derived>
482 template<
class OtherDerived>
483 EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(
const QuaternionBase<OtherDerived>& other)
485 coeffs() = other.coeffs();
491 template<
class Derived>
496 Scalar ha = Scalar(0.5)*aa.angle();
498 this->vec() = sin(ha) * aa.axis();
508 template<
class Derived>
509 template<
class MatrixDerived>
512 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
513 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
514 internal::quaternionbase_assign_impl<MatrixDerived>::run(*
this, xpr.derived());
521 template<
class Derived>
531 const Scalar tx = Scalar(2)*this->x();
532 const Scalar ty = Scalar(2)*this->y();
533 const Scalar tz = Scalar(2)*this->z();
534 const Scalar twx = tx*this->w();
535 const Scalar twy = ty*this->w();
536 const Scalar twz = tz*this->w();
537 const Scalar txx = tx*this->x();
538 const Scalar txy = ty*this->x();
539 const Scalar txz = tz*this->x();
540 const Scalar tyy = ty*this->y();
541 const Scalar tyz = tz*this->y();
542 const Scalar tzz = tz*this->z();
544 res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
545 res.coeffRef(0,1) = txy-twz;
546 res.coeffRef(0,2) = txz+twy;
547 res.coeffRef(1,0) = txy+twz;
548 res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
549 res.coeffRef(1,2) = tyz-twx;
550 res.coeffRef(2,0) = txz-twy;
551 res.coeffRef(2,1) = tyz+twx;
552 res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
567 template<
class Derived>
568 template<
typename Derived1,
typename Derived2>
575 Scalar c = v1.dot(v0);
587 c = max<Scalar>(c,-1);
592 Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
593 this->w() = sqrt(w2);
594 this->vec() = axis * sqrt(Scalar(1) - w2);
598 Scalar s = sqrt((Scalar(1)+c)*Scalar(2));
599 Scalar invs = Scalar(1)/s;
600 this->vec() = axis * invs;
601 this->w() = s * Scalar(0.5);
617 template<
typename Scalar,
int Options>
618 template<
typename Derived1,
typename Derived2>
622 quat.setFromTwoVectors(a, b);
633 template <
class Derived>
637 Scalar n2 = this->squaredNorm();
653 template <
class Derived>
663 template <
class Derived>
664 template <
class OtherDerived>
665 inline typename internal::traits<Derived>::Scalar
670 double d = abs(this->dot(other));
673 return static_cast<Scalar
>(2 * acos(d));
684 template <
class Derived>
685 template <
class OtherDerived>
693 Scalar d = this->dot(other);
694 Scalar absD = abs(d);
701 scale0 = Scalar(1) - t;
707 Scalar theta = acos(absD);
708 Scalar sinTheta = sin(theta);
710 scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;
711 scale1 = sin( ( t * theta) ) / sinTheta;
713 if(d<0) scale1 = -scale1;
721 template<
typename Other>
722 struct quaternionbase_assign_impl<Other,3,3>
724 typedef typename Other::Scalar Scalar;
725 typedef DenseIndex Index;
731 Scalar t = mat.trace();
734 t = sqrt(t + Scalar(1.0));
735 q.w() = Scalar(0.5)*t;
737 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
738 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
739 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
744 if (mat.coeff(1,1) > mat.coeff(0,0))
746 if (mat.coeff(2,2) > mat.coeff(i,i))
748 DenseIndex j = (i+1)%3;
749 DenseIndex k = (j+1)%3;
751 t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
752 q.coeffs().coeffRef(i) = Scalar(0.5) * t;
754 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
755 q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
756 q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
762 template<
typename Other>
763 struct quaternionbase_assign_impl<Other,4,1>
765 typedef typename Other::Scalar Scalar;
766 template<
class Derived>
static inline void run(QuaternionBase<Derived>& q,
const Other& vec)
776 #endif // EIGEN_QUATERNION_H
Map(const Scalar *coeffs)
Definition: Quaternion.h:351
Scalar x() const
Definition: Quaternion.h:60
Quaternion< float > Quaternionf
Definition: Quaternion.h:293
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:634
VectorBlock< Coefficients, 3 > vec()
Definition: Quaternion.h:81
Map(Scalar *coeffs)
Definition: Quaternion.h:388
Expression of a fixed-size or dynamic-size sub-vector.
Definition: ForwardDeclarations.h:83
Quaternion(const AngleAxisType &aa)
Definition: Quaternion.h:257
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:251
Quaternion< Scalar > conjugate() const
Definition: Quaternion.h:655
Definition: Constants.h:270
Quaternion< double > Quaterniond
Definition: Quaternion.h:296
Derived & setFromTwoVectors(const MatrixBase< Derived1 > &a, const MatrixBase< Derived2 > &b)
Definition: Quaternion.h:569
Quaternion< Scalar > normalized() const
Definition: Quaternion.h:126
internal::cast_return_type< Derived, Quaternion< NewScalarType > >::type cast() const
Definition: Quaternion.h:172
Quaternion(const Quaternion< OtherScalar, OtherOptions > &other)
Definition: Quaternion.h:268
Base class for quaternion expressions.
Definition: ForwardDeclarations.h:233
Matrix< Scalar, 3, 1 > Vector3
Definition: Quaternion.h:51
Quaternion(const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)
Definition: Quaternion.h:248
AngleAxis< Scalar > AngleAxisType
Definition: Quaternion.h:55
Scalar & x()
Definition: Quaternion.h:69
Matrix3 toRotationMatrix() const
Definition: Quaternion.h:523
const MatrixVType & matrixV() const
Definition: JacobiSVD.h:621
Scalar z() const
Definition: Quaternion.h:64
Map< Quaternion< double >, Aligned > QuaternionMapAlignedd
Definition: Quaternion.h:408
bool isApprox(const QuaternionBase< OtherDerived > &other, const RealScalar &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: Quaternion.h:160
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:260
Vector3 _transformVector(Vector3 v) const
Definition: Quaternion.h:462
Definition: Constants.h:194
Quaternion(const MatrixBase< Derived > &other)
Definition: Quaternion.h:264
Two-sided Jacobi SVD decomposition of a rectangular matrix.
Definition: ForwardDeclarations.h:224
Map< Quaternion< float >, Aligned > QuaternionMapAlignedf
Definition: Quaternion.h:405
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:399
Map< Quaternion< double >, 0 > QuaternionMapd
Definition: Quaternion.h:402
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:254
Derived & operator*=(const QuaternionBase< OtherDerived > &q)
Definition: Quaternion.h:447
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