From 3005c737996cf7196397b3b116f50b01e1652028 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@laas.fr>
Date: Tue, 14 Mar 2017 11:47:36 +0100
Subject: [PATCH] added derivations for bezier curves
---
include/spline/bezier_curve.h | 36 +++++++++++++++++++++++++++++++---
include/spline/curve_abc.h | 7 +++++++
include/spline/exact_cubic.h | 3 ++-
include/spline/spline_curve.h | 5 +++--
src/tests/spline_test/Main.cpp | 26 ++++++++++++++++++++++--
5 files changed, 69 insertions(+), 8 deletions(-)
diff --git a/include/spline/bezier_curve.h b/include/spline/bezier_curve.h
index 703b41e..680352b 100644
--- a/include/spline/bezier_curve.h
+++ b/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
///
diff --git a/include/spline/curve_abc.h b/include/spline/curve_abc.h
index c0bf4e1..f999cc8 100644
--- a/include/spline/curve_abc.h
+++ b/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*/
diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index 10af708..63da338 100644
--- a/include/spline/exact_cubic.h
+++ b/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)
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index 8279c51..369ae2a 100644
--- a/include/spline/spline_curve.h
+++ b/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_);
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 3796947..2f2cd2d 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/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";
--
GitLab