Commit c7fdc964 authored by jcarpent's avatar jcarpent
Browse files

[C++] Update bindings of Quaternion class

- Update API to follow Eigen conventions
- Add Doc
- Remove unaligned conversions
parent af32ca45
......@@ -28,7 +28,6 @@ namespace eigenpy
class ExceptionIndex : public Exception
{
public:
int index;
ExceptionIndex(int index,int imin,int imax) : Exception("")
{
std::ostringstream oss; oss << "Index " << index << " out of range " << imin << ".."<< imax <<".";
......@@ -36,16 +35,6 @@ namespace eigenpy
}
};
template<>
struct UnalignedEquivalent<Eigen::Quaterniond>
{
#ifndef EIGENPY_ALIGNED
typedef Eigen::Quaternion<Eigen::Quaterniond::Scalar,Eigen::DontAlign> type;
#else
typedef Eigen::Quaterniond type;
#endif
};
namespace bp = boost::python;
template<typename Quaternion>
......@@ -53,135 +42,169 @@ namespace eigenpy
: public boost::python::def_visitor< QuaternionVisitor<Quaternion> >
{
typedef Eigen::QuaternionBase<Quaternion> QuaternionBase;
typedef typename eigenpy::UnalignedEquivalent<Quaternion>::type QuaternionUnaligned;
typedef typename QuaternionBase::Scalar Scalar;
typedef typename QuaternionUnaligned::Coefficients Coefficients;
#ifndef EIGENPY_ALIGNED
typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vector3;
typedef Eigen::Matrix<Scalar,3,3,Eigen::DontAlign> Matrix3;
#else
typedef Eigen::Matrix<Scalar,3,1,0> Vector3;
typedef Eigen::Matrix<Scalar,3,3,0> Matrix3;
#endif
typedef typename Quaternion::Coefficients Coefficients;
typedef Eigen::Matrix<Scalar,3,1> Vector3;
typedef Eigen::Matrix<Scalar,4,1> Vector4;
typedef Eigen::Matrix<Scalar,3,3> Matrix3;
typedef Eigen::AngleAxis<Scalar> AngleAxisUnaligned;
typedef Eigen::AngleAxis<Scalar> AngleAxis;
public:
/* Conversion from C++ to Python. */
static PyObject* convert(Quaternion const& q)
{
QuaternionUnaligned qx = q;
return boost::python::incref(boost::python::object(qx).ptr());
}
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<Matrix3>((bp::arg("matrixRotation")),"Initialize from rotation matrix."))
.def(bp::init<AngleAxisUnaligned>((bp::arg("angleaxis")),"Initialize from angle axis."))
.def(bp::init<QuaternionUnaligned>((bp::arg("clone")),"Copy constructor."))
.def("__init__",bp::make_constructor(&QuaternionVisitor::fromTwoVectors,
bp::default_call_policies(),
(bp::arg("u"),bp::arg("v"))),"Initialize from two vector u,v")
.def(bp::init<Scalar,Scalar,Scalar,Scalar>
((bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")),
"Initialize from coefficients.\n\n"
"... note:: The order of coefficients is *w*, *x*, *y*, *z*. "
"The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))
/* --- Methods --- */
.def("coeffs",&QuaternionVisitor::coeffs)
.def("matrix",&QuaternionUnaligned::toRotationMatrix)
.def(bp::init<>("Default constructor"))
.def(bp::init<Matrix3>((bp::arg("matrixRotation")),"Initialize from rotation matrix."))
.def(bp::init<AngleAxis>((bp::arg("angleaxis")),"Initialize from angle axis."))
.def(bp::init<Quaternion>((bp::arg("clone")),"Copy constructor."))
.def("__init__",bp::make_constructor(&QuaternionVisitor::fromTwoVectors,
bp::default_call_policies(),
(bp::arg("u"),bp::arg("v"))),"Initialize from two vector u,v")
.def(bp::init<Scalar,Scalar,Scalar,Scalar>
((bp::arg("w"),bp::arg("x"),bp::arg("y"),bp::arg("z")),
"Initialize from coefficients.\n\n"
"... note:: The order of coefficients is *w*, *x*, *y*, *z*. "
"The [] operator numbers them differently, 0...4 for *x* *y* *z* *w*!"))
.add_property("x",
(Scalar (Quaternion::*)()const)&Quaternion::x,
&QuaternionVisitor::setCoeff<0>,"The x coefficient.")
.add_property("y",
(Scalar (Quaternion::*)()const)&Quaternion::y,
&QuaternionVisitor::setCoeff<1>,"The y coefficient.")
.add_property("z",
(Scalar (Quaternion::*)()const)&Quaternion::z,
&QuaternionVisitor::setCoeff<2>,"The z coefficient.")
.add_property("w",
(Scalar (Quaternion::*)()const)&Quaternion::w,
&QuaternionVisitor::setCoeff<3>,"The w coefficient.")
.def("isApprox",(bool (Quaternion::*)(const Quaternion &))&Quaternion::template isApprox<Quaternion>,
"Returns true if *this is approximately equal to other.")
.def("isApprox",(bool (Quaternion::*)(const Quaternion &, const Scalar prec))&Quaternion::template isApprox<Quaternion>,
"Returns true if *this is approximately equal to other, within the precision determined by prec..")
/* --- Methods --- */
.def("coeffs",(const Vector4 & (Quaternion::*)()const)&Quaternion::coeffs,
bp::return_value_policy<bp::copy_const_reference>())
.def("matrix",&Quaternion::matrix,"Returns an equivalent rotation matrix")
.def("toRotationMatrix ",&Quaternion::toRotationMatrix,"Returns an equivalent 3x3 rotation matrix.")
.def("setFromTwoVectors",&setFromTwoVectors,((bp::arg("a"),bp::arg("b"))),"Set *this to be the quaternion which transform a into b through a rotation."
,bp::return_self<>())
.def("conjugate",&Quaternion::conjugate,"Returns the conjugated quaternion. The conjugate of a quaternion represents the opposite rotation.")
.def("inverse",&Quaternion::inverse,"Returns the quaternion describing the inverse rotation.")
.def("setIdentity",&Quaternion::setIdentity,bp::return_self<>(),"Set *this to the idendity rotation.")
.def("norm",&Quaternion::norm,"Returns the norm of the quaternion's coefficients.")
.def("normalize",&Quaternion::normalize,"Normalizes the quaternion *this.")
.def("normalized",&Quaternion::normalized,"Returns a normalized copy of *this.")
.def("squaredNorm",&Quaternion::squaredNorm,"Returns the squared norm of the quaternion's coefficients.")
.def("dot",&Quaternion::template dot<Quaternion>,bp::arg("other"),"Returns the dot product of *this with other"
"Geometrically speaking, the dot product of two unit quaternions corresponds to the cosine of half the angle between the two rotations.")
.def("_transformVector",&Quaternion::_transformVector,bp::arg("vector"),"Rotation of a vector by a quaternion.")
.def("vec",&vec,"Returns a vector expression of the imaginary part (x,y,z).")
.def("angularDistance",&Quaternion::template angularDistance<Quaternion>,"Returns the angle (in radian) between two rotations.")
.def("slerp",&Quaternion::template slerp<Quaternion>,bp::args("t","other"),
"Returns the spherical linear interpolation between the two quaternions *this and other at the parameter t in [0;1].")
/* --- Operators --- */
.def(bp::self * bp::self)
.def(bp::self *= bp::self)
.def(bp::self * bp::other<Vector3>())
.def("__eq__",&QuaternionVisitor::__eq__)
.def("__ne__",&QuaternionVisitor::__ne__)
.def("__abs__",&Quaternion::norm)
.def("__len__",&QuaternionVisitor::__len__).staticmethod("__len__")
.def("__setitem__",&QuaternionVisitor::__setitem__)
.def("__getitem__",&QuaternionVisitor::__getitem__)
.def("assign",&assign<Quaternion>,
bp::arg("quat"),"Set *this from an quaternion quat and returns a reference to *this.",bp::return_self<>())
.def("assign",(Quaternion & (Quaternion::*)(const AngleAxis &))&Quaternion::operator=,
bp::arg("aa"),"Set *this from an angle-axis aa and returns a reference to *this.",bp::return_self<>())
.def("__str__",&print)
.def("__repr__",&print)
.def("FromTwoVectors",&Quaternion::template FromTwoVectors<Vector3,Vector3>,
bp::args("a","b"),
"Returns the quaternion which transform a into b through a rotation.")
.staticmethod("FromTwoVectors")
.def("Identity",&Quaternion::Identity,"Returns a quaternion representing an identity rotation.")
.staticmethod("Identity")
.def("setFromTwoVectors",&QuaternionVisitor::setFromTwoVectors,((bp::arg("u"),bp::arg("v"))))
.def("conjugate",&QuaternionUnaligned::conjugate)
.def("inverse",&QuaternionUnaligned::inverse)
.def("norm",&QuaternionUnaligned::norm)
.def("normalize",&QuaternionUnaligned::normalize)
.def("normalized",&QuaternionUnaligned::normalized)
.def("apply",&QuaternionUnaligned::_transformVector)
/* --- Operators --- */
.def(bp::self * bp::self)
.def(bp::self *= bp::self)
.def(bp::self * bp::other<Vector3>())
.def("__eq__",&QuaternionVisitor::__eq__)
.def("__ne__",&QuaternionVisitor::__ne__)
.def("__abs__",&QuaternionUnaligned::norm)
.def("__len__",&QuaternionVisitor::__len__).staticmethod("__len__")
.def("__setitem__",&QuaternionVisitor::__setitem__)
.def("__getitem__",&QuaternionVisitor::__getitem__)
;
;
}
private:
static QuaternionUnaligned* fromTwoVectors(const Vector3& u, const Vector3& v)
template<int i>
static void setCoeff(Quaternion & self, Scalar value) { self.coeffs()[i] = value; }
static Quaternion & setFromTwoVectors(Quaternion & self, const Vector3 & a, const Vector3 & b)
{ return self.setFromTwoVectors(a,b); }
template<typename OtherQuat>
static Quaternion & assign(Quaternion & self, const OtherQuat & quat)
{ return self = quat; }
static Quaternion* fromTwoVectors(const Vector3& u, const Vector3& v)
{
QuaternionUnaligned* q(new QuaternionUnaligned); q->setFromTwoVectors(u,v);
Quaternion* q(new Quaternion); q->setFromTwoVectors(u,v);
return q;
}
static Coefficients coeffs(const QuaternionUnaligned& self)
{
return self.coeffs();
}
static void setFromTwoVectors(QuaternionUnaligned& self, const Vector3& u, const Vector3& v)
{
self.setFromTwoVectors(u,v);
}
static bool __eq__(const QuaternionUnaligned& u, const QuaternionUnaligned& v)
static bool __eq__(const Quaternion& u, const Quaternion& v)
{
return u.isApprox(v,1e-9);
}
static bool __ne__(const QuaternionUnaligned& u, const QuaternionUnaligned& v)
static bool __ne__(const Quaternion& u, const Quaternion& v)
{
return !__eq__(u,v);
}
static double __getitem__(const QuaternionUnaligned & self, int idx)
static Scalar __getitem__(const Quaternion & self, int idx)
{
if((idx<0) || (idx>4)) throw eigenpy::ExceptionIndex(idx,0,4);
if(idx==0) return self.x();
else if(idx==1) return self.y();
else if(idx==2) return self.z();
else return self.w();
if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
return self.coeffs()[idx];
}
static void __setitem__(QuaternionUnaligned& self, int idx, double value)
static void __setitem__(Quaternion& self, int idx, const Scalar value)
{
if((idx<0) || (idx>4)) throw eigenpy::ExceptionIndex(idx,0,4);
if(idx==0) self.x()=value;
else if(idx==1) self.y()=value;
else if(idx==2) self.z()=value;
else self.w()=value;
if((idx<0) || (idx>=4)) throw eigenpy::ExceptionIndex(idx,0,3);
self.coeffs()[idx] = value;
}
static int __len__() { return 4; }
static int __len__() { return 4; }
static Vector3 vec(const Quaternion & self) { return self.vec(); }
static std::string print(const Quaternion & self)
{
std::stringstream ss;
ss << "(x,y,z,w) = " << self.coeffs().transpose() << std::endl;
return ss.str();
}
public:
static void expose()
{
bp::class_<QuaternionUnaligned>("Quaternion",
"Quaternion representing rotation.\n\n"
"Supported operations "
"('q is a Quaternion, 'v' is a Vector3): "
"'q*q' (rotation composition), "
"'q*=q', "
"'q*v' (rotating 'v' by 'q'), "
"'q==q', 'q!=q', 'q[0..3]'.",
bp::init<>())
.def(QuaternionVisitor<Quaternion>())
;
bp::class_<Quaternion>("Quaternion",
"Quaternion representing rotation.\n\n"
"Supported operations "
"('q is a Quaternion, 'v' is a Vector3): "
"'q*q' (rotation composition), "
"'q*=q', "
"'q*v' (rotating 'v' by 'q'), "
"'q==q', 'q!=q', 'q[0..3]'.",
bp::no_init)
.def(QuaternionVisitor<Quaternion>())
;
#ifndef EIGENPY_ALIGNED
bp::to_python_converter< Quaternion,QuaternionVisitor<Quaternion> >();
#endif
}
};
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment