bezier to polynom conversion routine

include/spline/bezier_curve.h

@@ -253,10 +253,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
 /*Helpers*/
 
 	public:
-    const time_t minBound_, maxBound_;
-    const std::size_t size_;
-    const std::size_t degree_;
-    const std::vector<Bern<Numeric> > bernstein_;
+    /*const*/ time_t minBound_, maxBound_;
+    /*const*/ std::size_t size_;
+    /*const*/ std::size_t degree_;
+    /*const*/ std::vector<Bern<Numeric> > bernstein_;
 	
     private:
     t_point_t  pts_;

include/spline/bezier_polynom_conversion.h

+/**
+* \file bezier_curve.h
+* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
+* \author Steve T.
+* \version 0.1
+* \date 06/17/2013
+*/
+
+
+#ifndef _BEZIER_POLY_CONVERSION
+#define _BEZIER_POLY_CONVERSION
+
+#include "curve_abc.h"
+#include "bernstein.h"
+#include "curve_constraint.h"
+
+#include "MathDefs.h"
+
+#include <vector>
+#include <stdexcept>
+
+#include <iostream>
+
+namespace spline
+{
+/// \brief Provides methods for converting a curve from a bernstein representation
+/// to a polynom representation
+///
+///
+///
+
+
+///\brief Converts a Bezier curve to a polynom
+///\param bezier: the Bezier curve to be converted from
+///\return the equivalent polynom
+template<typename Bezier, typename Polynom>
+Polynom from_bezier(const Bezier& curve)
+{
+    typedef typename Polynom::t_point_t    t_point_t;
+    typedef typename Polynom::num_t    num_t;
+    assert (curve.min() == 0.);
+    assert (curve.max() == 1.);
+    t_point_t coefficients;
+    Bezier current (curve);
+    coefficients.push_back(curve(0.));
+    num_t fact = 1;
+    for(std::size_t i = 1; i<= curve.degree_; ++i)
+    {
+        current = current.compute_derivate(1);
+        fact *= i;
+        coefficients.push_back(current(0.)/fact);
+    }
+    return Polynom(coefficients,curve.min(),curve.max());
+}
+
+///\brief Converts a polynom to a Bezier curve
+///\param polynom: the polynom to be converted from
+///\return the equivalent Bezier curve
+/*template<typename Bezier, typename Polynom>
+Bezier from_polynom(const Polynom& polynom)
+{
+    typedef Bezier::point_t 	point_t;
+    typedef Bezier::time_t 	time_t;
+    typedef Bezier::num_t	num_t;
+    typedef Bezier::curve_constraints_t    curve_constraints_t;
+    typedef Bezier::t_point_t    t_point_t;
+    typedef Bezier::cit_point_t    cit_point_t;
+    typedef Bezier::bezier_curve_t   bezier_curve_t;
+}*/
+}
+#endif //_BEZIER_POLY_CONVERSION
+

include/spline/polynom.h

@@ -35,6 +35,7 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
 struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
 {
     typedef Point 	point_t;
+    typedef T_Point t_point_t;
     typedef Time 	time_t;
     typedef Numeric	num_t;
     typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;

python/spline_python.cpp

@@ -3,6 +3,8 @@
 #include "spline/exact_cubic.h"
 #include "spline/spline_deriv_constraint.h"
 #include "spline/curve_constraint.h"
+#include "spline/bezier_polynom_conversion.h"
+
 
 #include <vector>
 
@@ -300,6 +302,10 @@ BOOST_PYTHON_MODULE(spline)
         ;
     /** END spline_deriv_constraints**/
 
+    /** BEGIN Bezier to polynom conversion**/
+    def("from_bezier", from_bezier<bezier_t,polynom_t>);
+    /** END Bezier to polynom conversion**/
+
 
 }
 

python/test/test.py

-from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint
+from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier
 
 from numpy import matrix
 from numpy.linalg import norm
 
+__EPS = 1e-6
+
 waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
 waypoints6 = matrix([[1.,2.,3.,7.,5.,5.],[4.,5.,6.,4.,5.,6.]]).transpose()
 time_waypoints = matrix([0.,1.])
@@ -78,3 +80,9 @@ c.end_acc  = matrix([0.,1.,1.]);
 
 a = spline_deriv_constraint (waypoints, time_waypoints)
 a = spline_deriv_constraint (waypoints, time_waypoints, c)
+
+#converting bezier to polynom
+
+a = bezier(waypoints)
+a_pol = from_bezier(a)
+assert norm(a(0.3) - a_pol(0.3)) < __EPS

src/tests/spline_test/Main.cpp

@@ -5,6 +5,7 @@
 #include "spline/spline_deriv_constraint.h"
 #include "spline/helpers/effector_spline.h"
 #include "spline/helpers/effector_spline_rotation.h"
+#include "spline/bezier_polynom_conversion.h"
 
 #include <string>
 #include <iostream>
@@ -377,6 +378,41 @@ void BezierDerivativeCurveConstraintTest(bool& error)
 }
 
 
+void BezierToPolynomConversionTest(bool& error)
+{
+    std::string errMsg("In test BezierToPolynomConversionTest ; unexpected result for x ");
+    point_t a(1,2,3);
+    point_t b(2,3,4);
+    point_t c(3,4,5);
+    point_t d(3,6,7);
+    point_t e(3,61,7);
+    point_t f(3,56,7);
+    point_t g(3,36,7);
+    point_t h(43,6,7);
+    point_t i(3,6,77);
+
+    std::vector<point_t> params;
+    params.push_back(a);
+    params.push_back(b);
+    params.push_back(c);
+    params.push_back(d);
+    params.push_back(e);
+    params.push_back(f);
+    params.push_back(g);
+    params.push_back(h);
+    params.push_back(i);
+
+    bezier_curve_t cf(params.begin(), params.end());
+    polynom_t pol =from_bezier<bezier_curve_t, polynom_t>(cf);
+    for(double i =0.; i<1.; i+=0.01)
+    {
+        std::cout << "START " << cf(i).transpose() << std::endl;
+        std::cout << pol(i).transpose() << "END " << std::endl;
+        ComparePoints(cf(i),pol(i),errMsg, error, true);
+        ComparePoints(cf(i),pol(i),errMsg, error, false);
+    }
+}
+
 /*Exact Cubic Function tests*/
 void ExactCubicNoErrorTest(bool& error)
 {
@@ -714,7 +750,7 @@ int main(int /*argc*/, char** /*argv[]*/)
 {
     std::cout << "performing tests... \n";
     bool error = false;
-    /*CubicFunctionTest(error);
+   /* CubicFunctionTest(error);
     ExactCubicNoErrorTest(error);
     ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
     ExactCubicTwoPointsTest(error);
@@ -727,8 +763,9 @@ int main(int /*argc*/, char** /*argv[]*/)
     EffectorSplineRotationWayPointRotationTest(error);
     BezierCurveTest(error);
     BezierDerivativeCurveTest(error);
-    BezierDerivativeCurveConstraintTest(error);*/
-    BezierCurveTestCompareHornerAndBernstein(error);
+    BezierDerivativeCurveConstraintTest(error);
+    //BezierCurveTestCompareHornerAndBernstein(error);*/
+    BezierToPolynomConversionTest(error);
 	if(error)
 	{
         std::cout << "There were some errors\n";