Commit 3005c737 authored by Steve Tonneau's avatar Steve Tonneau

added derivations for bezier curves

parent 4d962ec0
......@@ -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
///
......
......@@ -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*/
......
......@@ -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)
......
......@@ -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_);
......
......@@ -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";
......
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