Eigen  3.4.90 (git rev a4098ac676528a83cfb73d4d26ce1b42ec05f47c)
Quaternion.h
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
5// Copyright (C) 2009 Mathieu Gautier <mathieu.gautier@cea.fr>
6//
7// This Source Code Form is subject to the terms of the Mozilla
8// Public License v. 2.0. If a copy of the MPL was not distributed
9// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
10
11#ifndef EIGEN_QUATERNION_H
12#define EIGEN_QUATERNION_H
13#include "./InternalHeaderCheck.h"
14
15namespace Eigen {
16
17
18/***************************************************************************
19* Definition of QuaternionBase<Derived>
20* The implementation is at the end of the file
21***************************************************************************/
22
23namespace internal {
24template<typename Other,
25 int OtherRows=Other::RowsAtCompileTime,
26 int OtherCols=Other::ColsAtCompileTime>
27struct quaternionbase_assign_impl;
28}
29
36template<class Derived>
37class QuaternionBase : public RotationBase<Derived, 3>
38{
39 public:
41
42 using Base::operator*;
43 using Base::derived;
44
45 typedef typename internal::traits<Derived>::Scalar Scalar;
46 typedef typename NumTraits<Scalar>::Real RealScalar;
47 typedef typename internal::traits<Derived>::Coefficients Coefficients;
48 typedef typename Coefficients::CoeffReturnType CoeffReturnType;
49 typedef typename internal::conditional<bool(internal::traits<Derived>::Flags&LvalueBit),
50 Scalar&, CoeffReturnType>::type NonConstCoeffReturnType;
51
52
53 enum {
54 Flags = Eigen::internal::traits<Derived>::Flags
55 };
56
57 // typedef typename Matrix<Scalar,4,1> Coefficients;
64
65
66
68 EIGEN_DEVICE_FUNC inline CoeffReturnType x() const { return this->derived().coeffs().coeff(0); }
70 EIGEN_DEVICE_FUNC inline CoeffReturnType y() const { return this->derived().coeffs().coeff(1); }
72 EIGEN_DEVICE_FUNC inline CoeffReturnType z() const { return this->derived().coeffs().coeff(2); }
74 EIGEN_DEVICE_FUNC inline CoeffReturnType w() const { return this->derived().coeffs().coeff(3); }
75
77 EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType x() { return this->derived().coeffs().x(); }
79 EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType y() { return this->derived().coeffs().y(); }
81 EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType z() { return this->derived().coeffs().z(); }
83 EIGEN_DEVICE_FUNC inline NonConstCoeffReturnType w() { return this->derived().coeffs().w(); }
84
86 EIGEN_DEVICE_FUNC inline const VectorBlock<const Coefficients,3> vec() const { return coeffs().template head<3>(); }
87
89 EIGEN_DEVICE_FUNC inline VectorBlock<Coefficients,3> vec() { return coeffs().template head<3>(); }
90
92 EIGEN_DEVICE_FUNC inline const typename internal::traits<Derived>::Coefficients& coeffs() const { return derived().coeffs(); }
93
95 EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
96
97 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
98 template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
99
100// disabled this copy operator as it is giving very strange compilation errors when compiling
101// test_stdvector with GCC 4.4.2. This looks like a GCC bug though, so feel free to re-enable it if it's
102// useful; however notice that we already have the templated operator= above and e.g. in MatrixBase
103// we didn't have to add, in addition to templated operator=, such a non-templated copy operator.
104// Derived& operator=(const QuaternionBase& other)
105// { return operator=<Derived>(other); }
106
107 EIGEN_DEVICE_FUNC Derived& operator=(const AngleAxisType& aa);
108 template<class OtherDerived> EIGEN_DEVICE_FUNC Derived& operator=(const MatrixBase<OtherDerived>& m);
109
113 EIGEN_DEVICE_FUNC static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(Scalar(1), Scalar(0), Scalar(0), Scalar(0)); }
114
117 EIGEN_DEVICE_FUNC inline QuaternionBase& setIdentity() { coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1); return *this; }
118
122 EIGEN_DEVICE_FUNC inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
123
127 EIGEN_DEVICE_FUNC inline Scalar norm() const { return coeffs().norm(); }
128
131 EIGEN_DEVICE_FUNC inline void normalize() { coeffs().normalize(); }
134 EIGEN_DEVICE_FUNC inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
135
141 template<class OtherDerived> EIGEN_DEVICE_FUNC inline Scalar dot(const QuaternionBase<OtherDerived>& other) const { return coeffs().dot(other.coeffs()); }
142
143 template<class OtherDerived> EIGEN_DEVICE_FUNC Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
144
146 EIGEN_DEVICE_FUNC inline Matrix3 toRotationMatrix() const;
147
149 template<typename Derived1, typename Derived2>
150 EIGEN_DEVICE_FUNC Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
151
152 template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion<Scalar> operator* (const QuaternionBase<OtherDerived>& q) const;
153 template<class OtherDerived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*= (const QuaternionBase<OtherDerived>& q);
154
156 EIGEN_DEVICE_FUNC Quaternion<Scalar> inverse() const;
157
159 EIGEN_DEVICE_FUNC Quaternion<Scalar> conjugate() const;
160
161 template<class OtherDerived> EIGEN_DEVICE_FUNC Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const;
162
167 template<class OtherDerived>
168 EIGEN_DEVICE_FUNC inline bool operator==(const QuaternionBase<OtherDerived>& other) const
169 { return coeffs() == other.coeffs(); }
170
175 template<class OtherDerived>
176 EIGEN_DEVICE_FUNC inline bool operator!=(const QuaternionBase<OtherDerived>& other) const
177 { return coeffs() != other.coeffs(); }
178
183 template<class OtherDerived>
184 EIGEN_DEVICE_FUNC bool isApprox(const QuaternionBase<OtherDerived>& other, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const
185 { return coeffs().isApprox(other.coeffs(), prec); }
186
188 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const;
189
190 #ifdef EIGEN_PARSED_BY_DOXYGEN
196 template<typename NewScalarType>
197 EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Derived,Quaternion<NewScalarType> >::type cast() const;
198
199 #else
200
201 template<typename NewScalarType>
202 EIGEN_DEVICE_FUNC inline
203 typename internal::enable_if<internal::is_same<Scalar,NewScalarType>::value,const Derived&>::type cast() const
204 {
205 return derived();
206 }
207
208 template<typename NewScalarType>
209 EIGEN_DEVICE_FUNC inline
210 typename internal::enable_if<!internal::is_same<Scalar,NewScalarType>::value,Quaternion<NewScalarType> >::type cast() const
211 {
212 return Quaternion<NewScalarType>(coeffs().template cast<NewScalarType>());
213 }
214 #endif
215
216#ifndef EIGEN_NO_IO
217 friend std::ostream& operator<<(std::ostream& s, const QuaternionBase<Derived>& q) {
218 s << q.x() << "i + " << q.y() << "j + " << q.z() << "k" << " + " << q.w();
219 return s;
220 }
221#endif
222
223#ifdef EIGEN_QUATERNIONBASE_PLUGIN
224# include EIGEN_QUATERNIONBASE_PLUGIN
225#endif
226protected:
227 EIGEN_DEFAULT_COPY_CONSTRUCTOR(QuaternionBase)
228 EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(QuaternionBase)
229};
230
231/***************************************************************************
232* Definition/implementation of Quaternion<Scalar>
233***************************************************************************/
234
260namespace internal {
261template<typename Scalar_,int Options_>
262struct traits<Quaternion<Scalar_,Options_> >
263{
264 typedef Quaternion<Scalar_,Options_> PlainObject;
265 typedef Scalar_ Scalar;
266 typedef Matrix<Scalar_,4,1,Options_> Coefficients;
267 enum{
268 Alignment = internal::traits<Coefficients>::Alignment,
269 Flags = LvalueBit
270 };
271};
272}
273
274template<typename Scalar_, int Options_>
275class Quaternion : public QuaternionBase<Quaternion<Scalar_,Options_> >
276{
277public:
279 enum { NeedsAlignment = internal::traits<Quaternion>::Alignment>0 };
280
281 typedef Scalar_ Scalar;
282
283 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion)
284 using Base::operator*=;
285
286 typedef typename internal::traits<Quaternion>::Coefficients Coefficients;
287 typedef typename Base::AngleAxisType AngleAxisType;
288
290 EIGEN_DEVICE_FUNC inline Quaternion() {}
291
299 EIGEN_DEVICE_FUNC inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z) : m_coeffs(x, y, z, w){}
300
302 EIGEN_DEVICE_FUNC explicit inline Quaternion(const Scalar* data) : m_coeffs(data) {}
303
305 template<class Derived> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) { this->Base::operator=(other); }
306
308 EIGEN_DEVICE_FUNC explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
309
314 template<typename Derived>
315 EIGEN_DEVICE_FUNC explicit inline Quaternion(const MatrixBase<Derived>& other) { *this = other; }
316
318 template<typename OtherScalar, int OtherOptions>
319 EIGEN_DEVICE_FUNC explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
320 { m_coeffs = other.coeffs().template cast<Scalar>(); }
321
322 // We define a copy constructor, which means we don't get an implicit move constructor or assignment operator.
324 EIGEN_DEVICE_FUNC inline Quaternion(Quaternion&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<Scalar>::value)
325 : m_coeffs(std::move(other.coeffs()))
326 {}
327
329 EIGEN_DEVICE_FUNC Quaternion& operator=(Quaternion&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value)
330 {
331 m_coeffs = std::move(other.coeffs());
332 return *this;
333 }
334
335 EIGEN_DEVICE_FUNC static Quaternion UnitRandom();
336
337 template<typename Derived1, typename Derived2>
338 EIGEN_DEVICE_FUNC static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
339
340 EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs;}
341 EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs;}
342
343 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(NeedsAlignment))
344
345#ifdef EIGEN_QUATERNION_PLUGIN
346# include EIGEN_QUATERNION_PLUGIN
347#endif
348
349protected:
350 Coefficients m_coeffs;
351
352#ifndef EIGEN_PARSED_BY_DOXYGEN
353 EIGEN_STATIC_ASSERT( (Options_ & DontAlign) == Options_,
354 INVALID_MATRIX_TEMPLATE_PARAMETERS)
355#endif
356};
357
364
365/***************************************************************************
366* Specialization of Map<Quaternion<Scalar>>
367***************************************************************************/
368
369namespace internal {
370 template<typename Scalar_, int Options_>
371 struct traits<Map<Quaternion<Scalar_>, Options_> > : traits<Quaternion<Scalar_, (int(Options_)&Aligned)==Aligned ? AutoAlign : DontAlign> >
372 {
373 typedef Map<Matrix<Scalar_,4,1>, Options_> Coefficients;
374 };
375}
376
377namespace internal {
378 template<typename Scalar_, int Options_>
379 struct traits<Map<const Quaternion<Scalar_>, Options_> > : traits<Quaternion<Scalar_, (int(Options_)&Aligned)==Aligned ? AutoAlign : DontAlign> >
380 {
381 typedef Map<const Matrix<Scalar_,4,1>, Options_> Coefficients;
382 typedef traits<Quaternion<Scalar_, (int(Options_)&Aligned)==Aligned ? AutoAlign : DontAlign> > TraitsBase;
383 enum {
384 Flags = TraitsBase::Flags & ~LvalueBit
385 };
386 };
387}
388
400template<typename Scalar_, int Options_>
401class Map<const Quaternion<Scalar_>, Options_ >
402 : public QuaternionBase<Map<const Quaternion<Scalar_>, Options_> >
403{
404 public:
406
407 typedef Scalar_ Scalar;
408 typedef typename internal::traits<Map>::Coefficients Coefficients;
409 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
410 using Base::operator*=;
411
418 EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
419
420 EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs;}
421
422 protected:
423 const Coefficients m_coeffs;
424};
425
437template<typename Scalar_, int Options_>
438class Map<Quaternion<Scalar_>, Options_ >
439 : public QuaternionBase<Map<Quaternion<Scalar_>, Options_> >
440{
441 public:
442 typedef QuaternionBase<Map<Quaternion<Scalar_>, Options_> > Base;
443
444 typedef Scalar_ Scalar;
445 typedef typename internal::traits<Map>::Coefficients Coefficients;
446 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
447 using Base::operator*=;
448
455 EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
456
457 EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; }
458 EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; }
459
460 protected:
461 Coefficients m_coeffs;
462};
463
476
477/***************************************************************************
478* Implementation of QuaternionBase methods
479***************************************************************************/
480
481// Generic Quaternion * Quaternion product
482// This product can be specialized for a given architecture via the Arch template argument.
483namespace internal {
484template<int Arch, class Derived1, class Derived2, typename Scalar> struct quat_product
485{
486 EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& a, const QuaternionBase<Derived2>& b){
487 return Quaternion<Scalar>
488 (
489 a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
490 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
491 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
492 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()
493 );
494 }
495};
496}
497
499template <class Derived>
500template <class OtherDerived>
501EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
503{
504 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
505 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
506 return internal::quat_product<Architecture::Target, Derived, OtherDerived,
507 typename internal::traits<Derived>::Scalar>::run(*this, other);
508}
509
511template <class Derived>
512template <class OtherDerived>
513EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*= (const QuaternionBase<OtherDerived>& other)
514{
515 derived() = derived() * other.derived();
516 return derived();
517}
518
526template <class Derived>
527EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
529{
530 // Note that this algorithm comes from the optimization by hand
531 // of the conversion to a Matrix followed by a Matrix/Vector product.
532 // It appears to be much faster than the common algorithm found
533 // in the literature (30 versus 39 flops). It also requires two
534 // Vector3 as temporaries.
535 Vector3 uv = this->vec().cross(v);
536 uv += uv;
537 return v + this->w() * uv + this->vec().cross(uv);
538}
539
540template<class Derived>
541EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(const QuaternionBase<Derived>& other)
542{
543 coeffs() = other.coeffs();
544 return derived();
545}
546
547template<class Derived>
548template<class OtherDerived>
549EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const QuaternionBase<OtherDerived>& other)
550{
551 coeffs() = other.coeffs();
552 return derived();
553}
554
557template<class Derived>
558EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa)
559{
560 EIGEN_USING_STD(cos)
561 EIGEN_USING_STD(sin)
562 Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision loss warnings
563 this->w() = cos(ha);
564 this->vec() = sin(ha) * aa.axis();
565 return derived();
566}
567
574template<class Derived>
575template<class MatrixDerived>
576EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr)
577{
578 EIGEN_STATIC_ASSERT((internal::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
579 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
580 internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
581 return derived();
582}
583
587template<class Derived>
588EIGEN_DEVICE_FUNC inline typename QuaternionBase<Derived>::Matrix3
590{
591 // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc 4.3 !!)
592 // if not inlined then the cost of the return by value is huge ~ +35%,
593 // however, not inlining this function is an order of magnitude slower, so
594 // it has to be inlined, and so the return by value is not an issue
595 Matrix3 res;
596
597 const Scalar tx = Scalar(2)*this->x();
598 const Scalar ty = Scalar(2)*this->y();
599 const Scalar tz = Scalar(2)*this->z();
600 const Scalar twx = tx*this->w();
601 const Scalar twy = ty*this->w();
602 const Scalar twz = tz*this->w();
603 const Scalar txx = tx*this->x();
604 const Scalar txy = ty*this->x();
605 const Scalar txz = tz*this->x();
606 const Scalar tyy = ty*this->y();
607 const Scalar tyz = tz*this->y();
608 const Scalar tzz = tz*this->z();
609
610 res.coeffRef(0,0) = Scalar(1)-(tyy+tzz);
611 res.coeffRef(0,1) = txy-twz;
612 res.coeffRef(0,2) = txz+twy;
613 res.coeffRef(1,0) = txy+twz;
614 res.coeffRef(1,1) = Scalar(1)-(txx+tzz);
615 res.coeffRef(1,2) = tyz-twx;
616 res.coeffRef(2,0) = txz-twy;
617 res.coeffRef(2,1) = tyz+twx;
618 res.coeffRef(2,2) = Scalar(1)-(txx+tyy);
619
620 return res;
621}
622
633template<class Derived>
634template<typename Derived1, typename Derived2>
636{
637 EIGEN_USING_STD(sqrt)
638 Vector3 v0 = a.normalized();
639 Vector3 v1 = b.normalized();
640 Scalar c = v1.dot(v0);
641
642 // if dot == -1, vectors are nearly opposites
643 // => accurately compute the rotation axis by computing the
644 // intersection of the two planes. This is done by solving:
645 // x^T v0 = 0
646 // x^T v1 = 0
647 // under the constraint:
648 // ||x|| = 1
649 // which yields a singular value problem
650 if (c < Scalar(-1)+NumTraits<Scalar>::dummy_precision())
651 {
652 c = numext::maxi(c,Scalar(-1));
653 Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
655 Vector3 axis = svd.matrixV().col(2);
656
657 Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
658 this->w() = sqrt(w2);
659 this->vec() = axis * sqrt(Scalar(1) - w2);
660 return derived();
661 }
662 Vector3 axis = v0.cross(v1);
663 Scalar s = sqrt((Scalar(1)+c)*Scalar(2));
664 Scalar invs = Scalar(1)/s;
665 this->vec() = axis * invs;
666 this->w() = s * Scalar(0.5);
667
668 return derived();
669}
670
675template<typename Scalar, int Options>
677{
678 EIGEN_USING_STD(sqrt)
679 EIGEN_USING_STD(sin)
680 EIGEN_USING_STD(cos)
681 const Scalar u1 = internal::random<Scalar>(0, 1),
682 u2 = internal::random<Scalar>(0, 2*EIGEN_PI),
683 u3 = internal::random<Scalar>(0, 2*EIGEN_PI);
684 const Scalar a = sqrt(Scalar(1) - u1),
685 b = sqrt(u1);
686 return Quaternion (a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3));
687}
688
689
700template<typename Scalar, int Options>
701template<typename Derived1, typename Derived2>
703{
704 Quaternion quat;
705 quat.setFromTwoVectors(a, b);
706 return quat;
707}
708
709
716template <class Derived>
718{
719 // FIXME should this function be called multiplicativeInverse and conjugate() be called inverse() or opposite() ??
720 Scalar n2 = this->squaredNorm();
721 if (n2 > Scalar(0))
722 return Quaternion<Scalar>(conjugate().coeffs() / n2);
723 else
724 {
725 // return an invalid result to flag the error
726 return Quaternion<Scalar>(Coefficients::Zero());
727 }
728}
729
730// Generic conjugate of a Quaternion
731namespace internal {
732template<int Arch, class Derived, typename Scalar> struct quat_conj
733{
734 EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived>& q){
735 return Quaternion<Scalar>(q.w(),-q.x(),-q.y(),-q.z());
736 }
737};
738}
739
746template <class Derived>
747EIGEN_DEVICE_FUNC inline Quaternion<typename internal::traits<Derived>::Scalar>
749{
750 return internal::quat_conj<Architecture::Target, Derived,
751 typename internal::traits<Derived>::Scalar>::run(*this);
752
753}
754
758template <class Derived>
759template <class OtherDerived>
760EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar
762{
763 EIGEN_USING_STD(atan2)
764 Quaternion<Scalar> d = (*this) * other.conjugate();
765 return Scalar(2) * atan2( d.vec().norm(), numext::abs(d.w()) );
766}
767
768
769
776template <class Derived>
777template <class OtherDerived>
780{
781 EIGEN_USING_STD(acos)
782 EIGEN_USING_STD(sin)
783 const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
784 Scalar d = this->dot(other);
785 Scalar absD = numext::abs(d);
786
787 Scalar scale0;
788 Scalar scale1;
789
790 if(absD>=one)
791 {
792 scale0 = Scalar(1) - t;
793 scale1 = t;
794 }
795 else
796 {
797 // theta is the angle between the 2 quaternions
798 Scalar theta = acos(absD);
799 Scalar sinTheta = sin(theta);
800
801 scale0 = sin( ( Scalar(1) - t ) * theta) / sinTheta;
802 scale1 = sin( ( t * theta) ) / sinTheta;
803 }
804 if(d<Scalar(0)) scale1 = -scale1;
805
806 return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
807}
808
809namespace internal {
810
811// set from a rotation matrix
812template<typename Other>
813struct quaternionbase_assign_impl<Other,3,3>
814{
815 typedef typename Other::Scalar Scalar;
816 template<class Derived> EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& a_mat)
817 {
818 const typename internal::nested_eval<Other,2>::type mat(a_mat);
819 EIGEN_USING_STD(sqrt)
820 // This algorithm comes from "Quaternion Calculus and Fast Animation",
821 // Ken Shoemake, 1987 SIGGRAPH course notes
822 Scalar t = mat.trace();
823 if (t > Scalar(0))
824 {
825 t = sqrt(t + Scalar(1.0));
826 q.w() = Scalar(0.5)*t;
827 t = Scalar(0.5)/t;
828 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
829 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
830 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
831 }
832 else
833 {
834 Index i = 0;
835 if (mat.coeff(1,1) > mat.coeff(0,0))
836 i = 1;
837 if (mat.coeff(2,2) > mat.coeff(i,i))
838 i = 2;
839 Index j = (i+1)%3;
840 Index k = (j+1)%3;
841
842 t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
843 q.coeffs().coeffRef(i) = Scalar(0.5) * t;
844 t = Scalar(0.5)/t;
845 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
846 q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
847 q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
848 }
849 }
850};
851
852// set from a vector of coefficients assumed to be a quaternion
853template<typename Other>
854struct quaternionbase_assign_impl<Other,4,1>
855{
856 typedef typename Other::Scalar Scalar;
857 template<class Derived> EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& vec)
858 {
859 q.coeffs() = vec;
860 }
861};
862
863} // end namespace internal
864
865} // end namespace Eigen
866
867#endif // EIGEN_QUATERNION_H
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition: AngleAxis.h:52
const Vector3 & axis() const
Definition: AngleAxis.h:98
Scalar angle() const
Definition: AngleAxis.h:93
Derived & derived()
Definition: EigenBase.h:48
Two-sided Jacobi SVD decomposition of a rectangular matrix.
Definition: JacobiSVD.h:492
Map(Scalar *coeffs)
Definition: Quaternion.h:455
Map(const Scalar *coeffs)
Definition: Quaternion.h:418
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:98
Base class for all dense matrices, vectors, and expressions.
Definition: MatrixBase.h:52
const PlainObject normalized() const
Definition: Dot.h:126
Scalar & coeffRef(Index rowId, Index colId)
Definition: PlainObjectBase.h:187
Base class for quaternion expressions.
Definition: Quaternion.h:38
Scalar squaredNorm() const
Definition: Quaternion.h:122
QuaternionBase & setIdentity()
Definition: Quaternion.h:117
Quaternion< Scalar > normalized() const
Definition: Quaternion.h:134
internal::traits< Derived >::Coefficients & coeffs()
Definition: Quaternion.h:95
internal::cast_return_type< Derived, Quaternion< NewScalarType > >::type cast() const
VectorBlock< Coefficients, 3 > vec()
Definition: Quaternion.h:89
const VectorBlock< const Coefficients, 3 > vec() const
Definition: Quaternion.h:86
static Quaternion< Scalar > Identity()
Definition: Quaternion.h:113
bool operator!=(const QuaternionBase< OtherDerived > &other) const
Definition: Quaternion.h:176
Quaternion< Scalar > conjugate() const
Definition: Quaternion.h:748
NonConstCoeffReturnType y()
Definition: Quaternion.h:79
bool isApprox(const QuaternionBase< OtherDerived > &other, const RealScalar &prec=NumTraits< Scalar >::dummy_precision()) const
Definition: Quaternion.h:184
void normalize()
Definition: Quaternion.h:131
Derived & setFromTwoVectors(const MatrixBase< Derived1 > &a, const MatrixBase< Derived2 > &b)
Definition: Quaternion.h:635
CoeffReturnType z() const
Definition: Quaternion.h:72
NonConstCoeffReturnType x()
Definition: Quaternion.h:77
Matrix3 toRotationMatrix() const
Definition: Quaternion.h:589
Matrix< Scalar, 3, 1 > Vector3
Definition: Quaternion.h:59
NonConstCoeffReturnType z()
Definition: Quaternion.h:81
Scalar dot(const QuaternionBase< OtherDerived > &other) const
Definition: Quaternion.h:141
Derived & operator=(const AngleAxisType &aa)
Definition: Quaternion.h:558
Vector3 _transformVector(const Vector3 &v) const
Definition: Quaternion.h:528
CoeffReturnType y() const
Definition: Quaternion.h:70
Scalar norm() const
Definition: Quaternion.h:127
Quaternion< Scalar > inverse() const
Definition: Quaternion.h:717
CoeffReturnType w() const
Definition: Quaternion.h:74
Matrix< Scalar, 3, 3 > Matrix3
Definition: Quaternion.h:61
const internal::traits< Derived >::Coefficients & coeffs() const
Definition: Quaternion.h:92
bool operator==(const QuaternionBase< OtherDerived > &other) const
Definition: Quaternion.h:168
NonConstCoeffReturnType w()
Definition: Quaternion.h:83
AngleAxis< Scalar > AngleAxisType
Definition: Quaternion.h:63
Derived & operator*=(const QuaternionBase< OtherDerived > &q)
Definition: Quaternion.h:513
CoeffReturnType x() const
Definition: Quaternion.h:68
The quaternion class used to represent 3D orientations and rotations.
Definition: Quaternion.h:276
Quaternion(const Quaternion< OtherScalar, OtherOptions > &other)
Definition: Quaternion.h:319
Quaternion(const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)
Definition: Quaternion.h:299
Quaternion(const QuaternionBase< Derived > &other)
Definition: Quaternion.h:305
static Quaternion UnitRandom()
Definition: Quaternion.h:676
Quaternion(const MatrixBase< Derived > &other)
Definition: Quaternion.h:315
Quaternion(const AngleAxisType &aa)
Definition: Quaternion.h:308
Quaternion()
Definition: Quaternion.h:290
Quaternion & operator=(Quaternion &&other) EIGEN_NOEXCEPT_IF(std
Definition: Quaternion.h:329
Quaternion(Quaternion &&other) EIGEN_NOEXCEPT_IF(std
Definition: Quaternion.h:324
Quaternion(const Scalar *data)
Definition: Quaternion.h:302
Common base class for compact rotation representations.
Definition: RotationBase.h:32
internal::traits< Derived >::Scalar Scalar
Definition: RotationBase.h:36
const MatrixVType & matrixV() const
Definition: SVDBase.h:119
Expression of a fixed-size or dynamic-size sub-vector.
Definition: VectorBlock.h:62
Map< Quaternion< double >, Aligned > QuaternionMapAlignedd
Definition: Quaternion.h:475
Quaternion< double > Quaterniond
Definition: Quaternion.h:363
Quaternion< float > Quaternionf
Definition: Quaternion.h:360
Map< Quaternion< float >, 0 > QuaternionMapf
Definition: Quaternion.h:466
Map< Quaternion< double >, 0 > QuaternionMapd
Definition: Quaternion.h:469
Map< Quaternion< float >, Aligned > QuaternionMapAlignedf
Definition: Quaternion.h:472
@ Aligned
Definition: Constants.h:242
@ DontAlign
Definition: Constants.h:327
@ AutoAlign
Definition: Constants.h:325
@ ComputeFullV
Definition: Constants.h:399
const unsigned int LvalueBit
Definition: Constants.h:146
Namespace containing all symbols from the Eigen library.
Definition: B01_Experimental.dox:1
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_cos_op< typename Derived::Scalar >, const Derived > cos(const Eigen::ArrayBase< Derived > &x)
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sqrt_op< typename Derived::Scalar >, const Derived > sqrt(const Eigen::ArrayBase< Derived > &x)
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:59
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sin_op< typename Derived::Scalar >, const Derived > sin(const Eigen::ArrayBase< Derived > &x)
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_acos_op< typename Derived::Scalar >, const Derived > acos(const Eigen::ArrayBase< Derived > &x)
Holds information about the various numeric (i.e. scalar) types allowed by Eigen.
Definition: NumTraits.h:235