diff --git a/include/spline/bezier_curve.h b/include/spline/bezier_curve.h
index dfdc94e058061eb6528d37b46f8a87382bfb3989..163e1d3addb41ddbf36899fd29117a7d7cd5487e 100644
--- a/include/spline/bezier_curve.h
+++ b/include/spline/bezier_curve.h
@@ -258,6 +258,82 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
const t_point_t& waypoints() const {return pts_;}
+
+ /**
+ * @brief evalDeCasteljau evaluate the curve value at time t using deCasteljau algorithm
+ * @param t unNormalized time
+ * @return the point at time t
+ */
+ point_t evalDeCasteljau(const Numeric T) const {
+ // normalize time :
+ const Numeric t = T/T_;
+ t_point_t pts = deCasteljauReduction(waypoints(),t);
+ while(pts.size() > 1){
+ pts = deCasteljauReduction(pts,t);
+ }
+ return pts[0]*mult_T_;
+ }
+
+ t_point_t deCasteljauReduction(const Numeric t) const{
+ return deCasteljauReduction(waypoints(),t/T_);
+ }
+
+ /**
+ * @brief deCasteljauReduction compute the de Casteljau's reduction of the given list of points at time t
+ * @param pts the original list of points
+ * @param t the NORMALIZED time
+ * @return the reduced list of point (size of pts - 1)
+ */
+ t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric t) const{
+ if(t < 0 || t > 1)
+ throw std::out_of_range("In deCasteljau reduction : t is not in [0;1]");
+ if(pts.size() == 1)
+ return pts;
+
+ t_point_t new_pts;
+ for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit){
+ new_pts.push_back((1-t) * (*cit) + t*(*(cit+1)));
+ }
+ return new_pts;
+ }
+
+
+ /**
+ * @brief split split the curve in 2 at time t
+ * @param t
+ * @return
+ */
+ std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric T){
+ t_point_t wps_first(size_),wps_second(size_);
+ const double t = T/T_;
+ wps_first[0] = pts_.front();
+ wps_second[degree_] = pts_.back();
+ t_point_t casteljau_pts = waypoints();
+ size_t id = 1;
+ while(casteljau_pts.size() > 1){
+ casteljau_pts = deCasteljauReduction(casteljau_pts,t);
+ wps_first[id] = casteljau_pts.front();
+ wps_second[degree_-id] = casteljau_pts.back();
+ ++id;
+ }
+
+ bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T,mult_T_);
+ bezier_curve_t c_second(wps_second.begin(), wps_second.end(), T_-T,mult_T_);
+ return std::make_pair(c_first,c_second);
+ }
+
+ bezier_curve_t extract(const Numeric t1, const Numeric t2){
+ if(t1 < 0. || t1 > T_ || t2 < 0. || t2 > T_)
+ throw std::out_of_range("In Extract curve : times out of bounds");
+ if(t1 == 0.)
+ return split(t2).first;
+ if(t2 == T_)
+ return split(t1).second;
+
+ std::pair<bezier_curve_t,bezier_curve_t> c_split = this->split(t1);
+ return c_split.second->split(t2).first;
+ }
+
private:
template<typename In>
t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index ddb35e1049a0409c26b9edef61aa46adcb8b2c8f..686e8f6ab631d7bce4834e0e931a5e8bf50b39f3 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -255,7 +255,7 @@ void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
// 3d curve
bezier_curve_t cf(params.begin(), params.end());
- clock_t s0,e0,s1,e1,s2,e2;
+ clock_t s0,e0,s1,e1,s2,e2,s3,e3;
s0 = clock();
for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
{
@@ -277,12 +277,18 @@ void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
}
e2 = clock();
-
- std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
+ s3 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf.evalDeCasteljau(*cit);
+ }
+ e3 = clock();
+ std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
std::cout << "now with high order polynom " << std::endl;
@@ -317,10 +323,19 @@ void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
}
e0 = clock();
+ s3 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf2.evalDeCasteljau(*cit);
+ }
+ e3 = clock();
+
+
- std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
}
@@ -768,6 +783,143 @@ void TestReparametrization(bool& error)
}
}
+point_t randomPoint(const double min, const double max ){
+ point_t p;
+ for(size_t i = 0 ; i < 3 ; ++i)
+ p[i] = (rand()/(double)RAND_MAX ) * (max-min) + min;
+ return p;
+}
+
+void BezierEvalDeCasteljau(bool& error){
+ using namespace std;
+ std::vector<double> values;
+ for (int i =0; i < 100000; ++i)
+ values.push_back(rand()/RAND_MAX);
+
+ //first compare regular evaluation (low dim pol)
+ 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);
+
+ // 3d curve
+ bezier_curve_t cf(params.begin(), params.end());
+
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ if(cf.evalDeCasteljau(*cit) != cf(*cit)){
+ error = true;
+ std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
+ }
+ }
+
+ 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 cf2(params.begin(), params.end());
+
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ if(cf.evalDeCasteljau(*cit) != cf(*cit)){
+ error = true;
+ std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
+ }
+ }
+
+}
+
+
+/**
+ * @brief BezierSplitCurve test the 'split' method of bezier curve
+ * @param error
+ */
+void BezierSplitCurve(bool& error){
+ // test for degree 5
+ int n = 5;
+ double t_min = 0.2;
+ double t_max = 10;
+ for(size_t i = 0 ; i < 1 ; ++i){
+ // build a random curve and split it at random time :
+ //std::cout<<"build a random curve"<<std::endl;
+ point_t a;
+ std::vector<point_t> wps;
+ for(size_t j = 0 ; j <= n ; ++j){
+ wps.push_back(randomPoint(-10.,10.));
+ }
+ double t = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
+ double ts = (rand()/(double)RAND_MAX )*(t);
+
+ bezier_curve_t c(wps.begin(), wps.end(),t);
+ std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
+ //std::cout<<"split curve of duration "<<t<<" at "<<ts<<std::endl;
+
+ // test on splitted curves :
+
+ if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) )){
+ error = true;
+ std::cout<<" Degree of the splitted curve are not the same as the original curve"<<std::endl;
+ }
+
+ if(c.max() != (cs.first.max() + cs.second.max())){
+ error = true;
+ std::cout<<"Duration of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ if(c(0) != cs.first(0)){
+ error = true;
+ std::cout<<"initial point of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ if(c(t) != cs.second(cs.second.max())){
+ error = true;
+ std::cout<<"final point of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ if(cs.first.max() != ts){
+ error = true;
+ std::cout<<"timing of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ if(cs.first(ts) != cs.second(0.)){
+ error = true;
+ std::cout<<"splitting point of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ // check along curve :
+ double ti = 0.;
+ while(ti <= ts){
+ if((cs.first(ti) - c(ti)).norm() > 1e-14){
+ error = true;
+ std::cout<<"first splitted curve and original curve doesn't correspond, error = "<<cs.first(ti) - c(ti) <<std::endl;
+ }
+ ti += 0.01;
+ }
+ while(ti <= t){
+ if((cs.second(ti-ts) - c(ti)).norm() > 1e-14){
+ error = true;
+ std::cout<<"second splitted curve and original curve doesn't correspond, error = "<<cs.second(ti-ts) - c(ti)<<std::endl;
+ }
+ ti += 0.01;
+ }
+ }
+}
+
+
+
int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "performing tests... \n";
@@ -789,7 +941,9 @@ int main(int /*argc*/, char** /*argv[]*/)
BezierCurveTestCompareHornerAndBernstein(error);
BezierDerivativeCurveTimeReparametrizationTest(error);
BezierToPolynomConversionTest(error);
- if(error)
+ BezierEvalDeCasteljau(error);
+ BezierSplitCurve(error);
+ if(error)
{
std::cout << "There were some errors\n";
return -1;