diff --git a/src/quaternion.hpp b/src/quaternion.hpp
index 97c53d0a632f1a2dc0e1019686198a0515ee7bae..52c68cac23267895b714baa99549bd425ece50eb 100644
--- a/src/quaternion.hpp
+++ b/src/quaternion.hpp
@@ -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
}
};