1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
|
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#ifndef EIGEN_ANGLEAXIS_H
#define EIGEN_ANGLEAXIS_H
/** \geometry_module \ingroup Geometry
*
* \class AngleAxis
*
* \brief Represents a 3D rotation as a rotation angle around an arbitrary 3D axis
*
* \param _Scalar the scalar type, i.e., the type of the coefficients.
*
* The following two typedefs are provided for convenience:
* \li \c AngleAxisf for \c float
* \li \c AngleAxisd for \c double
*
* \addexample AngleAxisForEuler \label How to define a rotation from Euler-angles
*
* Combined with MatrixBase::Unit{X,Y,Z}, AngleAxis can be used to easily
* mimic Euler-angles. Here is an example:
* \include AngleAxis_mimic_euler.cpp
* Output: \verbinclude AngleAxis_mimic_euler.out
*
* \sa class Quaternion, class Transform, MatrixBase::UnitX()
*/
template<typename _Scalar>
class AngleAxis
{
public:
enum { Dim = 3 };
/** the scalar type of the coefficients */
typedef _Scalar Scalar;
typedef Matrix<Scalar,3,3> Matrix3;
typedef Matrix<Scalar,3,1> Vector3;
typedef Quaternion<Scalar> QuaternionType;
protected:
Vector3 m_axis;
Scalar m_angle;
public:
/** Default constructor without initialization. */
AngleAxis() {}
/** Constructs and initialize the angle-axis rotation from an \a angle in radian
* and an \a axis which must be normalized. */
template<typename Derived>
inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
/** Constructs and initialize the angle-axis rotation from a quaternion \a q. */
inline AngleAxis(const QuaternionType& q) { *this = q; }
/** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
template<typename Derived>
inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
Scalar angle() const { return m_angle; }
Scalar& angle() { return m_angle; }
const Vector3& axis() const { return m_axis; }
Vector3& axis() { return m_axis; }
/** Concatenates two rotations */
inline QuaternionType operator* (const AngleAxis& other) const
{ return QuaternionType(*this) * QuaternionType(other); }
/** Concatenates two rotations */
inline QuaternionType operator* (const QuaternionType& other) const
{ return QuaternionType(*this) * other; }
/** Concatenates two rotations */
friend inline QuaternionType operator* (const QuaternionType& a, const AngleAxis& b)
{ return a * QuaternionType(b); }
/** Concatenates two rotations */
inline Matrix3
operator* (const Matrix3& other) const
{ return toRotationMatrix() * other; }
/** Concatenates two rotations */
inline friend Matrix3
operator* (const Matrix3& a, const AngleAxis& b)
{ return a * b.toRotationMatrix(); }
/** Applies rotation to vector */
inline Vector3
operator* (const Vector3& other) const
{ return toRotationMatrix() * other; }
/** \returns the inverse rotation, i.e., an angle-axis with opposite rotation angle */
AngleAxis inverse() const
{ return AngleAxis(-m_angle, m_axis); }
AngleAxis& operator=(const QuaternionType& q);
template<typename Derived>
AngleAxis& operator=(const MatrixBase<Derived>& m);
template<typename Derived>
AngleAxis& fromRotationMatrix(const MatrixBase<Derived>& m);
Matrix3 toRotationMatrix(void) const;
};
/** \ingroup Geometry
* single precision angle-axis type */
typedef AngleAxis<float> AngleAxisf;
/** \ingroup Geometry
* double precision angle-axis type */
typedef AngleAxis<double> AngleAxisd;
/** Set \c *this from a quaternion.
* The axis is normalized.
*/
template<typename Scalar>
AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
{
Scalar n2 = q.vec().norm2();
if (n2 < precision<Scalar>()*precision<Scalar>())
{
m_angle = 0;
m_axis << 1, 0, 0;
}
else
{
m_angle = 2*std::acos(q.w());
m_axis = q.vec() / ei_sqrt(n2);
}
return *this;
}
/** Set \c *this from a 3x3 rotation matrix \a mat.
*/
template<typename Scalar>
template<typename Derived>
AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const MatrixBase<Derived>& mat)
{
// Since a direct conversion would not be really faster,
// let's use the robust Quaternion implementation:
return *this = QuaternionType(mat);
}
/** Constructs and \returns an equivalent 3x3 rotation matrix.
*/
template<typename Scalar>
typename AngleAxis<Scalar>::Matrix3
AngleAxis<Scalar>::toRotationMatrix(void) const
{
Matrix3 res;
Vector3 sin_axis = ei_sin(m_angle) * m_axis;
Scalar c = ei_cos(m_angle);
Vector3 cos1_axis = (Scalar(1)-c) * m_axis;
Scalar tmp;
tmp = cos1_axis.x() * m_axis.y();
res.coeffRef(0,1) = tmp - sin_axis.z();
res.coeffRef(1,0) = tmp + sin_axis.z();
tmp = cos1_axis.x() * m_axis.z();
res.coeffRef(0,2) = tmp + sin_axis.y();
res.coeffRef(2,0) = tmp - sin_axis.y();
tmp = cos1_axis.y() * m_axis.z();
res.coeffRef(1,2) = tmp - sin_axis.x();
res.coeffRef(2,1) = tmp + sin_axis.x();
res.diagonal() = (cos1_axis.cwise() * m_axis).cwise() + c;
return res;
}
/** \geometry_module
*
* Constructs a 3x3 rotation matrix from the angle-axis \a aa
*
* \sa Matrix(const Quaternion&)
*/
template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols>
Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>::Matrix(const AngleAxis<Scalar>& aa)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,3,3);
*this = aa.toRotationMatrix();
}
/** \geometry_module
*
* Set a 3x3 rotation matrix from the angle-axis \a aa
*/
template<typename _Scalar, int _Rows, int _Cols, int _Storage, int _MaxRows, int _MaxCols>
Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>&
Matrix<_Scalar, _Rows, _Cols, _Storage, _MaxRows, _MaxCols>::operator=(const AngleAxis<Scalar>& aa)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix,3,3);
return *this = aa.toRotationMatrix();
}
#endif // EIGEN_ANGLEAXIS_H
|
