51 lines
1.6 KiB
C++
51 lines
1.6 KiB
C++
/*
|
|
* Copyright 2014-2019, CNRS
|
|
* Copyright 2018-2023, INRIA
|
|
*/
|
|
|
|
#ifndef __eigenpy_geometry_conversion_hpp__
|
|
#define __eigenpy_geometry_conversion_hpp__
|
|
|
|
#include "eigenpy/fwd.hpp"
|
|
|
|
namespace eigenpy {
|
|
|
|
template <typename Scalar, int Options = 0>
|
|
struct EulerAnglesConvertor {
|
|
typedef typename Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
|
|
typedef typename Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
|
|
typedef typename Vector3::Index Index;
|
|
|
|
typedef typename Eigen::AngleAxis<Scalar> AngleAxis;
|
|
|
|
static void expose() {
|
|
bp::def("toEulerAngles", &EulerAnglesConvertor::toEulerAngles,
|
|
bp::args("rotation_matrix", "a0", "a1", "a2"),
|
|
"It returns the Euler-angles of the rotation matrix mat using the "
|
|
"convention defined by the triplet (a0,a1,a2).");
|
|
|
|
bp::def("fromEulerAngles", &EulerAnglesConvertor::fromEulerAngles,
|
|
bp::args("euler_angles", "a0", "a1", "a2"),
|
|
"It returns the rotation matrix associated to the Euler angles "
|
|
"using the convention defined by the triplet (a0,a1,a2).");
|
|
}
|
|
|
|
static Vector3 toEulerAngles(const Matrix3& mat, Index a0, Index a1,
|
|
Index a2) {
|
|
return mat.eulerAngles(a0, a1, a2);
|
|
}
|
|
|
|
static Matrix3 fromEulerAngles(const Vector3& ea, Index a0, Index a1,
|
|
Index a2) {
|
|
Matrix3 mat;
|
|
mat = AngleAxis(ea[0], Vector3::Unit(a0)) *
|
|
AngleAxis(ea[1], Vector3::Unit(a1)) *
|
|
AngleAxis(ea[2], Vector3::Unit(a2));
|
|
return mat;
|
|
}
|
|
};
|
|
|
|
} // namespace eigenpy
|
|
|
|
#endif // define __eigenpy_geometry_conversion_hpp__
|