131 lines
4.4 KiB
C++
131 lines
4.4 KiB
C++
/*
|
|
* Copyright 2014-2023 CNRS INRIA
|
|
*/
|
|
|
|
#ifndef __eigenpy_angle_axis_hpp__
|
|
#define __eigenpy_angle_axis_hpp__
|
|
|
|
#include "eigenpy/eigenpy.hpp"
|
|
|
|
namespace eigenpy {
|
|
|
|
template <typename AngleAxis>
|
|
class AngleAxisVisitor;
|
|
|
|
template <typename Scalar>
|
|
struct call<Eigen::AngleAxis<Scalar> > {
|
|
typedef Eigen::AngleAxis<Scalar> AngleAxis;
|
|
|
|
static inline void expose() { AngleAxisVisitor<AngleAxis>::expose(); }
|
|
|
|
static inline bool isApprox(
|
|
const AngleAxis& self, const AngleAxis& other,
|
|
const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) {
|
|
return self.isApprox(other, prec);
|
|
}
|
|
};
|
|
|
|
template <typename AngleAxis>
|
|
class AngleAxisVisitor : public bp::def_visitor<AngleAxisVisitor<AngleAxis> > {
|
|
typedef typename AngleAxis::Scalar Scalar;
|
|
typedef typename AngleAxis::Vector3 Vector3;
|
|
typedef typename AngleAxis::Matrix3 Matrix3;
|
|
|
|
typedef typename Eigen::Quaternion<Scalar, 0> Quaternion;
|
|
typedef Eigen::RotationBase<AngleAxis, 3> RotationBase;
|
|
|
|
BOOST_PYTHON_FUNCTION_OVERLOADS(isApproxAngleAxis_overload,
|
|
call<AngleAxis>::isApprox, 2, 3)
|
|
|
|
public:
|
|
template <class PyClass>
|
|
void visit(PyClass& cl) const {
|
|
cl.def(bp::init<>(bp::arg("self"), "Default constructor"))
|
|
.def(bp::init<Scalar, Vector3>(bp::args("self", "angle", "axis"),
|
|
"Initialize from angle and axis."))
|
|
.def(bp::init<Matrix3>(bp::args("self", "R"),
|
|
"Initialize from a rotation matrix"))
|
|
.def(bp::init<Quaternion>(bp::args("self", "quaternion"),
|
|
"Initialize from a quaternion."))
|
|
.def(bp::init<AngleAxis>(bp::args("self", "copy"), "Copy constructor."))
|
|
|
|
/* --- Properties --- */
|
|
.add_property(
|
|
"axis",
|
|
bp::make_function((Vector3 & (AngleAxis::*)()) & AngleAxis::axis,
|
|
bp::return_internal_reference<>()),
|
|
&AngleAxisVisitor::setAxis, "The rotation axis.")
|
|
.add_property("angle",
|
|
(Scalar(AngleAxis::*)() const) & AngleAxis::angle,
|
|
&AngleAxisVisitor::setAngle, "The rotation angle.")
|
|
|
|
/* --- Methods --- */
|
|
.def("inverse", &AngleAxis::inverse, bp::arg("self"),
|
|
"Return the inverse rotation.")
|
|
.def("fromRotationMatrix",
|
|
&AngleAxis::template fromRotationMatrix<Matrix3>,
|
|
(bp::arg("self"), bp::arg("rotation matrix")),
|
|
"Sets *this from a 3x3 rotation matrix", bp::return_self<>())
|
|
.def("toRotationMatrix", &AngleAxis::toRotationMatrix,
|
|
// bp::arg("self"),
|
|
"Constructs and returns an equivalent rotation matrix.")
|
|
.def("matrix", &AngleAxis::matrix, bp::arg("self"),
|
|
"Returns an equivalent rotation matrix.")
|
|
|
|
.def("isApprox", &call<AngleAxis>::isApprox,
|
|
isApproxAngleAxis_overload(
|
|
bp::args("self", "other", "prec"),
|
|
"Returns true if *this is approximately equal to other, "
|
|
"within the precision determined by prec."))
|
|
|
|
/* --- Operators --- */
|
|
.def(bp::self * bp::other<Vector3>())
|
|
.def(bp::self * bp::other<Quaternion>())
|
|
.def(bp::self * bp::self)
|
|
.def("__eq__", &AngleAxisVisitor::__eq__)
|
|
.def("__ne__", &AngleAxisVisitor::__ne__)
|
|
|
|
.def("__str__", &print)
|
|
.def("__repr__", &print);
|
|
}
|
|
|
|
private:
|
|
static void setAxis(AngleAxis& self, const Vector3& axis) {
|
|
self.axis() = axis;
|
|
}
|
|
|
|
static void setAngle(AngleAxis& self, const Scalar& angle) {
|
|
self.angle() = angle;
|
|
}
|
|
|
|
static bool __eq__(const AngleAxis& u, const AngleAxis& v) {
|
|
return u.axis() == v.axis() && v.angle() == u.angle();
|
|
}
|
|
|
|
static bool __ne__(const AngleAxis& u, const AngleAxis& v) {
|
|
return !__eq__(u, v);
|
|
}
|
|
|
|
static std::string print(const AngleAxis& self) {
|
|
std::stringstream ss;
|
|
ss << "angle: " << self.angle() << std::endl;
|
|
ss << "axis: " << self.axis().transpose() << std::endl;
|
|
|
|
return ss.str();
|
|
}
|
|
|
|
public:
|
|
static void expose() {
|
|
bp::class_<AngleAxis>(
|
|
"AngleAxis", "AngleAxis representation of a rotation.\n\n", bp::no_init)
|
|
.def(AngleAxisVisitor<AngleAxis>());
|
|
|
|
// Cast to Eigen::RotationBase
|
|
bp::implicitly_convertible<AngleAxis, RotationBase>();
|
|
}
|
|
};
|
|
|
|
} // namespace eigenpy
|
|
|
|
#endif // ifndef __eigenpy_angle_axis_hpp__
|