geometry-conversion.hpp

Go to the documentation of this file.

/*
 * 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__