[BUG FIX] bezier derivative + integrate method

include/spline/bezier_curve.h

@@ -18,6 +18,8 @@
 #include <vector>
 #include <stdexcept>
 
+#include <iostream>
+
 namespace spline
 {
 /// \class BezierCurve
@@ -115,15 +117,39 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
     bezier_curve_t compute_derivate(const std::size_t order) const
     {
         if(order == 0) return *this;
+        num_t degree = (num_t)(size_-1);
         t_point_t derived_wp;
         for(typename t_point_t::const_iterator pit =  pts_.begin(); pit != pts_.end()-1; ++pit)
-            derived_wp.push_back(*(pit+1) - (*pit));
+            derived_wp.push_back(degree * (*(pit+1) - (*pit)));
         if(derived_wp.empty())
             derived_wp.push_back(point_t::Zero());
         bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),minBound_,maxBound_);
+        assert(deriv.size_ +1 == this->size_);
         return deriv.compute_derivate(order-1);
     }
 
+    ///  \brief Computes the primitive of the curve at order N.
+    ///  \param constant : value of the primitive at t = 0
+    ///  \param return : the curve x_1(t) such that d/dt(x_1(t)) = x_1(t)
+    bezier_curve_t compute_primitive(const std::size_t order) const
+    {
+        if(order == 0) return *this;
+        num_t new_degree = (num_t)(size_);
+        t_point_t n_wp;
+        point_t current_sum =  point_t::Zero();
+        // recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
+        // then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
+        n_wp.push_back(current_sum);
+        for(typename t_point_t::const_iterator pit =  pts_.begin(); pit != pts_.end(); ++pit)
+        {
+            current_sum += *pit;
+            n_wp.push_back(current_sum / new_degree);
+        }
+        bezier_curve_t integ(n_wp.begin(), n_wp.end(),minBound_,maxBound_);
+        assert(integ.size_== this->size_ +1 );
+        return integ.compute_primitive(order-1);
+    }
+
     ///  \brief Evaluates the derivative at order N of the curve.
     ///  If the derivative is to be evaluated several times, it is
     ///  rather recommended to compute the derivative curve using compute_derivate

python/spline_python.cpp

@@ -161,6 +161,7 @@ BOOST_PYTHON_MODULE(spline)
             .def("__call__", &bezier_t::operator())
             .def("derivate", &bezier_t::derivate)
             .def("compute_derivate", &bezier_t::compute_derivate)
+            .def("compute_primitive", &bezier_t::compute_primitive)
         ;
     /** END bezier curve**/
 

python/test/test.py

@@ -6,16 +6,30 @@ waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
 time_waypoints = matrix([0.,1.])
 
 #testing bezier curve
-a = bezier(waypoints)
 a = bezier(waypoints, -1., 3.)
+
+a = bezier(waypoints)
 a.min()
 a.max()
 a(0.4)
 assert((a.derivate(0.4,0) == a(0.4)).all())
 a.derivate(0.4,2)
-
 a = a.compute_derivate(100)
 
+prim = a.compute_primitive(1)
+
+for i in range(10):
+	t = float(i) / 10.
+	assert(a(t) == prim.derivate(t,1)).all()
+assert(prim(0) == matrix([0.,0.,0.])).all()
+
+prim = a.compute_primitive(2)
+for i in range(10):
+	t = float(i) / 10.
+	assert(a(t) == prim.derivate(t,2)).all()
+assert(prim(0) == matrix([0.,0.,0.])).all()
+
+
 
 #testing spline function
 a = spline(waypoints)