Eigen  5.0.1-dev+master
 
Loading...
Searching...
No Matches
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// SPDX-License-Identifier: MPL-2.0
11
12#ifndef EIGEN_QUATERNION_H
13#define EIGEN_QUATERNION_H
14// IWYU pragma: private
15#include "./InternalHeaderCheck.h"
16
17namespace Eigen {
18
19/***************************************************************************
20 * Definition of QuaternionBase<Derived>
21 * The implementation is at the end of the file
22 ***************************************************************************/
23
24namespace internal {
25template <typename Other, int OtherRows = Other::RowsAtCompileTime, int OtherCols = Other::ColsAtCompileTime>
26struct quaternionbase_assign_impl;
27}
28
35template <class Derived>
36class QuaternionBase : public RotationBase<Derived, 3> {
37 public:
38 using Base = RotationBase<Derived, 3>;
39
40 using Base::operator*;
41 using Base::derived;
42
43 using Scalar = typename internal::traits<Derived>::Scalar;
44 using RealScalar = typename NumTraits<Scalar>::Real;
45 using Coefficients = typename internal::traits<Derived>::Coefficients;
46 using CoeffReturnType = typename Coefficients::CoeffReturnType;
47 using NonConstCoeffReturnType =
48 std::conditional_t<bool(internal::traits<Derived>::Flags& LvalueBit), Scalar&, CoeffReturnType>;
49
50 enum { Flags = Eigen::internal::traits<Derived>::Flags };
51
58
60 EIGEN_DEVICE_FUNC constexpr CoeffReturnType x() const { return this->derived().coeffs().coeff(0); }
62 EIGEN_DEVICE_FUNC constexpr CoeffReturnType y() const { return this->derived().coeffs().coeff(1); }
64 EIGEN_DEVICE_FUNC constexpr CoeffReturnType z() const { return this->derived().coeffs().coeff(2); }
66 EIGEN_DEVICE_FUNC constexpr CoeffReturnType w() const { return this->derived().coeffs().coeff(3); }
67
69 EIGEN_DEVICE_FUNC constexpr NonConstCoeffReturnType x() { return this->derived().coeffs().x(); }
71 EIGEN_DEVICE_FUNC constexpr NonConstCoeffReturnType y() { return this->derived().coeffs().y(); }
73 EIGEN_DEVICE_FUNC constexpr NonConstCoeffReturnType z() { return this->derived().coeffs().z(); }
75 EIGEN_DEVICE_FUNC constexpr NonConstCoeffReturnType w() { return this->derived().coeffs().w(); }
76
78 EIGEN_DEVICE_FUNC inline const VectorBlock<const Coefficients, 3> vec() const { return coeffs().template head<3>(); }
79
81 EIGEN_DEVICE_FUNC inline VectorBlock<Coefficients, 3> vec() { return coeffs().template head<3>(); }
82
84 EIGEN_DEVICE_FUNC inline const typename internal::traits<Derived>::Coefficients& coeffs() const {
85 return derived().coeffs();
86 }
87
96 EIGEN_DEVICE_FUNC inline Matrix<Scalar, 4, 1> coeffsScalarFirst() const { return derived().coeffsScalarFirst(); }
97
105 EIGEN_DEVICE_FUNC inline Matrix<Scalar, 4, 1> coeffsScalarLast() const { return derived().coeffsScalarLast(); }
106
108 EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Coefficients& coeffs() { return derived().coeffs(); }
109
110 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& operator=(const QuaternionBase<Derived>& other);
111 template <class OtherDerived>
112 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator=(const QuaternionBase<OtherDerived>& other);
113
114 EIGEN_DEVICE_FUNC Derived& operator=(const AngleAxisType& aa);
115 template <class OtherDerived>
116 EIGEN_DEVICE_FUNC Derived& operator=(const MatrixBase<OtherDerived>& m);
117
121 EIGEN_DEVICE_FUNC static inline Quaternion<Scalar> Identity() {
122 return Quaternion<Scalar>(Scalar(1), Scalar(0), Scalar(0), Scalar(0));
123 }
124
127 EIGEN_DEVICE_FUNC inline QuaternionBase& setIdentity() {
128 coeffs() << Scalar(0), Scalar(0), Scalar(0), Scalar(1);
129 return *this;
130 }
131
135 EIGEN_DEVICE_FUNC inline Scalar squaredNorm() const { return coeffs().squaredNorm(); }
136
140 EIGEN_DEVICE_FUNC inline Scalar norm() const { return coeffs().norm(); }
141
144 EIGEN_DEVICE_FUNC inline void normalize() { coeffs().normalize(); }
147 EIGEN_DEVICE_FUNC inline Quaternion<Scalar> normalized() const { return Quaternion<Scalar>(coeffs().normalized()); }
148
154 template <class OtherDerived>
155 EIGEN_DEVICE_FUNC inline Scalar dot(const QuaternionBase<OtherDerived>& other) const {
156 return coeffs().dot(other.coeffs());
157 }
158
159 template <class OtherDerived>
160 EIGEN_DEVICE_FUNC Scalar angularDistance(const QuaternionBase<OtherDerived>& other) const;
161
163 EIGEN_DEVICE_FUNC inline Matrix3 toRotationMatrix() const;
164
166 template <typename Derived1, typename Derived2>
167 EIGEN_DEVICE_FUNC Derived& setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
168
169 template <typename OtherDerived>
170 EIGEN_DEVICE_FUNC Derived& setFromScaledAxis(const MatrixBase<OtherDerived>& scaled_axis);
171
172 EIGEN_DEVICE_FUNC inline Vector3 toScaledAxis() const;
173
174 template <class OtherDerived>
175 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion<Scalar> operator*(const QuaternionBase<OtherDerived>& q) const;
176 template <class OtherDerived>
177 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& operator*=(const QuaternionBase<OtherDerived>& q);
178
180 EIGEN_DEVICE_FUNC Quaternion<Scalar> inverse() const;
181
183 EIGEN_DEVICE_FUNC Quaternion<Scalar> conjugate() const;
184
185 template <class OtherDerived>
186 EIGEN_DEVICE_FUNC Quaternion<Scalar> slerp(const Scalar& t, const QuaternionBase<OtherDerived>& other) const;
187
192 template <class OtherDerived>
193 EIGEN_DEVICE_FUNC inline bool operator==(const QuaternionBase<OtherDerived>& other) const {
194 return coeffs() == other.coeffs();
195 }
196
201 template <class OtherDerived>
202 EIGEN_DEVICE_FUNC inline bool operator!=(const QuaternionBase<OtherDerived>& other) const {
203 return coeffs() != other.coeffs();
204 }
205
210 template <class OtherDerived>
211 EIGEN_DEVICE_FUNC bool isApprox(const QuaternionBase<OtherDerived>& other,
212 const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const {
213 return coeffs().isApprox(other.coeffs(), prec);
214 }
215
217 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const Vector3& v) const;
218
219#ifdef EIGEN_PARSED_BY_DOXYGEN
225 template <typename NewScalarType>
226 EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<Derived, Quaternion<NewScalarType> >::type cast() const;
227
228#else
229
230 template <typename NewScalarType>
231 EIGEN_DEVICE_FUNC inline std::enable_if_t<std::is_same<Scalar, NewScalarType>::value, const Derived&> cast() const {
232 return derived();
233 }
234
235 template <typename NewScalarType>
236 EIGEN_DEVICE_FUNC inline std::enable_if_t<!std::is_same<Scalar, NewScalarType>::value, Quaternion<NewScalarType> >
237 cast() const {
238 return Quaternion<NewScalarType>(coeffs().template cast<NewScalarType>());
239 }
240#endif
241
242#ifndef EIGEN_NO_IO
243 friend std::ostream& operator<<(std::ostream& s, const QuaternionBase<Derived>& q) {
244 s << q.x() << "i + " << q.y() << "j + " << q.z() << "k"
245 << " + " << q.w();
246 return s;
247 }
248#endif
249
250#ifdef EIGEN_QUATERNIONBASE_PLUGIN
251#include EIGEN_QUATERNIONBASE_PLUGIN
252#endif
253 protected:
254 EIGEN_DEFAULT_COPY_CONSTRUCTOR(QuaternionBase)
255 EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(QuaternionBase)
256};
257
258/***************************************************************************
259 * Definition/implementation of Quaternion<Scalar>
260 ***************************************************************************/
261
288
289namespace internal {
290template <typename Scalar_, int Options_>
291struct traits<Quaternion<Scalar_, Options_> > {
292 using PlainObject = Quaternion<Scalar_, Options_>;
293 using Scalar = Scalar_;
294 using Coefficients = Matrix<Scalar_, 4, 1, Options_>;
295 enum { Alignment = internal::traits<Coefficients>::Alignment, Flags = LvalueBit };
296};
297} // namespace internal
298
299template <typename Scalar_, int Options_>
300class Quaternion : public QuaternionBase<Quaternion<Scalar_, Options_> > {
301 public:
303 enum { NeedsAlignment = internal::traits<Quaternion>::Alignment > 0 };
304
305 using Scalar = Scalar_;
306
307 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Quaternion)
308 using Base::operator*=;
309
310 using Coefficients = typename internal::traits<Quaternion>::Coefficients;
311 using AngleAxisType = typename Base::AngleAxisType;
312
314 EIGEN_DEVICE_FUNC inline Quaternion() {}
315
323 EIGEN_DEVICE_FUNC inline Quaternion(const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z)
324 : m_coeffs(x, y, z, w) {}
325
329 template <typename Derived>
330 EIGEN_DEVICE_FUNC inline Quaternion(const Scalar& w, const Eigen::MatrixBase<Derived>& vec)
331 : m_coeffs(vec.x(), vec.y(), vec.z(), w) {
332 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
333 }
334
336 EIGEN_DEVICE_FUNC explicit inline Quaternion(const Scalar* data) : m_coeffs(data) {}
337
339 template <class Derived>
340 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion(const QuaternionBase<Derived>& other) {
341 this->Base::operator=(other);
342 }
343
345 EIGEN_DEVICE_FUNC explicit inline Quaternion(const AngleAxisType& aa) { *this = aa; }
346
351 template <typename Derived>
352 EIGEN_DEVICE_FUNC explicit inline Quaternion(const MatrixBase<Derived>& other) {
353 *this = other;
354 }
355
357 template <typename OtherScalar, int OtherOptions>
358 EIGEN_DEVICE_FUNC explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other) {
359 m_coeffs = other.coeffs().template cast<Scalar>();
360 }
361
362 // We define a copy constructor, which means we don't get an implicit move constructor or assignment operator.
364 EIGEN_DEVICE_FUNC inline Quaternion(Quaternion&& other) noexcept(std::is_nothrow_move_constructible<Scalar>::value)
365 : m_coeffs(std::move(other.coeffs())) {}
366
368 EIGEN_DEVICE_FUNC Quaternion& operator=(Quaternion&& other) noexcept(std::is_nothrow_move_assignable<Scalar>::value) {
369 m_coeffs = std::move(other.coeffs());
370 return *this;
371 }
372
373 EIGEN_DEVICE_FUNC static Quaternion UnitRandom();
374
375 EIGEN_DEVICE_FUNC static Quaternion FromCoeffsScalarLast(const Scalar& x, const Scalar& y, const Scalar& z,
376 const Scalar& w);
377
378 EIGEN_DEVICE_FUNC static Quaternion FromCoeffsScalarFirst(const Scalar& w, const Scalar& x, const Scalar& y,
379 const Scalar& z);
380
381 template <typename Derived1, typename Derived2>
382 EIGEN_DEVICE_FUNC static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
383
384 template <typename Derived>
385 EIGEN_DEVICE_FUNC static Quaternion FromScaledAxis(const MatrixBase<Derived>& scaled_axis);
386
387 EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; }
388 EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; }
389
390 EIGEN_DEVICE_FUNC inline Coefficients coeffsScalarLast() const { return m_coeffs; }
391
392 EIGEN_DEVICE_FUNC inline Coefficients coeffsScalarFirst() const {
393 return {m_coeffs.w(), m_coeffs.x(), m_coeffs.y(), m_coeffs.z()};
394 }
395 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(NeedsAlignment))
396
397#ifdef EIGEN_QUATERNION_PLUGIN
398#include EIGEN_QUATERNION_PLUGIN
399#endif
400
401 protected:
402 Coefficients m_coeffs;
403
404#ifndef EIGEN_PARSED_BY_DOXYGEN
405 EIGEN_STATIC_ASSERT((Options_ & DontAlign) == Options_, INVALID_MATRIX_TEMPLATE_PARAMETERS)
406#endif
407};
408
415
416/***************************************************************************
417 * Specialization of Map<Quaternion<Scalar>>
418 ***************************************************************************/
419
420namespace internal {
421template <typename Scalar_, int Options_>
422struct traits<Map<Quaternion<Scalar_>, Options_> >
423 : traits<Quaternion<Scalar_, (int(Options_) & Aligned) == Aligned ? AutoAlign : DontAlign> > {
424 using Coefficients = Map<Matrix<Scalar_, 4, 1>, Options_>;
425};
426} // namespace internal
427
428namespace internal {
429template <typename Scalar_, int Options_>
430struct traits<Map<const Quaternion<Scalar_>, Options_> >
431 : traits<Quaternion<Scalar_, (int(Options_) & Aligned) == Aligned ? AutoAlign : DontAlign> > {
432 using Coefficients = Map<const Matrix<Scalar_, 4, 1>, Options_>;
433 using TraitsBase = traits<Quaternion<Scalar_, (int(Options_) & Aligned) == Aligned ? AutoAlign : DontAlign>>;
434 enum { Flags = TraitsBase::Flags & ~LvalueBit };
435};
436} // namespace internal
437
449template <typename Scalar_, int Options_>
450class Map<const Quaternion<Scalar_>, Options_> : public QuaternionBase<Map<const Quaternion<Scalar_>, Options_> > {
451 public:
452 using Base = QuaternionBase<Map<const Quaternion<Scalar_>, Options_>>;
453
454 using Scalar = Scalar_;
455 using Coefficients = typename internal::traits<Map>::Coefficients;
456 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
457 using Base::operator*=;
458
465 EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(const Scalar* coeffs) : m_coeffs(coeffs) {}
466
467 EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; }
468
469 EIGEN_DEVICE_FUNC inline Matrix<Scalar, 4, 1> coeffsScalarLast() const { return m_coeffs; }
470
471 EIGEN_DEVICE_FUNC inline Matrix<Scalar, 4, 1> coeffsScalarFirst() const {
472 return {m_coeffs.w(), m_coeffs.x(), m_coeffs.y(), m_coeffs.z()};
473 }
474
475 protected:
476 const Coefficients m_coeffs;
477};
478
490template <typename Scalar_, int Options_>
491class Map<Quaternion<Scalar_>, Options_> : public QuaternionBase<Map<Quaternion<Scalar_>, Options_> > {
492 public:
493 using Base = QuaternionBase<Map<Quaternion<Scalar_>, Options_>>;
494
495 using Scalar = Scalar_;
496 using Coefficients = typename internal::traits<Map>::Coefficients;
497 EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Map)
498 using Base::operator*=;
499
506 EIGEN_DEVICE_FUNC explicit EIGEN_STRONG_INLINE Map(Scalar* coeffs) : m_coeffs(coeffs) {}
507
508 EIGEN_DEVICE_FUNC inline Coefficients& coeffs() { return m_coeffs; }
509 EIGEN_DEVICE_FUNC inline const Coefficients& coeffs() const { return m_coeffs; }
510
511 EIGEN_DEVICE_FUNC inline Matrix<Scalar, 4, 1> coeffsScalarLast() const { return m_coeffs; }
512
513 EIGEN_DEVICE_FUNC inline Matrix<Scalar, 4, 1> coeffsScalarFirst() const {
514 return {m_coeffs.w(), m_coeffs.x(), m_coeffs.y(), m_coeffs.z()};
515 }
516
517 protected:
518 Coefficients m_coeffs;
519};
520
533
534/***************************************************************************
535 * Implementation of QuaternionBase methods
536 ***************************************************************************/
537
538// Generic Quaternion * Quaternion product
539// This product can be specialized for a given architecture via the Arch template argument.
540namespace internal {
541template <int Arch, class Derived1, class Derived2, typename Scalar>
542struct quat_product {
543 EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived1>& input_a,
544 const QuaternionBase<Derived2>& input_b) {
545 const Derived1* a_ptr = &input_a.derived();
546 const Derived2* b_ptr = &input_b.derived();
547#if EIGEN_ARCH_ARM && EIGEN_COMP_CLANG
548 EIGEN_IF_CONSTEXPR (std::is_same<Scalar, double>::value) {
549 // Keep operand addresses opaque to LLVM's ARM load/store optimizer (llvm-project#223630).
550 // Fixed upstream for Clang 24 by https://github.com/llvm/llvm-project/pull/226937.
551 // Register constraints avoid the spills introduced by the general optimization barrier.
552 asm("" : "+r"(a_ptr), "+r"(b_ptr));
553 }
554#endif
555 const Derived1& a = *a_ptr;
556 const Derived2& b = *b_ptr;
557 return Quaternion<Scalar>(a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
558 a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
559 a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
560 a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x());
561 }
562};
563} // namespace internal
564
566template <class Derived>
567template <class OtherDerived>
568EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Quaternion<typename internal::traits<Derived>::Scalar>
569QuaternionBase<Derived>::operator*(const QuaternionBase<OtherDerived>& other) const {
570 EIGEN_STATIC_ASSERT(
571 (std::is_same<typename Derived::Scalar, typename OtherDerived::Scalar>::value),
572 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
573 return internal::quat_product<Architecture::Target, Derived, OtherDerived,
574 typename internal::traits<Derived>::Scalar>::run(*this, other);
575}
576
578template <class Derived>
579template <class OtherDerived>
580EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator*=(
581 const QuaternionBase<OtherDerived>& other) {
582 derived() = derived() * other.derived();
583 return derived();
584}
585
593template <class Derived>
594EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename QuaternionBase<Derived>::Vector3
596 // Note that this algorithm comes from the optimization by hand
597 // of the conversion to a Matrix followed by a Matrix/Vector product.
598 // It appears to be much faster than the common algorithm found
599 // in the literature (30 versus 39 flops). It also requires two
600 // Vector3 as temporaries.
601 Vector3 uv = this->vec().cross(v);
602 uv += uv;
603 return v + this->w() * uv + this->vec().cross(uv);
604}
605
606template <class Derived>
607EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE QuaternionBase<Derived>& QuaternionBase<Derived>::operator=(
608 const QuaternionBase<Derived>& other) {
609 coeffs() = other.coeffs();
610 return derived();
611}
612
613template <class Derived>
614template <class OtherDerived>
615EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(
616 const QuaternionBase<OtherDerived>& other) {
617 coeffs() = other.coeffs();
618 return derived();
619}
620
623template <class Derived>
624EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& QuaternionBase<Derived>::operator=(const AngleAxisType& aa) {
625 Scalar ha = Scalar(0.5) * aa.angle(); // Scalar(0.5) to suppress precision loss warnings
626 this->w() = numext::cos(ha);
627 this->vec() = numext::sin(ha) * aa.axis();
628 return derived();
629}
630
636
637template <class Derived>
638template <class MatrixDerived>
639EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::operator=(const MatrixBase<MatrixDerived>& xpr) {
640 EIGEN_STATIC_ASSERT(
641 (std::is_same<typename Derived::Scalar, typename MatrixDerived::Scalar>::value),
642 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
643 internal::quaternionbase_assign_impl<MatrixDerived>::run(*this, xpr.derived());
644 return derived();
645}
646
650template <class Derived>
652 void) const {
653 // Keep this inline: forcing an out-of-line call is much more expensive than returning Matrix3 by value.
654 Matrix3 res;
655
656 const Scalar tx = Scalar(2) * this->x();
657 const Scalar ty = Scalar(2) * this->y();
658 const Scalar tz = Scalar(2) * this->z();
659 const Scalar twx = tx * this->w();
660 const Scalar twy = ty * this->w();
661 const Scalar twz = tz * this->w();
662 const Scalar txx = tx * this->x();
663 const Scalar txy = ty * this->x();
664 const Scalar txz = tz * this->x();
665 const Scalar tyy = ty * this->y();
666 const Scalar tyz = tz * this->y();
667 const Scalar tzz = tz * this->z();
668
669 res.coeffRef(0, 0) = Scalar(1) - (tyy + tzz);
670 res.coeffRef(0, 1) = txy - twz;
671 res.coeffRef(0, 2) = txz + twy;
672 res.coeffRef(1, 0) = txy + twz;
673 res.coeffRef(1, 1) = Scalar(1) - (txx + tzz);
674 res.coeffRef(1, 2) = tyz - twx;
675 res.coeffRef(2, 0) = txz - twy;
676 res.coeffRef(2, 1) = tyz + twx;
677 res.coeffRef(2, 2) = Scalar(1) - (txx + tyy);
678
679 return res;
680}
681
692template <class Derived>
693template <typename Derived1, typename Derived2>
694EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a,
695 const MatrixBase<Derived2>& b) {
696 Vector3 v0 = a.normalized();
697 Vector3 v1 = b.normalized();
698 Scalar c = v1.dot(v0);
699
700 // if dot == -1, vectors are nearly opposites: an axis orthogonal to v0 alone does not send v0 to v1.
701 // => rotate through a direction m orthogonal to v0, so that both v0 -> m and m -> v1 are ~90 degree rotations.
702 // With u = v0.unitOrthogonal() and m = u x v0, both legs turn about v0 x m = u, so v1 = -v0 still gives (0, u).
703 if (c < Scalar(-1) + NumTraits<Scalar>::dummy_precision()) {
704 const Vector3 m = v0.unitOrthogonal().cross(v0);
705 coeffs() = (Quaternion<Scalar>::FromTwoVectors(m, v1) * Quaternion<Scalar>::FromTwoVectors(v0, m)).coeffs();
706 return derived();
707 }
708 Vector3 axis = v0.cross(v1);
709 Scalar s = numext::sqrt<Scalar>((Scalar(1) + c) * Scalar(2));
710 Scalar invs = Scalar(1) / s;
711 this->vec() = axis * invs;
712 this->w() = s * Scalar(0.5);
713
714 return derived();
715}
716
728template <class Derived>
729template <typename OtherDerived>
730EIGEN_DEVICE_FUNC inline Derived& QuaternionBase<Derived>::setFromScaledAxis(
731 const MatrixBase<OtherDerived>& scaled_axis) {
732 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 3)
733
734 // Materialise the input once so the caller's expression (e.g. omega*dt) is not
735 // recomputed by the norm and the subsequent assignment to vec().
736 const Vector3 v = scaled_axis;
737
738 // Fast path with stableNorm() fallback when norm() returns a suspiciously small
739 // value (below epsilon, where squaredNorm() may have underflowed); same idiom
740 // as AngleAxis::operator=(QuaternionBase).
741 Scalar angle = v.norm();
742 if (angle < NumTraits<Scalar>::epsilon()) angle = v.stableNorm();
743 if (numext::is_exactly_zero(angle)) {
744 this->w() = Scalar(1);
745 this->vec().setZero();
746 return derived();
747 }
748
749 const Scalar half_angle = Scalar(0.5) * angle;
750 this->w() = numext::cos(half_angle);
751 this->vec() = (numext::sin(half_angle) / angle) * v;
752 return derived();
753}
754
768template <class Derived>
770 eigen_assert(internal::isApprox(this->squaredNorm(), Scalar(1), Scalar(8) * NumTraits<Scalar>::dummy_precision()) &&
771 "QuaternionBase::toScaledAxis(): the quaternion must be approximately a unit quaternion");
772
773 // Fast path with stableNorm() fallback; same idiom as AngleAxis::operator=(QuaternionBase).
774 Scalar sin_half_angle = this->vec().norm();
775 if (sin_half_angle < NumTraits<Scalar>::epsilon()) sin_half_angle = this->vec().stableNorm();
776 if (numext::is_exactly_zero(sin_half_angle)) return Vector3::Zero();
777
778 // |w| in atan2 prevents -0.0 from flipping the sign; recover it via the comparison below
779 // (false for both +0.0 and -0.0).
780 // Cannot overflow: for unit q, atan2(s, |c|) is O(s) as s -> 0, so k stays in [2, pi].
781 Scalar k = Scalar(2) * numext::atan2(sin_half_angle, numext::abs(this->w())) / sin_half_angle;
782 if (this->w() < Scalar(0)) k = -k;
783 return k * this->vec();
784}
785
790template <typename Scalar, int Options>
792 const Scalar u1 = internal::random<Scalar>(0, 1), u2 = internal::random<Scalar>(0, 2 * EIGEN_PI),
793 u3 = internal::random<Scalar>(0, 2 * EIGEN_PI);
794 const Scalar a = numext::sqrt<Scalar>(Scalar(1) - u1), b = numext::sqrt(u1);
795 return Quaternion(a * numext::sin(u2), a * numext::cos(u2), b * numext::sin(u3), b * numext::cos(u3));
796}
797
804template <typename Scalar, int Options>
806 const Scalar& y,
807 const Scalar& z,
808 const Scalar& w) {
809 return Quaternion(w, x, y, z);
810}
811
819template <typename Scalar, int Options>
821 const Scalar& x,
822 const Scalar& y,
823 const Scalar& z) {
824 return Quaternion(w, x, y, z);
825}
826
837template <typename Scalar, int Options>
838template <typename Derived1, typename Derived2>
839EIGEN_DEVICE_FUNC Quaternion<Scalar, Options> Quaternion<Scalar, Options>::FromTwoVectors(
840 const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) {
841 Quaternion quat;
842 quat.setFromTwoVectors(a, b);
843 return quat;
844}
845
852template <typename Scalar, int Options>
853template <typename Derived>
854EIGEN_DEVICE_FUNC Quaternion<Scalar, Options> Quaternion<Scalar, Options>::FromScaledAxis(
855 const MatrixBase<Derived>& scaled_axis) {
856 Quaternion quat;
857 quat.setFromScaledAxis(scaled_axis);
858 return quat;
859}
860
867template <class Derived>
869 const {
870 // FIXME: consider renaming to multiplicativeInverse() and renaming conjugate() to inverse() or opposite().
871 Scalar n2 = this->squaredNorm();
872 if (n2 > Scalar(0))
873 return Quaternion<Scalar>(conjugate().coeffs() / n2);
874 else {
875 // return an invalid result to flag the error
876 return Quaternion<Scalar>(Coefficients::Zero());
877 }
878}
879
880// Generic conjugate of a Quaternion
881namespace internal {
882template <int Arch, class Derived, typename Scalar>
883struct quat_conj {
884 EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Quaternion<Scalar> run(const QuaternionBase<Derived>& q) {
885 return Quaternion<Scalar>(q.w(), -q.x(), -q.y(), -q.z());
886 }
887};
888} // namespace internal
889
896template <class Derived>
898 const {
899 return internal::quat_conj<Architecture::Target, Derived, typename internal::traits<Derived>::Scalar>::run(*this);
900}
901
905template <class Derived>
906template <class OtherDerived>
907EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar QuaternionBase<Derived>::angularDistance(
908 const QuaternionBase<OtherDerived>& other) const {
909 Quaternion<Scalar> d = (*this) * other.conjugate();
910 return Scalar(2) * numext::atan2(d.vec().norm(), numext::abs(d.w()));
911}
912
919template <class Derived>
920template <class OtherDerived>
921EIGEN_DEVICE_FUNC Quaternion<typename internal::traits<Derived>::Scalar> QuaternionBase<Derived>::slerp(
922 const Scalar& t, const QuaternionBase<OtherDerived>& other) const {
923 const Scalar one = Scalar(1) - NumTraits<Scalar>::epsilon();
924 Scalar d = this->dot(other);
925 Scalar absD = numext::abs(d);
926
927 Scalar scale0;
928 Scalar scale1;
929
930 if (absD >= one) {
931 // Near-parallel quaternions: use lerp to avoid division by ~zero sinTheta.
932 scale0 = Scalar(1) - t;
933 scale1 = t;
934 } else {
935 // theta is the angle between the 2 quaternions
936 Scalar theta = numext::acos(absD);
937 // 1 - absD is exact for absD >= 1/2 (Sterbenz), so sinTheta has O(eps) relative error. sqrt(1 - absD * absD)
938 // cancels as absD -> 1: relative error up to min(eps / (8 * (1 - absD^2)), (1 - absD) / 4) <= sqrt(eps) / 8.
939 Scalar sinTheta = numext::sqrt<Scalar>((Scalar(1) - absD) * (Scalar(1) + absD));
940
941 scale0 = numext::sin<Scalar>((Scalar(1) - t) * theta) / sinTheta;
942 scale1 = numext::sin<Scalar>(t * theta) / sinTheta;
943 }
944 if (d < Scalar(0)) scale1 = -scale1;
945
946 return Quaternion<Scalar>(scale0 * coeffs() + scale1 * other.coeffs());
947}
948
949namespace internal {
950
951// set from a rotation matrix
952template <typename Other>
953struct quaternionbase_assign_impl<Other, 3, 3> {
954 using Scalar = typename Other::Scalar;
955
956 template <bool Normalize>
957 EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar compute_scale(Scalar& t) {
958 if (!Normalize) return Scalar(1);
959 t = numext::sqrt(t);
960 Scalar s = Scalar(0.5) / t;
961 t *= Scalar(0.5);
962 return s;
963 }
964
965 template <int AxisI, int AxisJ, int AxisK, bool Normalize, class Derived, class Mat>
966 EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE void assign_branch(QuaternionBase<Derived>& q, const Mat& mat,
967 Scalar mii, Scalar mjj, Scalar mkk) {
968 // Guard against slightly negative argument from non-orthogonal matrices.
969 Scalar t = numext::maxi(mii - mjj - mkk + Scalar(1.0), Scalar(0));
970 const Scalar s = compute_scale<Normalize>(t);
971 q.coeffs().coeffRef(AxisI) = t;
972 q.w() = (mat.coeff(AxisK, AxisJ) - mat.coeff(AxisJ, AxisK)) * s;
973 q.coeffs().coeffRef(AxisJ) = (mat.coeff(AxisJ, AxisI) + mat.coeff(AxisI, AxisJ)) * s;
974 q.coeffs().coeffRef(AxisK) = (mat.coeff(AxisK, AxisI) + mat.coeff(AxisI, AxisK)) * s;
975 }
976
977 template <bool Normalize = true, class Derived>
978 EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& a_mat) {
979 const typename internal::nested_eval<Other, 2>::type mat(a_mat);
980 // This algorithm comes from "Quaternion Calculus and Fast Animation",
981 // Ken Shoemake, 1987 SIGGRAPH course notes
982 const Scalar m00 = mat.coeff(0, 0);
983 const Scalar m11 = mat.coeff(1, 1);
984 const Scalar m22 = mat.coeff(2, 2);
985 Scalar t = m00 + (m11 + m22);
986 if (t > Scalar(0)) {
987 t = numext::maxi(t + Scalar(1.0), Scalar(0));
988 const Scalar s = compute_scale<Normalize>(t);
989 q.w() = t;
990 q.x() = (mat.coeff(2, 1) - mat.coeff(1, 2)) * s;
991 q.y() = (mat.coeff(0, 2) - mat.coeff(2, 0)) * s;
992 q.z() = (mat.coeff(1, 0) - mat.coeff(0, 1)) * s;
993 } else if (m00 >= m11 && m00 >= m22) {
994 assign_branch<0, 1, 2, Normalize>(q, mat, m00, m11, m22);
995 } else if (m11 >= m22) {
996 assign_branch<1, 2, 0, Normalize>(q, mat, m11, m22, m00);
997 } else {
998 assign_branch<2, 0, 1, Normalize>(q, mat, m22, m00, m11);
999 }
1000 }
1001};
1002
1003// set from a vector of coefficients assumed to be a quaternion
1004template <typename Other>
1005struct quaternionbase_assign_impl<Other, 4, 1> {
1006 using Scalar = typename Other::Scalar;
1007 template <class Derived>
1008 EIGEN_DEVICE_FUNC static inline void run(QuaternionBase<Derived>& q, const Other& vec) {
1009 q.coeffs() = vec;
1010 }
1011};
1012
1013} // end namespace internal
1014
1015} // end namespace Eigen
1016
1017#endif // EIGEN_QUATERNION_H
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition AngleAxis.h:53
const Vector3 & axis() const
Definition AngleAxis.h:100
Scalar angle() const
Definition AngleAxis.h:95
Map(Scalar *coeffs)
Definition Quaternion.h:506
Map(const Scalar *coeffs)
Definition Quaternion.h:465
A matrix or vector expression mapping an existing array of data.
Definition Map.h:97
Base class for all dense matrices, vectors, and expressions.
Definition MatrixBase.h:53
const PlainObject normalized() const
Definition Dot.h:340
The matrix class, also used for vectors and row-vectors.
Definition Matrix.h:188
constexpr Scalar & coeffRef(Index rowId, Index colId)
Definition PlainObjectBase.h:205
Base class for quaternion expressions.
Definition Quaternion.h:36
constexpr CoeffReturnType z() const
Definition Quaternion.h:64
Scalar squaredNorm() const
Definition Quaternion.h:135
QuaternionBase & setIdentity()
Definition Quaternion.h:127
Quaternion< Scalar > normalized() const
Definition Quaternion.h:147
internal::traits< Derived >::Coefficients & coeffs()
Definition Quaternion.h:108
constexpr CoeffReturnType x() const
Definition Quaternion.h:60
internal::cast_return_type< Derived, Quaternion< NewScalarType > >::type cast() const
constexpr NonConstCoeffReturnType z()
Definition Quaternion.h:73
VectorBlock< Coefficients, 3 > vec()
Definition Quaternion.h:81
const VectorBlock< const Coefficients, 3 > vec() const
Definition Quaternion.h:78
constexpr NonConstCoeffReturnType w()
Definition Quaternion.h:75
static Quaternion< Scalar > Identity()
Definition Quaternion.h:121
Matrix< Scalar, 3, 1 > Vector3
Definition Quaternion.h:53
Matrix< Scalar, 3, 3 > Matrix3
Definition Quaternion.h:55
Matrix< Scalar, 4, 1 > coeffsScalarLast() const
Definition Quaternion.h:105
bool operator!=(const QuaternionBase< OtherDerived > &other) const
Definition Quaternion.h:202
Quaternion< Scalar > conjugate() const
Definition Quaternion.h:897
Matrix< Scalar, 4, 1 > coeffsScalarFirst() const
Definition Quaternion.h:96
bool isApprox(const QuaternionBase< OtherDerived > &other, const RealScalar &prec=NumTraits< Scalar >::dummy_precision()) const
Definition Quaternion.h:211
Vector3 toScaledAxis() const
Definition Quaternion.h:769
void normalize()
Definition Quaternion.h:144
Derived & setFromTwoVectors(const MatrixBase< Derived1 > &a, const MatrixBase< Derived2 > &b)
Definition Quaternion.h:694
Matrix3 toRotationMatrix() const
Definition Quaternion.h:651
constexpr CoeffReturnType y() const
Definition Quaternion.h:62
Scalar dot(const QuaternionBase< OtherDerived > &other) const
Definition Quaternion.h:155
Derived & operator=(const AngleAxisType &aa)
Definition Quaternion.h:624
Vector3 _transformVector(const Vector3 &v) const
Definition Quaternion.h:595
Scalar norm() const
Definition Quaternion.h:140
Quaternion< Scalar > inverse() const
Definition Quaternion.h:868
AngleAxis< Scalar > AngleAxisType
Definition Quaternion.h:57
const internal::traits< Derived >::Coefficients & coeffs() const
Definition Quaternion.h:84
bool operator==(const QuaternionBase< OtherDerived > &other) const
Definition Quaternion.h:193
constexpr CoeffReturnType w() const
Definition Quaternion.h:66
Derived & setFromScaledAxis(const MatrixBase< OtherDerived > &scaled_axis)
Definition Quaternion.h:730
constexpr NonConstCoeffReturnType x()
Definition Quaternion.h:69
constexpr NonConstCoeffReturnType y()
Definition Quaternion.h:71
Derived & operator*=(const QuaternionBase< OtherDerived > &q)
Definition Quaternion.h:580
The quaternion class used to represent 3D orientations and rotations.
Definition Quaternion.h:300
Quaternion(Quaternion &&other) noexcept(std::is_nothrow_move_constructible< Scalar >::value)
Definition Quaternion.h:364
Quaternion(const Quaternion< OtherScalar, OtherOptions > &other)
Definition Quaternion.h:358
static Quaternion FromCoeffsScalarLast(const Scalar &x, const Scalar &y, const Scalar &z, const Scalar &w)
Definition Quaternion.h:805
Quaternion(const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)
Definition Quaternion.h:323
Quaternion(const QuaternionBase< Derived > &other)
Definition Quaternion.h:340
Quaternion(const Scalar &w, const Eigen::MatrixBase< Derived > &vec)
Definition Quaternion.h:330
static Quaternion UnitRandom()
Definition Quaternion.h:791
Quaternion(const MatrixBase< Derived > &other)
Definition Quaternion.h:352
Quaternion(const AngleAxisType &aa)
Definition Quaternion.h:345
Quaternion()
Definition Quaternion.h:314
static Quaternion FromCoeffsScalarFirst(const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)
Definition Quaternion.h:820
Quaternion & operator=(Quaternion &&other) noexcept(std::is_nothrow_move_assignable< Scalar >::value)
Definition Quaternion.h:368
Quaternion(const Scalar *data)
Definition Quaternion.h:336
Common base class for compact rotation representations.
Definition RotationBase.h:33
Expression of a fixed-size or dynamic-size sub-vector.
Definition VectorBlock.h:59
Map< Quaternion< double >, 0 > QuaternionMapd
Definition Quaternion.h:526
Map< Quaternion< float >, Aligned > QuaternionMapAlignedf
Definition Quaternion.h:529
Map< Quaternion< float >, 0 > QuaternionMapf
Definition Quaternion.h:523
Quaternion< float > Quaternionf
Definition Quaternion.h:411
Quaternion< double > Quaterniond
Definition Quaternion.h:414
Map< Quaternion< double >, Aligned > QuaternionMapAlignedd
Definition Quaternion.h:532
@ Aligned
Definition Constants.h:243
@ DontAlign
Definition Constants.h:325
@ AutoAlign
Definition Constants.h:323
constexpr unsigned int LvalueBit
Definition Constants.h:149