[piecewise_curve] Add conversion function from discrete points, Test OK

include/curves/piecewise_curve.h

@@ -180,6 +180,54 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>
         return pc_res;
     }
 
+    template<typename Polynomial>
+    static piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> 
+    convert_discrete_points_to_polynomial(T_Point points, Time T_min, Time T_max)
+    {
+        if(Safe &! (points.size()>1))
+        {
+            //std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
+            throw std::invalid_argument("piecewise_curve -> convert_discrete_points_to_polynomial, Error, less than 2 discrete points");
+        }
+        typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
+        Time discretization_step = (T_max-T_min)/(points.size()-1);
+        Time time_actual = T_min;
+        // Initialization at first points
+        point_t actual_point = points[0];
+        point_t next_point = points[1];
+        point_t coeff_order_zero(actual_point);
+        point_t coeff_order_one((next_point-actual_point)/discretization_step);
+        t_point_t coeffs;
+        coeffs.push_back(coeff_order_zero);
+        coeffs.push_back(coeff_order_one);
+        Polynomial pol(coeffs,time_actual,time_actual+discretization_step);
+        piecewise_curve_out_t ppc(pol);
+        time_actual += discretization_step;
+        // Other points
+        for (int i=1; i<points.size()-2; i++)
+        {
+            coeffs.clear();
+            actual_point = points[i];
+            next_point = points[i+1];
+            coeff_order_zero = actual_point;
+            coeff_order_one = (next_point-actual_point)/discretization_step;
+            coeffs.push_back(coeff_order_zero);
+            coeffs.push_back(coeff_order_one);
+            ppc.add_curve(Polynomial(coeffs,time_actual,time_actual+discretization_step));
+            time_actual += discretization_step;
+        }
+        // Last points
+        coeffs.clear();
+        actual_point = points[points.size()-2];
+        next_point = points[points.size()-1];
+        coeff_order_zero = actual_point;
+        coeff_order_one = (next_point-actual_point)/discretization_step;
+        coeffs.push_back(coeff_order_zero);
+        coeffs.push_back(coeff_order_one);
+        ppc.add_curve(Polynomial(coeffs,time_actual,T_max));
+        return ppc;
+    }
+
     private:
 
     /// \brief Get index of the interval corresponding to time t for the interpolation.

tests/Main.cpp

@@ -1392,6 +1392,35 @@ void curveAbcDimDynamicTest(bool& error)
     
 }
 
+void piecewiseCurveConversionFromDiscretePointsTest(bool& error)
+{
+    std::string errMsg("piecewiseCurveConversionFromDiscretePointsTest, Error, value on curve is wrong : ");
+    point_t p0(0.,0.,0.);
+    point_t p1(1.,2.,3.);
+    point_t p2(4.,4.,4.);
+    point_t p3(10.,10.,10.);
+    point_t p_test_0_5 = (p0+p1)/2.0;
+    t_point_t points;
+    points.push_back(p0);
+    points.push_back(p1);
+    points.push_back(p2);
+    points.push_back(p3);
+    double T_min = 1.0;
+    double T_max = 3.0;
+    double timestep = (T_max-T_min)/(points.size()-1);
+    piecewise_polynomial_curve_t ppc =  piecewise_polynomial_curve_t::
+                                        convert_discrete_points_to_polynomial<polynomial_t>(points,T_min,T_max);
+    if (!ppc.is_continuous(0))
+    {
+        std::cout<<"piecewiseCurveConversionFromDiscretePointsTest, Error, piecewise curve is not C0"<<std::endl;
+        error = true;
+    }
+    ComparePoints(p0, ppc(T_min), errMsg, error);
+    ComparePoints(p_test_0_5, ppc(T_min+timestep/2.0), errMsg, error);
+    ComparePoints(p1, ppc(T_min+timestep), errMsg, error);
+    ComparePoints(p2, ppc(T_min+2*timestep), errMsg, error);
+    ComparePoints(p3, ppc(T_max), errMsg, error);
+
 int main(int /*argc*/, char** /*argv[]*/)
 {
     std::cout << "performing tests... \n";
@@ -1417,6 +1446,7 @@ int main(int /*argc*/, char** /*argv[]*/)
     BezierSplitCurve(error);
     CubicHermitePairsPositionDerivativeTest(error);
     piecewiseCurveTest(error);
+    piecewiseCurveConversionFromDiscretePointsTest(error);
     toPolynomialConversionTest(error);
     cubicConversionTest(error);
     curveAbcDimDynamicTest(error);