1 |
|
|
/* |
2 |
|
|
* Copyright 2014-2019, CNRS |
3 |
|
|
* Copyright 2018-2023, INRIA |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#ifndef __eigenpy_geometry_conversion_hpp__ |
7 |
|
|
#define __eigenpy_geometry_conversion_hpp__ |
8 |
|
|
|
9 |
|
|
#include "eigenpy/fwd.hpp" |
10 |
|
|
|
11 |
|
|
namespace eigenpy { |
12 |
|
|
|
13 |
|
|
template <typename Scalar, int Options = 0> |
14 |
|
|
struct EulerAnglesConvertor { |
15 |
|
|
typedef typename Eigen::Matrix<Scalar, 3, 1, Options> Vector3; |
16 |
|
|
typedef typename Eigen::Matrix<Scalar, 3, 3, Options> Matrix3; |
17 |
|
|
typedef typename Vector3::Index Index; |
18 |
|
|
|
19 |
|
|
typedef typename Eigen::AngleAxis<Scalar> AngleAxis; |
20 |
|
|
|
21 |
|
10 |
static void expose() { |
22 |
✓✗ |
10 |
bp::def("toEulerAngles", &EulerAnglesConvertor::toEulerAngles, |
23 |
|
|
bp::args("rotation_matrix", "a0", "a1", "a2"), |
24 |
|
|
"It returns the Euler-angles of the rotation matrix mat using the " |
25 |
|
|
"convention defined by the triplet (a0,a1,a2)."); |
26 |
|
|
|
27 |
✓✗ |
10 |
bp::def("fromEulerAngles", &EulerAnglesConvertor::fromEulerAngles, |
28 |
|
|
bp::args("euler_angles", "a0", "a1", "a2"), |
29 |
|
|
"It returns the rotation matrix associated to the Euler angles " |
30 |
|
|
"using the convention defined by the triplet (a0,a1,a2)."); |
31 |
|
10 |
} |
32 |
|
|
|
33 |
|
|
static Vector3 toEulerAngles(const Matrix3& mat, Index a0, Index a1, |
34 |
|
|
Index a2) { |
35 |
|
|
return mat.eulerAngles(a0, a1, a2); |
36 |
|
|
} |
37 |
|
|
|
38 |
|
|
static Matrix3 fromEulerAngles(const Vector3& ea, Index a0, Index a1, |
39 |
|
|
Index a2) { |
40 |
|
|
Matrix3 mat; |
41 |
|
|
mat = AngleAxis(ea[0], Vector3::Unit(a0)) * |
42 |
|
|
AngleAxis(ea[1], Vector3::Unit(a1)) * |
43 |
|
|
AngleAxis(ea[2], Vector3::Unit(a2)); |
44 |
|
|
return mat; |
45 |
|
|
} |
46 |
|
|
}; |
47 |
|
|
|
48 |
|
|
} // namespace eigenpy |
49 |
|
|
|
50 |
|
|
#endif // define __eigenpy_geometry_conversion_hpp__ |