[polynomial] Add compute_derivative function + python binding => Tests OK

include/curves/polynomial.h

@@ -46,6 +46,7 @@ namespace curves
     typedef curve_abc<Time, Numeric, Safe, Point> curve_abc_t;
     typedef Eigen::Matrix<double, Dim, Eigen::Dynamic> coeff_t;
     typedef Eigen::Ref<coeff_t> coeff_t_ref;
+    typedef polynomial<Time, Numeric, Dim, Safe, Point, T_Point> polynomial_t;
 
     /* Constructors - destructors */
     public:
@@ -174,6 +175,17 @@ namespace curves
         return currentPoint_;
       }
 
+      polynomial_t compute_derivate(const std::size_t order) const
+      {
+        if(order == 0) 
+        {
+          return *this;
+        }
+        coeff_t coeff_derivated = deriv_coeff(coefficients_);
+        polynomial_t deriv(coeff_derivated, T_min_, T_max_);
+        return deriv.compute_derivate(order-1);
+      }
+
     private:
       num_t fact(const std::size_t n, const std::size_t order) const
       {
@@ -184,6 +196,15 @@ namespace curves
         }
         return res;
       }
+
+      coeff_t deriv_coeff(coeff_t coeff) const
+      {
+        coeff_t coeff_derivated(coeff.rows(), coeff.cols()-1);
+        for (std::size_t i=0; i<coeff_derivated.cols(); i++) {
+          coeff_derivated.col(i) = coeff.col(i+1)*(i+1);
+        }
+        return coeff_derivated;
+      }
     /*Operations*/
 
     public:
@@ -204,7 +225,6 @@ namespace curves
       /*Attributes*/
 
     private:
-
       template<typename In>
       coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient)
       {

python/curves_python.cpp

@@ -373,6 +373,7 @@ namespace curves
       .def("max", &polynomial_t::max,"Get the HIGHER bound on interval definition of the curve.")
       .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.")
       .def(SerializableVisitor<polynomial_t>())
     ;
 

python/test/test.py

@@ -109,6 +109,8 @@ class TestCurves(unittest.TestCase):
         a(0.4)
         self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
         a.derivate(0.4, 2)
+        a_derivated = a.compute_derivate(1)
+        self.assertTrue((a.derivate(0.4, 1) == a_derivated(0.4)).all())
         # Test serialization
         a.saveAsText("serialization_curve.test")
         b = polynomial()

tests/Main.cpp

@@ -87,7 +87,7 @@ void CompareCurves(curve1 c1, curve2 c2, const std::string& errMsg, bool& error)
 }
 
 /*Cubic Function tests*/
-void CubicFunctionTest(bool& error)
+void PolynomialCubicFunctionTest(bool& error)
 {
   std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
   point_t a(1,2,3);
@@ -154,6 +154,19 @@ void CubicFunctionTest(bool& error)
     error = true;
     std::cout << "Evaluation of cubic cf error, MinBound should be equal to 1\n";
   }
+  // Test derivate and compute_derivative
+  // Order 1
+  polynomial_t cf_derivated = cf.compute_derivate(1);
+  ComparePoints(cf.derivate(0,1), cf_derivated(0), errMsg+" - derivate order 1 : ", error);
+  ComparePoints(cf.derivate(0.3,1), cf_derivated(0.5), errMsg+" - derivate order 1 : ", error);
+  ComparePoints(cf.derivate(0.5,1), cf_derivated(0.5), errMsg+" - derivate order 1 : ", error);
+  ComparePoints(cf.derivate(1,1), cf_derivated(1), errMsg+" - derivate order 1 : ", error);
+  // Order 2
+  polynomial_t cf_derivated_2 = cf.compute_derivate(2);
+  ComparePoints(cf.derivate(0,2), cf_derivated_2(0), errMsg+" - derivate order 1 : ", error);
+  ComparePoints(cf.derivate(0.3,2), cf_derivated_2(0.5), errMsg+" - derivate order 1 : ", error);
+  ComparePoints(cf.derivate(0.5,2), cf_derivated_2(0.5), errMsg+" - derivate order 1 : ", error);
+  ComparePoints(cf.derivate(1,2), cf_derivated_2(1), errMsg+" - derivate order 1 : ", error);
 }
 
 /*bezier_curve Function tests*/
@@ -1425,7 +1438,7 @@ int main(int /*argc*/, char** /*argv[]*/)
 {
   std::cout << "performing tests... \n";
   bool error = false;
-  CubicFunctionTest(error);
+  PolynomialCubicFunctionTest(error);
   ExactCubicNoErrorTest(error);
   ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
   ExactCubicTwoPointsTest(error);