diff --git a/src/angle-axis.hpp b/src/angle-axis.hpp
index c04deef66d7e99119e27445e15806fc2232fb13c..8048a38ac54453021a87f50c2f4fbfed36ea7f34 100644
--- a/src/angle-axis.hpp
+++ b/src/angle-axis.hpp
@@ -23,119 +23,99 @@
namespace eigenpy
{
- template<>
- struct UnalignedEquivalent<Eigen::AngleAxisd>
- {
- typedef Eigen::AngleAxis<double> type;
- };
namespace bp = boost::python;
- template<typename D>
+ template<typename AngleAxis>
class AngleAxisVisitor
- : public boost::python::def_visitor< AngleAxisVisitor<D> >
+ : public boost::python::def_visitor< AngleAxisVisitor<AngleAxis> >
{
- typedef D AngleAxis;
- typedef typename eigenpy::UnalignedEquivalent<D>::type AngleAxisUnaligned;
-
- typedef typename AngleAxisUnaligned::Scalar Scalar;
- typedef typename eigenpy::UnalignedEquivalent<typename AngleAxisUnaligned::Vector3>::type Vector3;
- typedef typename eigenpy::UnalignedEquivalent<typename AngleAxisUnaligned::Matrix3>::type Matrix3;
-
- typedef eigenpy::UnalignedEquivalent<Eigen::Quaternion<Scalar> > Quaternion;
+ typedef typename AngleAxis::Scalar Scalar;
+ typedef typename AngleAxis::Vector3 Vector3;
+ typedef typename AngleAxis::Matrix3 Matrix3;
+
+ typedef typename Eigen::Quaternion<Scalar,0> Quaternion;
+
public:
template<class PyClass>
void visit(PyClass& cl) const
{
cl
- .def(bp::init<Scalar,Vector3>
- ((bp::arg("angle"),bp::arg("axis")),
- "Initialize from angle and axis"))
- .def(bp::init<Matrix3>
- ((bp::arg("rotationMatrix")),
- "Initialize from a rotation matrix"))
- .def("__init__",bp::make_constructor(&AngleAxisVisitor::constructFromQuaternion,
- bp::default_call_policies(),
- (bp::arg("quaternion"))),"Initialize from quaternion")
- .def(bp::init<AngleAxisUnaligned>((bp::arg("clone"))))
-
- .def("matrix",&AngleAxisUnaligned::toRotationMatrix,"Return the corresponding rotation matrix 3x3.")
- .def("vector",&AngleAxisVisitor::toVector3,"Return the correspond angle*axis vector3")
- .add_property("axis",&AngleAxisVisitor::getAxis,&AngleAxisVisitor::setAxis)
- .add_property("angle",&AngleAxisVisitor::getAngle,&AngleAxisVisitor::setAngle)
-
- /* --- Methods --- */
- .def("normalize",&AngleAxisVisitor::normalize,"Normalize the axis vector (without changing the angle).")
- .def("inverse",&AngleAxisUnaligned::inverse,"Return the inverse rotation.")
- .def("apply",&AngleAxisVisitor::apply,(bp::arg("vector3")),"Apply the rotation map to the vector")
-
- /* --- Operators --- */
- .def(bp::self * bp::other<Vector3>())
- .def("__eq__",&AngleAxisVisitor::__eq__)
- .def("__ne__",&AngleAxisVisitor::__ne__)
- .def("__abs__",&AngleAxisVisitor::getAngleAbs)
- ;
+ .def(bp::init<>("Default constructor"))
+ .def(bp::init<Scalar,Vector3>
+ ((bp::arg("angle"),bp::arg("axis")),
+ "Initialize from angle and axis."))
+ .def(bp::init<Matrix3>
+ ((bp::arg("rotationMatrix")),
+ "Initialize from a rotation matrix"))
+ .def(bp::init<Quaternion>(bp::arg("quaternion"),"Initialize from a quaternion."))
+ .def(bp::init<AngleAxis>(bp::arg("copy"),"Copy constructor."))
+
+ /* --- Properties --- */
+ .add_property("axis",
+ bp::make_function((const Vector3 & (AngleAxis::*)()const)&AngleAxis::axis,
+ bp::return_value_policy<bp::copy_const_reference>()),
+ &AngleAxisVisitor::setAxis,"The rotation axis.")
+ .add_property("angle",
+ (Scalar (AngleAxis::*)()const)&AngleAxis::angle,
+ &AngleAxisVisitor::setAngle,"The rotation angle.")
+
+ /* --- Methods --- */
+ .def("inverse",&AngleAxis::inverse,"Return the inverse rotation.")
+ .def("fromRotationMatrix",&AngleAxis::template fromRotationMatrix<Matrix3>,
+ bp::arg("Sets *this from a 3x3 rotation matrix."),bp::return_self<>())
+ .def("toRotationMatrix",&AngleAxis::toRotationMatrix,"Constructs and returns an equivalent 3x3 rotation matrix.")
+ .def("matrix",&AngleAxis::matrix,"Returns an equivalent rotation matrix.")
+ .def("isApprox",(bool (AngleAxis::*)(const AngleAxis &))&AngleAxis::isApprox,
+ "Returns true if *this is approximately equal to other.")
+ .def("isApprox",(bool (AngleAxis::*)(const AngleAxis &, const Scalar prec))&AngleAxis::isApprox,
+ bp::args("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 AngleAxisUnaligned* constructFromQuaternion(const Eigen::Quaternion<Scalar,Eigen::DontAlign> & qu)
- {
- Eigen::Quaternion<Scalar> q = qu;
- return new AngleAxisUnaligned(q);
- }
-
- static Vector3 apply(const AngleAxisUnaligned & r, const Vector3 & v ) { return r*v; }
- static Vector3 getAxis(const AngleAxisUnaligned& self) { return self.axis(); }
- static void setAxis(AngleAxisUnaligned& self, const Vector3 & r)
- {
- self = AngleAxisUnaligned( self.angle(),r );
- }
+ static void setAxis(AngleAxis & self, const Vector3 & axis)
+ { self.axis() = axis; }
- static double getAngle(const AngleAxisUnaligned& self) { return self.angle(); }
- static void setAngle( AngleAxisUnaligned& self, const double & th)
- {
- self = AngleAxisUnaligned( th,self.axis() );
- }
- static double getAngleAbs(const AngleAxisUnaligned& self) { return std::abs(self.angle()); }
+ static void setAngle( AngleAxis & self, const Scalar & angle)
+ { self.angle() = angle; }
- static bool __eq__(const AngleAxisUnaligned & u, const AngleAxisUnaligned & v)
- {
- return u.isApprox(v);
- }
- static bool __ne__(const AngleAxisUnaligned & u, const AngleAxisUnaligned & v)
- {
- return !__eq__(u,v);
- }
+ static bool __eq__(const AngleAxis & u, const AngleAxis & v)
+ { return u.isApprox(v); }
+ static bool __ne__(const AngleAxis & u, const AngleAxis & v)
+ { return !__eq__(u,v); }
- static Vector3 toVector3( const AngleAxisUnaligned & self ) { return self.axis()*self.angle(); }
- static void normalize( AngleAxisUnaligned & self )
+ static std::string print(const AngleAxis & self)
{
- setAxis(self,self.axis() / self.axis().norm());
- }
-
- private:
-
- static PyObject* convert(AngleAxis const& q)
- {
- AngleAxisUnaligned qx = q;
- return boost::python::incref(boost::python::object(qx).ptr());
+ 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_<AngleAxisUnaligned>("AngleAxis",
- "AngleAxis representation of rotations.\n\n",
- bp::init<>())
- .def(AngleAxisVisitor<D>());
-
- // TODO: check the problem of fix-size Angle Axis.
- //bp::to_python_converter< AngleAxis,AngleAxisVisitor<D> >();
-
+ bp::class_<AngleAxis>("AngleAxis",
+ "AngleAxis representation of rotations.\n\n",
+ bp::no_init)
+ .def(AngleAxisVisitor<AngleAxis>());
}
};