Merge branch 'topic/AddGettersForControlPointsOfCurves' into 'devel'

Topic / add functions to get waypoints of bezier and coeff of polynomial (for python)

See merge request loco-3d/curves!21

cmake @ cea261e3

-Subproject commit f389aae203c4d92649cd5eb66289fd6a17c03fde
+Subproject commit cea261e3da7d383844530070356bca76d20197a8

include/curves/bezier_curve.h

@@ -265,6 +265,16 @@ namespace curves
 
       const t_point_t& waypoints() const {return control_points_;}
 
+      const point_t waypointAtIndex(const std::size_t index) const
+      {
+        point_t waypoint;
+        if (index<=control_points_.size())
+        {
+          waypoint = control_points_[index];
+        }
+        return waypoint;
+      }
+
       /// \brief Evaluate the curve value at time t using deCasteljau algorithm.
       /// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control points 
       /// of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier

include/curves/polynomial.h

@@ -319,11 +319,26 @@ namespace curves
         return deriv.compute_derivate(order-1);
       }
 
+      Eigen::MatrixXd coeff() const
+      {
+        return coefficients_;
+      }
+
+      point_t coeffAtDegree(const std::size_t degree) const
+      {
+        point_t res;
+        if (degree <= degree_)
+        {
+          res = coefficients_.col(degree);
+        }
+        return res;
+      }
+
     private:
       num_t fact(const std::size_t n, const std::size_t order) const
       {
         num_t res(1);
-        for(std::size_t i = 0; i < order; ++i)
+        for(std::size_t i = 0; i < std::size_t(order); ++i)
         {
           res *= (num_t)(n-i);
         }

python/curves_python.cpp

@@ -98,7 +98,7 @@ namespace curves
   bezier_t* wrapBezierConstructorBoundsConstraints(const pointX_list_t& array, const curve_constraints_t& constraints,
                                                    const real T_min, const real T_max)
   {
-  return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(array, constraints, 
+    return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(array, constraints, 
                                                                                                             T_min, T_max) ;
   }
   /*END 3D constructors bezier */
@@ -316,6 +316,7 @@ namespace curves
       .def("derivate", &bezier3_t::derivate)
       .def("compute_derivate", &bezier3_t::compute_derivate)
       .def("compute_primitive", &bezier3_t::compute_primitive)
+      .def("waypointAtIndex", &bezier3_t::waypointAtIndex)
       .def("saveAsText", &bezier3_t::saveAsText<bezier3_t>,bp::args("filename"),"Saves *this inside a text file.")
       .def("loadFromText",&bezier3_t::loadFromText<bezier3_t>,bp::args("filename"),"Loads *this from a text file.")
       .def("saveAsXML",&bezier3_t::saveAsXML<bezier3_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
@@ -340,6 +341,7 @@ namespace curves
       .def("derivate", &bezier_t::derivate)
       .def("compute_derivate", &bezier_t::compute_derivate)
       .def("compute_primitive", &bezier_t::compute_primitive)
+      .def("waypointAtIndex", &bezier_t::waypointAtIndex)
       .def("saveAsText", &bezier_t::saveAsText<bezier_t>,bp::args("filename"),"Saves *this inside a text file.")
       .def("loadFromText",&bezier_t::loadFromText<bezier_t>,bp::args("filename"),"Loads *this from a text file.")
       .def("saveAsXML",&bezier_t::saveAsXML<bezier_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
@@ -407,6 +409,8 @@ namespace curves
       .def("min", &polynomial_t::min, "Get the LOWER bound on interval definition of the curve.")
       .def("max", &polynomial_t::max,"Get the HIGHER bound on interval definition of the curve.")
       .def("dim", &polynomial_t::dim)
+      .def("coeffAtDegree", &polynomial_t::coeffAtDegree)
+      .def("coeff", &polynomial_t::coeff)
       .def("__call__", &polynomial_t::operator(),"Evaluate the spline at the given time.")
       .def("derivate", &polynomial_t::derivate,"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
       .def("compute_derivate", &polynomial_t::compute_derivate,"Compute derivative of order N of curve at time t.")

python/test/test.py

@@ -36,6 +36,14 @@ class TestCurves(unittest.TestCase):
         time_waypoints = matrix([0., 1.]).transpose()
         # Create bezier6 and bezier
         a = bezier(waypoints, 0., 3.)
+        # Test waypoints
+        self.assertTrue (a.nbWaypoints == 2)
+        for i in range(0, a.nbWaypoints):
+            if i==0:
+                self.assertTrue ((a.waypointAtIndex(0) == matrix([1., 2., 3.]).transpose()).all())
+            elif i==1:
+                self.assertTrue ((a.waypointAtIndex(1) == matrix([4., 5., 6.]).transpose()).all())
+        #self.assertTrue((a.waypoints == waypoints).all())
         # Test : Degree, min, max, derivate
         #self.print_str(("test 1")
         self.assertEqual(a.degree, a.nbWaypoints - 1)
@@ -108,13 +116,17 @@ class TestCurves(unittest.TestCase):
         print("test_polynomial")
         # To test :
         # - Functions : constructor, min, max, derivate, serialize, deserialize
-        waypoints_0 = matrix([[0.,0.,0.], [0.,0.,0.]]).transpose()
         waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
         a = polynomial(waypoints)  # Defined on [0.,1.]
         a = polynomial(waypoints, -1., 3.)  # Defined on [-1.,3.]
         a.min()
         a.max()
         a(0.4)
+        # Test get coefficient at degree
+        self.assertTrue((a.coeff()==waypoints).all())
+        self.assertTrue((a.coeffAtDegree(0) == matrix([1., 2., 3.]).transpose()).all())
+        self.assertTrue((a.coeffAtDegree(1) == matrix([4., 5., 6.]).transpose()).all())
+        # Other tests
         self.assertTrue ((a(a.min()) == matrix([1., 2., 3.]).transpose()).all())
         self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
         a.derivate(0.4, 2)