Commit 2b906c02 authored by Steve Tonneau's avatar Steve Tonneau

[BUG FIX] bezier derivative + integrate method

parent a1d9c6f5
......@@ -18,6 +18,8 @@
#include <vector>
#include <stdexcept>
#include <iostream>
namespace spline
{
/// \class BezierCurve
......@@ -115,15 +117,39 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
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!
Please register or to comment