[C++] Update bindings of Quaternion class

- Update API to follow Eigen conventions
- Add Doc
- Remove unaligned conversions

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
     }
 
   };