added derivations for bezier curves

include/spline/bezier_curve.h

@@ -27,12 +27,13 @@ namespace spline
 ///
 template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
 , typename Point= Eigen::Matrix<Numeric, Dim, 1> >
-struct bezier_curve : public  curve_abc<Time, Numeric, Dim, Safe, Point>
+struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
 {
 	typedef Point 	point_t;
 	typedef Time 	time_t;
     typedef Numeric	num_t;
     typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
+    typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
 
 /* Constructors - destructors */
 	public:
@@ -48,7 +49,7 @@ struct bezier_curve : public  curve_abc<Time, Numeric, Dim, Safe, Point>
 	{
         assert(bernstein_.size() == size_);
 		In it(PointsBegin);
-		if(Safe && (size_<=1 || minBound == maxBound))
+        if(Safe && (size_<1 || minBound >= maxBound))
 		{
             throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
 		}
@@ -85,7 +86,9 @@ struct bezier_curve : public  curve_abc<Time, Numeric, Dim, Safe, Point>
 		{
 			num_t dt = (1 - nT);
 			switch(size_)
-			{	
+            {
+                case 1 :
+                    return pts_[0];
 				case 2 :
 					return pts_[0] * dt +  nT * pts_[1];
 				break;
@@ -106,6 +109,33 @@ struct bezier_curve : public  curve_abc<Time, Numeric, Dim, Safe, Point>
 		}
 	}
 
+    ///  \brief Computes the derivative curve at order N.
+    ///  \param order : order of the derivative
+    ///  \param return : the value x(t)
+    bezier_curve_t compute_derivate(const std::size_t order) const
+    {
+        if(order == 0) return *this;
+        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));
+        if(derived_wp.empty())
+            derived_wp.push_back(point_t::Zero());
+        bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),minBound_,maxBound_);
+        return deriv.compute_derivate(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
+    ///  \param order : order of the derivative
+    ///  \param t : the time when to evaluate the spine
+    ///  \param return : the value x(t)
+    virtual point_t     derivate(const time_t t, const std::size_t order) const
+    {
+        bezier_curve_t deriv =compute_derivate(order);
+        return deriv(t);
+    }
+
     ///
     /// \brief Evaluates all Bernstein polynomes for a certain degree
     ///

include/spline/curve_abc.h

@@ -43,6 +43,13 @@ struct  curve_abc : std::unary_function<Time, Point>
 	///  \param t : the time when to evaluate the spine
 	///  \param return : the value x(t)
     virtual point_t operator()(const time_t t) const = 0;
+
+
+    ///  \brief Evaluation of the derivative spline at time t.
+    ///  \param t : the time when to evaluate the spline
+    ///  \param order : order of the derivative
+    ///  \param return : the value x(t)
+    virtual point_t derivate(const time_t t, const std::size_t order) const = 0;
 /*Operations*/
 
 /*Helpers*/

include/spline/exact_cubic.h

@@ -172,8 +172,9 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
 
     ///  \brief Evaluation of the derivative spline at time t.
     ///  \param t : the time when to evaluate the spline
+    ///  \param order : order of the derivative
     ///  \param return : the value x(t)
-    virtual point_t derivate(time_t t, std::size_t order) const
+    virtual point_t derivate(const time_t t, const std::size_t order) const
     {
         if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
         for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)

include/spline/spline_curve.h

@@ -130,9 +130,10 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
 
 
     ///  \brief Evaluation of the derivative spline at time t.
-    ///  \param t : the time when to evaluate the spine
+    ///  \param t : the time when to evaluate the spline
+    ///  \param order : order of the derivative
     ///  \param return : the value x(t)
-    point_t derivate(time_t t, std::size_t order) const
+    virtual point_t derivate(const time_t t, const std::size_t order) const
     {
         if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
         time_t const dt (t-t_min_);

src/tests/spline_test/Main.cpp

@@ -55,9 +55,9 @@ ostream& operator<<(ostream& os, const point_t& pt)
     return os;
 }
 
-void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const std::string& errmsg, bool& error)
+void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const std::string& errmsg, bool& error, bool notequal = false)
 {
-    if((pt1-pt2).norm() > margin)
+    if((pt1-pt2).norm() > margin && !notequal)
 	{
 		error = true;
         std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
@@ -228,6 +228,27 @@ void BezierCurveTest(bool& error)
 	}
 }
 
+void BezierDerivativeCurveTest(bool& error)
+{
+    std::string errMsg("In test BezierDerivativeCurveTest ; unexpected result for x ");
+    point_t a(1,2,3);
+    point_t b(2,3,4);
+    point_t c(3,4,5);
+
+    std::vector<point_t> params;
+    params.push_back(a);
+    params.push_back(b);
+
+    params.push_back(c);
+    bezier_curve_t cf3(params.begin(), params.end());
+
+    ComparePoints(cf3(0), cf3.derivate(0.,0), errMsg, error);
+    ComparePoints(cf3(0), cf3.derivate(0.,1), errMsg, error, true);
+    ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
+}
+
+
+
 /*Exact Cubic Function tests*/
 void ExactCubicNoErrorTest(bool& error)
 {
@@ -577,6 +598,7 @@ int main(int /*argc*/, char** /*argv[]*/)
     TestReparametrization(error);
     EffectorSplineRotationWayPointRotationTest(error);
     BezierCurveTest(error);
+    BezierDerivativeCurveTest(error);
 	if(error)
 	{
         std::cout << "There were some errors\n";