Eigen  5.0.1-dev+master
 
Loading...
Searching...
No Matches
AngleAxis.h
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
5//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9// SPDX-License-Identifier: MPL-2.0
10
11#ifndef EIGEN_ANGLEAXIS_H
12#define EIGEN_ANGLEAXIS_H
13
14// IWYU pragma: private
15#include "./InternalHeaderCheck.h"
16
17namespace Eigen {
18
44
45namespace internal {
46template <typename Scalar_>
47struct traits<AngleAxis<Scalar_> > {
48 using Scalar = Scalar_;
49};
50} // namespace internal
51
52template <typename Scalar_>
53class AngleAxis : public RotationBase<AngleAxis<Scalar_>, 3> {
54 using Base = RotationBase<AngleAxis<Scalar_>, 3>;
55
56 public:
57 using Base::operator*;
58
59 enum { Dim = 3 };
61 using Scalar = Scalar_;
62 using Matrix3 = Matrix<Scalar, 3, 3>;
63 using Vector3 = Matrix<Scalar, 3, 1>;
64 using QuaternionType = Quaternion<Scalar>;
65
66 protected:
67 Vector3 m_axis;
68 Scalar m_angle;
69
70 public:
72 EIGEN_DEVICE_FUNC AngleAxis() {}
78 template <typename Derived>
79 EIGEN_DEVICE_FUNC inline AngleAxis(const Scalar& angle, const MatrixBase<Derived>& axis)
80 : m_axis(axis), m_angle(angle) {}
81
84 template <typename QuatDerived>
85 EIGEN_DEVICE_FUNC inline explicit AngleAxis(const QuaternionBase<QuatDerived>& q) {
86 *this = q;
87 }
88
89 template <typename Derived>
90 EIGEN_DEVICE_FUNC inline explicit AngleAxis(const MatrixBase<Derived>& m) {
91 *this = m;
92 }
93
95 EIGEN_DEVICE_FUNC Scalar angle() const { return m_angle; }
97 EIGEN_DEVICE_FUNC Scalar& angle() { return m_angle; }
98
100 EIGEN_DEVICE_FUNC const Vector3& axis() const { return m_axis; }
105 EIGEN_DEVICE_FUNC Vector3& axis() { return m_axis; }
106
108 EIGEN_DEVICE_FUNC inline QuaternionType operator*(const AngleAxis& other) const {
109 return QuaternionType(*this) * QuaternionType(other);
110 }
111
113 EIGEN_DEVICE_FUNC inline QuaternionType operator*(const QuaternionType& other) const {
114 return QuaternionType(*this) * other;
115 }
116
118 friend EIGEN_DEVICE_FUNC inline QuaternionType operator*(const QuaternionType& a, const AngleAxis& b) {
119 return a * QuaternionType(b);
120 }
121
123 EIGEN_DEVICE_FUNC AngleAxis inverse() const { return AngleAxis(-m_angle, m_axis); }
124
125 template <class QuatDerived>
126 EIGEN_DEVICE_FUNC AngleAxis& operator=(const QuaternionBase<QuatDerived>& q);
127 template <typename Derived>
128 EIGEN_DEVICE_FUNC AngleAxis& operator=(const MatrixBase<Derived>& m);
129
130 template <typename Derived>
131 EIGEN_DEVICE_FUNC AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
132 EIGEN_DEVICE_FUNC Matrix3 toRotationMatrix(void) const;
133
136 template <typename OtherVectorType>
137 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Vector3 _transformVector(const OtherVectorType& v) const {
138 EIGEN_INTERNAL_ASSERT_SAME_SIZE(v, m_axis);
139
140 // Rodrigues' rotation formula: v' = v*cos(θ) + (k×v)*sin(θ) + k*(k·v)*(1-cos(θ))
141 const Vector3 v3(v);
142 const Scalar c = numext::cos(m_angle);
143 const Scalar s = numext::sin(m_angle);
144 return v3 * c + m_axis.cross(v3) * s + m_axis * (m_axis.dot(v3) * (Scalar(1) - c));
145 }
146
152 template <typename NewScalarType>
153 EIGEN_DEVICE_FUNC inline typename internal::cast_return_type<AngleAxis, AngleAxis<NewScalarType> >::type cast()
154 const {
155 return typename internal::cast_return_type<AngleAxis, AngleAxis<NewScalarType> >::type(*this);
156 }
157
159 template <typename OtherScalarType>
160 EIGEN_DEVICE_FUNC inline explicit AngleAxis(const AngleAxis<OtherScalarType>& other) {
161 m_axis = other.axis().template cast<Scalar>();
162 m_angle = Scalar(other.angle());
163 }
164
165 EIGEN_DEVICE_FUNC static inline const AngleAxis Identity() { return AngleAxis(Scalar(0), Vector3::UnitX()); }
166
171 EIGEN_DEVICE_FUNC bool isApprox(const AngleAxis& other, const typename NumTraits<Scalar>::Real& prec =
172 NumTraits<Scalar>::dummy_precision()) const {
173 return m_axis.isApprox(other.m_axis, prec) && internal::isApprox(m_angle, other.m_angle, prec);
174 }
175};
176
183
190template <typename Scalar>
191template <typename QuatDerived>
192EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionBase<QuatDerived>& q) {
193 Scalar n = q.vec().norm();
194 if (n < NumTraits<Scalar>::epsilon()) n = q.vec().stableNorm();
195
196 if (n != Scalar(0)) {
197 m_angle = Scalar(2) * numext::atan2(n, numext::abs(q.w()));
198 if (q.w() < Scalar(0)) n = -n;
199 m_axis = q.vec() / n;
200 } else {
201 m_angle = Scalar(0);
202 m_axis << Scalar(1), Scalar(0), Scalar(0);
203 }
204 return *this;
205}
206
209template <typename Scalar>
210template <typename Derived>
211EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat) {
212 // A 4D vector holds quaternion coefficients; anything else is a 3x3 rotation matrix.
213 if (mat.size() == 4) {
214 return *this = QuaternionType(mat);
215 }
216 return fromRotationMatrix(mat);
217}
218
224template <typename Scalar>
225template <typename Derived>
226EIGEN_DEVICE_FUNC AngleAxis<Scalar>& AngleAxis<Scalar>::fromRotationMatrix(const MatrixBase<Derived>& mat) {
227 EIGEN_STATIC_ASSERT(
228 (std::is_same<Scalar, typename Derived::Scalar>::value),
229 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
230 // operator= also instantiates this path for quaternion coefficients in C++14.
231 // TODO(C++17): use fixed-size if constexpr dispatch in operator= before adding a static 3-by-3 check here.
232 EIGEN_INTERNAL_SHAPE_ASSERT((internal::fixed_dimensions<Derived, 3, 3>::Proven), mat.cols() == 3 && mat.rows() == 3);
233
234 // Compute a (non-unit) quaternion proportional to the rotation using
235 // Shoemake's algorithm (shared with Quaternion::operator=(MatrixBase)),
236 // skipping the intermediate normalization since AngleAxis::operator=(QuaternionBase)
237 // normalizes the axis and uses atan2(|v|, |w|).
238 QuaternionType q;
239 internal::quaternionbase_assign_impl<Derived, 3, 3>::template run<false>(q, mat.derived());
240 return *this = q;
241}
242
245template <typename Scalar>
246typename AngleAxis<Scalar>::Matrix3 EIGEN_DEVICE_FUNC AngleAxis<Scalar>::toRotationMatrix(void) const {
247 Matrix3 res;
248 Vector3 sin_axis = numext::sin(m_angle) * m_axis;
249 Scalar c = numext::cos(m_angle);
250 Vector3 cos1_axis = (Scalar(1) - c) * m_axis;
251
252 Scalar tmp;
253 tmp = cos1_axis.x() * m_axis.y();
254 res.coeffRef(0, 1) = tmp - sin_axis.z();
255 res.coeffRef(1, 0) = tmp + sin_axis.z();
256
257 tmp = cos1_axis.x() * m_axis.z();
258 res.coeffRef(0, 2) = tmp + sin_axis.y();
259 res.coeffRef(2, 0) = tmp - sin_axis.y();
260
261 tmp = cos1_axis.y() * m_axis.z();
262 res.coeffRef(1, 2) = tmp - sin_axis.x();
263 res.coeffRef(2, 1) = tmp + sin_axis.x();
264
265 res.diagonal() = cos1_axis.cwiseProduct(m_axis).array() + c;
266
267 return res;
268}
269
270} // end namespace Eigen
271
272#endif // EIGEN_ANGLEAXIS_H
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
Definition AngleAxis.h:53
QuaternionType operator*(const QuaternionType &other) const
Definition AngleAxis.h:113
AngleAxis(const AngleAxis< OtherScalarType > &other)
Definition AngleAxis.h:160
QuaternionType operator*(const AngleAxis &other) const
Definition AngleAxis.h:108
Scalar & angle()
Definition AngleAxis.h:97
Vector3 & axis()
Definition AngleAxis.h:105
AngleAxis(const QuaternionBase< QuatDerived > &q)
Definition AngleAxis.h:85
internal::cast_return_type< AngleAxis, AngleAxis< NewScalarType > >::type cast() const
Definition AngleAxis.h:153
Vector3 _transformVector(const OtherVectorType &v) const
Definition AngleAxis.h:137
AngleAxis(const Scalar &angle, const MatrixBase< Derived > &axis)
Definition AngleAxis.h:79
Scalar_ Scalar
Definition AngleAxis.h:61
AngleAxis(const MatrixBase< Derived > &m)
Definition AngleAxis.h:90
AngleAxis inverse() const
Definition AngleAxis.h:123
const Vector3 & axis() const
Definition AngleAxis.h:100
AngleAxis()
Definition AngleAxis.h:72
bool isApprox(const AngleAxis &other, const typename NumTraits< Scalar >::Real &prec=NumTraits< Scalar >::dummy_precision()) const
Definition AngleAxis.h:171
friend QuaternionType operator*(const QuaternionType &a, const AngleAxis &b)
Definition AngleAxis.h:118
Matrix3 toRotationMatrix(void) const
Definition AngleAxis.h:246
Scalar angle() const
Definition AngleAxis.h:95
Base class for all dense matrices, vectors, and expressions.
Definition MatrixBase.h:53
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
const VectorBlock< const Coefficients, 3 > vec() const
Definition Quaternion.h:78
constexpr CoeffReturnType w() const
Definition Quaternion.h:66
The quaternion class used to represent 3D orientations and rotations.
Definition Quaternion.h:300
Common base class for compact rotation representations.
Definition RotationBase.h:33
AngleAxis< double > AngleAxisd
Definition AngleAxis.h:182
AngleAxis< float > AngleAxisf
Definition AngleAxis.h:179