Commit 2b906c02 by Steve Tonneau

### [BUG FIX] bezier derivative + integrate method

parent a1d9c6f5
 ... ... @@ -18,6 +18,8 @@ #include #include #include namespace spline { /// \class BezierCurve ... ... @@ -115,15 +117,39 @@ struct bezier_curve : public curve_abc bezier_curve_t compute_derivate(const std::size_t order) const { if(order == 0) return *this; num_t degree = (num_t)(size_-1); 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)); derived_wp.push_back(degree * (*(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_); assert(deriv.size_ +1 == this->size_); return deriv.compute_derivate(order-1); } /// \brief Computes the primitive of the curve at order N. /// \param constant : value of the primitive at t = 0 /// \param return : the curve x_1(t) such that d/dt(x_1(t)) = x_1(t) bezier_curve_t compute_primitive(const std::size_t order) const { if(order == 0) return *this; num_t new_degree = (num_t)(size_); t_point_t n_wp; point_t current_sum = point_t::Zero(); // recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant. // then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1 n_wp.push_back(current_sum); for(typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end(); ++pit) { current_sum += *pit; n_wp.push_back(current_sum / new_degree); } bezier_curve_t integ(n_wp.begin(), n_wp.end(),minBound_,maxBound_); assert(integ.size_== this->size_ +1 ); return integ.compute_primitive(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 ... ...
 ... ... @@ -161,6 +161,7 @@ BOOST_PYTHON_MODULE(spline) .def("__call__", &bezier_t::operator()) .def("derivate", &bezier_t::derivate) .def("compute_derivate", &bezier_t::compute_derivate) .def("compute_primitive", &bezier_t::compute_primitive) ; /** END bezier curve**/ ... ...
 ... ... @@ -6,16 +6,30 @@ waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose() time_waypoints = matrix([0.,1.]) #testing bezier curve a = bezier(waypoints) a = bezier(waypoints, -1., 3.) a = bezier(waypoints) a.min() a.max() a(0.4) assert((a.derivate(0.4,0) == a(0.4)).all()) a.derivate(0.4,2) a = a.compute_derivate(100) prim = a.compute_primitive(1) for i in range(10): t = float(i) / 10. assert(a(t) == prim.derivate(t,1)).all() assert(prim(0) == matrix([0.,0.,0.])).all() prim = a.compute_primitive(2) for i in range(10): t = float(i) / 10. assert(a(t) == prim.derivate(t,2)).all() assert(prim(0) == matrix([0.,0.,0.])).all() #testing spline function a = spline(waypoints) ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!