diff --git a/include/curves/CMakeLists.txt b/include/curves/CMakeLists.txt
index a4beb8a5df60991a625d801535d17f483d2545d1..8fefd30f211302e469c04d014d14f40909e44d9b 100644
--- a/include/curves/CMakeLists.txt
+++ b/include/curves/CMakeLists.txt
@@ -11,6 +11,7 @@ SET(${PROJECT_NAME}_HEADERS
quintic_spline.h
linear_variable.h
cubic_hermite_spline.h
+ piecewise_polynomial_curve.h
)
INSTALL(FILES
diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index 5c3cd83e7b90d2ba6b3495f1f5ef08ccf2ad52e4..600a2259ff0fb80945aeecfd31c32333efc2b03b 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -50,7 +50,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd)
- : T_(1.)
+ : T_min_(0.)
+ , T_max_(1.)
, mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
@@ -58,7 +59,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
- if(Safe && (size_<1 || T_ <= 0.))
+ if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::out_of_range("can't create bezier min bound is higher than max bound");
}
@@ -75,8 +76,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param T : upper bound of curve parameter which is between \f$[0;T]\f$ (default \f$[0;1]\f$).
///
template<typename In>
- bezier_curve(In PointsBegin, In PointsEnd, const time_t T)
- : T_(T)
+ bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min, const time_t T_max)
+ : T_min_(T_min)
+ , T_max_(T_max)
, mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
@@ -84,7 +86,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
- if(Safe && (size_<1 || T_ <= 0.))
+ if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
}
@@ -104,8 +106,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param mult_T : ... (default value is 1.0).
///
template<typename In>
- bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T)
- : T_(T)
+ bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min, const time_t T_max, const time_t mult_T)
+ : T_min_(T_min)
+ , T_max_(T_max)
, mult_T_(mult_T)
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
@@ -113,7 +116,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
- if(Safe && (size_<1 || T_ <= 0.))
+ if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
}
@@ -131,14 +134,16 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param constraints : constraints applying on start / end velocities and acceleration.
///
template<typename In>
- bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T=1.)
- : T_(T)
+ bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints,
+ const time_t T_min=0., const time_t T_max=1.)
+ : T_min_(T_min)
+ , T_max_(T_max)
, mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd)+4)
, degree_(size_-1)
, bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
{
- if(Safe && (size_<1 || T_ <= 0.))
+ if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::out_of_range("can't create bezier min bound is higher than max bound");
}
@@ -166,18 +171,18 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const
+ {
+ if(Safe &! (T_min_ <= t && t <= T_max_))
+ {
+ throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
+ }
+ if (size_ == 1)
{
- if(Safe &! (0 <= t && t <= T_))
- {
- throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
- }
- if (size_ == 1)
- {
- return mult_T_*pts_[0];
- }else
- {
- return evalHorner(t);
- }
+ return mult_T_*pts_[0];
+ }else
+ {
+ return evalHorner(t);
+ }
}
/// \brief Compute the derived curve at order N.
@@ -195,7 +200,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
if(derived_wp.empty())
derived_wp.push_back(point_t::Zero(Dim));
- bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_, mult_T_ * (1./T_) );
+ bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_min_, T_max_, mult_T_ * (1./(T_max_-T_min_)) );
return deriv.compute_derivate(order-1);
}
@@ -221,7 +226,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
current_sum += *pit;
n_wp.push_back(current_sum / new_degree);
}
- bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_, mult_T_*T_);
+ bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_min_, T_max_, mult_T_*(T_max_-T_min_));
return integ.compute_primitive(order-1);
}
@@ -248,7 +253,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
point_t evalBernstein(const Numeric t) const
{
- const Numeric u = t/T_;
+ const Numeric u = (t-T_min_)/(T_max_-T_min_);
point_t res = point_t::Zero(Dim);
typename t_point_t::const_iterator pts_it = pts_.begin();
for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
@@ -273,7 +278,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
point_t evalHorner(const Numeric t) const
{
- const Numeric u = t/T_;
+ const Numeric u = (t-T_min_)/(T_max_-T_min_);
typename t_point_t::const_iterator pts_it = pts_.begin();
Numeric u_op, bc, tn;
u_op = 1.0 - u;
@@ -300,7 +305,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
point_t evalDeCasteljau(const Numeric t) const {
// normalize time :
- const Numeric u = t/T_;
+ const Numeric u = (t-T_min_)/(T_max_-T_min_);
t_point_t pts = deCasteljauReduction(waypoints(),u);
while(pts.size() > 1)
{
@@ -311,7 +316,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
t_point_t deCasteljauReduction(const Numeric t) const{
- return deCasteljauReduction(waypoints(),t/T_);
+ const Numeric u = (t-T_min_)/(T_max_-T_min_);
+ return deCasteljauReduction(waypoints(),u);
}
/// \brief Compute de Casteljau's reduction of the given list of points at time t.
@@ -347,12 +353,13 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \return pair containing the first element of both bezier curve obtained.
///
std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t){
- if (t == T_)
+ if (t == T_max_)
{
throw std::runtime_error("can't split curve, interval range is equal to original curve");
}
t_point_t wps_first(size_),wps_second(size_);
- const double u = t/T_;
+ const Numeric u = (t-T_min_)/(T_max_-T_min_);
+ std::cout<<T_min_<<" and "<<T_max_<<" t="<<t<<" u="<<u<<std::endl;
wps_first[0] = pts_.front();
wps_second[degree_] = pts_.back();
t_point_t casteljau_pts = waypoints();
@@ -365,25 +372,25 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
++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_);
+ bezier_curve_t c_first(wps_first.begin(), wps_first.end(),T_min_,t,mult_T_);
+ bezier_curve_t c_second(wps_second.begin(), wps_second.end(),t, T_max_,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_)
+ if(t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_)
{
throw std::out_of_range("In Extract curve : times out of bounds");
}
- if(t1 == 0. && t2 == T_)
+ if(t1 == T_min_ && t2 == T_max_)
{
- return bezier_curve_t(waypoints().begin(), waypoints().end(), T_,mult_T_);
+ return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
}
- if(t1 == 0.)
+ if(t1 == T_min_)
{
return split(t2).first;
}
- if(t2 == T_)
+ if(t2 == T_max_)
{
return split(t1).second;
}
@@ -425,14 +432,17 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
public:
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$, lower bound of time range.
- virtual time_t min() const{return 0.;}
+ virtual time_t min() const{return T_min_;}
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
- virtual time_t max() const{return T_;}
+ virtual time_t max() const{return T_max_;}
/*Helpers*/
public:
- /*const*/ time_t T_;
+ /// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
+ /*const*/ time_t T_min_;
+ /// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
+ /*const*/ time_t T_max_;
/*const*/ time_t mult_T_;
/*const*/ std::size_t size_;
/*const*/ std::size_t degree_;
@@ -446,7 +456,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
std::vector<point_t> ts;
ts.push_back(point_t::Zero(Dim));
- return bezier_curve_t(ts.begin(), ts.end(),T);
+ return bezier_curve_t(ts.begin(), ts.end(),0.,T);
}
};
} // namespace curve
diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
index ddf274771968c264b63c07a9a96eb6f6bab9d507..7d69b7c202a10075c5a541267d04e71103fa5168 100644
--- a/include/curves/cubic_hermite_spline.h
+++ b/include/curves/cubic_hermite_spline.h
@@ -290,7 +290,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
h00 = 12.;
h10 = 6.;
h01 = -12.;
- h11 = 0.;
+ h11 = 6.;
}
else
{
diff --git a/include/curves/curve_conversion.h b/include/curves/curve_conversion.h
index 58266c7fa161ee4fab7015fd0aa20f2e8f9d08b6..d828458efbb0e7109ff0bdb890c4ed5782aa7a38 100644
--- a/include/curves/curve_conversion.h
+++ b/include/curves/curve_conversion.h
@@ -38,6 +38,25 @@ Polynomial polynom_from_bezier(const Bezier& curve)
return Polynomial(coefficients,curve.min(),curve.max());
}
+template<typename Hermite, typename Polynomial>
+Polynomial polynom_from_hermite(const Hermite& curve)
+{
+ typedef typename Polynomial::t_point_t t_point_t;
+ typedef typename Polynomial::num_t num_t;
+ assert (curve.min() == 0.);
+ assert (curve.max() == 1.);
+ t_point_t coefficients;
+ Hermite current (curve);
+ coefficients.push_back(curve(0.));
+ num_t fact = 1;
+ for(std::size_t i = 1; i<= 3; ++i)
+ {
+ fact *= (num_t)i;
+ coefficients.push_back(current.derivate(0.,i)/fact);
+ }
+ return Polynomial(coefficients,curve.min(),curve.max());
+}
+
/// \brief Converts a Cubic Hermite curve to a cubic bezier.
/// \param curve : the cubic hermite curve defined between [0,1] to convert.
/// \return the equivalent cubic bezier curve.
diff --git a/include/curves/piecewise_polynomial_curve.h b/include/curves/piecewise_polynomial_curve.h
index f1bbbf1ad04b2088d9241a5f909979c3276e79dc..44d694c80a84333d5fbe7e119c4435dca3eef367 100644
--- a/include/curves/piecewise_polynomial_curve.h
+++ b/include/curves/piecewise_polynomial_curve.h
@@ -1,11 +1,7 @@
#ifndef _CLASS_PIECEWISE_CURVE
#define _CLASS_PIECEWISE_CURVE
-#include <type_traits>
-
#include "curve_abc.h"
-#include "cubic_hermite_spline.h"
-#include "bezier_curve"
#include "polynomial.h"
#include "curve_conversion.h"
@@ -17,30 +13,61 @@ namespace curves
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Dim, 1>,
- typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point>,
- typename Polynomial= polynomial <double, double, 3, true, point_t, t_point_t> > >
-struct piecewise_polynomial_curve
+ typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point> >
+ >
+struct piecewise_polynomial_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
typedef Time time_t;
typedef Numeric num_t;
+ typedef int Index;
//typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
- typedef Polynomial polynomial_t;
- typedef std::vector < Polynomial > t_polynomial_t;
+ typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
+ typedef typename std::vector < polynomial_t > t_polynomial_t;
typedef std::vector<Time> t_vector_time_t;
public:
- piecewise_polynomial_curve(polynomial_t pol)
+ /// \brief Constructor.
+ /// Initialize a piecewise polynomial curve by giving the first polynomial curve.
+ /// \param pol : a polynomial curve.
+ ///
+ piecewise_polynomial_curve(const polynomial_t& pol)
{
size_ = 0;
T_min_ = pol.min();
+ time_polynomial_curves_.push_back(T_min_);
add_polynomial_curve(pol);
}
+ virtual ~piecewise_polynomial_curve(){}
+
+ virtual Point operator()(const Time t) const
+ {
+ if(Safe &! (T_min_ <= t && t <= T_max_))
+ {
+ throw std::out_of_range("can't evaluate piecewise curve, out of range");
+ }
+ return polynomial_curves_.at(findInterval(t))(t);
+ }
+
+ /// \brief Evaluate the derivative of order N of curve at time t.
+ /// \param t : time when to evaluate the spline.
+ /// \param order : order of derivative.
+ /// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
+ ///
+ virtual Point derivate(const Time t, const std::size_t order) const
+ {
+ if(Safe &! (T_min_ <= t && t <= T_max_))
+ {
+ throw std::out_of_range("can't evaluate piecewise curve, out of range");
+ }
+ return (polynomial_curves_.at(findInterval(t))).derivate(t, order);
+ }
+
void add_polynomial_curve(polynomial_t pol)
{
// Check time continuity : Begin time of pol must be equal to T_max_ of actual piecewise curve.
@@ -54,12 +81,93 @@ struct piecewise_polynomial_curve
time_polynomial_curves_.push_back(T_max_);
}
+ bool isContinuous(const std::size_t order)
+ {
+ bool isContinuous =true;
+ Index i=0;
+ point_t value_end, value_start;
+ while (isContinuous && i<(size_-1))
+ {
+ polynomial_t current = polynomial_curves_.at(i);
+ polynomial_t next = polynomial_curves_.at(i+1);
+
+ if (order == 0)
+ {
+ value_end = current(current.max());
+ value_start = next(next.min());
+ }
+ else
+ {
+ value_end = current.derivate(current.max(),order);
+ value_start = next.derivate(current.min(),order);
+ }
+
+ if ((value_end-value_start).norm() > margin)
+ {
+ isContinuous = false;
+ }
+ i++;
+ }
+ return isContinuous;
+ }
+
private:
+
+ /// \brief Get index of the interval corresponding to time t for the interpolation.
+ /// \param t : time where to look for interval.
+ /// \return Index of interval for time t.
+ ///
+ Index findInterval(const Numeric t) const
+ {
+ // time before first control point time.
+ if(t < time_polynomial_curves_[0])
+ {
+ return 0;
+ }
+ // time is after last control point time
+ if(t > time_polynomial_curves_[size_-1])
+ {
+ return size_-1;
+ }
+
+ Index left_id = 0;
+ Index right_id = size_-1;
+ while(left_id <= right_id)
+ {
+ const Index middle_id = left_id + (right_id - left_id)/2;
+ if(time_polynomial_curves_.at(middle_id) < t)
+ {
+ left_id = middle_id+1;
+ }
+ else if(time_polynomial_curves_.at(middle_id) > t)
+ {
+ right_id = middle_id-1;
+ }
+ else
+ {
+ return middle_id;
+ }
+ }
+ return left_id-1;
+ }
+
+ /*Helpers*/
+ public:
+ /// \brief Get the minimum time for which the curve is defined
+ /// \return \f$t_{min}\f$, lower bound of time range.
+ Time virtual min() const{return T_min_;}
+ /// \brief Get the maximum time for which the curve is defined.
+ /// \return \f$t_{max}\f$, upper bound of time range.
+ Time virtual max() const{return T_max_;}
+ /*Helpers*/
+
+ /* Variables */
t_polynomial_t polynomial_curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
t_vector_time_t time_polynomial_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
- Numeric size_;
+ Numeric size_; // Number of segments in piecewise curve
Time T_min_, T_max_;
-}
+ const double margin = 0.001;
+};
} // end namespace
diff --git a/include/curves/polynomial.h b/include/curves/polynomial.h
index f26704659b215cd1d871f493fcd4a564e6967eab..257fb20a9645699273bef3ed7594174497bcad54 100644
--- a/include/curves/polynomial.h
+++ b/include/curves/polynomial.h
@@ -142,7 +142,7 @@ struct polynomial : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if((t < t_min_ || t > t_max_) && Safe)
{
- throw std::out_of_range("time t to evaluate should be in range [Tmin, Tmax] of the curve");
+ throw std::out_of_range("error in polynomial : time t to evaluate should be in range [Tmin, Tmax] of the curve");
}
time_t const dt (t-t_min_);
point_t h = coefficients_.col(order_);
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 71866ac505f78629f3868579a3db1978999fcc60..71441862636f79ff3e42ce229be53e3a2325f511 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -65,17 +65,18 @@ using namespace boost::python;
/* Template constructor bezier */
template <typename Bezier, typename PointList, typename T_Point>
-Bezier* wrapBezierConstructorTemplate(const PointList& array, const real ub =1.)
+Bezier* wrapBezierConstructorTemplate(const PointList& array, const real T_min =0., const real T_max =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
- return new Bezier(asVector.begin(), asVector.end(), ub);
+ return new Bezier(asVector.begin(), asVector.end(), T_min, T_max);
}
template <typename Bezier, typename PointList, typename T_Point, typename CurveConstraints>
-Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints, const real ub =1.)
+Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints,
+ const real T_min =0., const real T_max =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
- return new Bezier(asVector.begin(), asVector.end(), constraints, ub);
+ return new Bezier(asVector.begin(), asVector.end(), constraints, T_min, T_max);
}
/* End Template constructor bezier */
@@ -84,17 +85,18 @@ bezier3_t* wrapBezierConstructor3(const point_list_t& array)
{
return wrapBezierConstructorTemplate<bezier3_t, point_list_t, t_point_t>(array) ;
}
-bezier3_t* wrapBezierConstructorBounds3(const point_list_t& array, const real ub)
+bezier3_t* wrapBezierConstructorBounds3(const point_list_t& array, const real T_min, const real T_max)
{
- return wrapBezierConstructorTemplate<bezier3_t, point_list_t, t_point_t>(array, ub) ;
+ return wrapBezierConstructorTemplate<bezier3_t, point_list_t, t_point_t>(array, T_min, T_max) ;
}
bezier3_t* wrapBezierConstructor3Constraints(const point_list_t& array, const curve_constraints_t& constraints)
{
return wrapBezierConstructorConstraintsTemplate<bezier3_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints) ;
}
-bezier3_t* wrapBezierConstructorBounds3Constraints(const point_list_t& array, const curve_constraints_t& constraints, const real ub)
+bezier3_t* wrapBezierConstructorBounds3Constraints(const point_list_t& array, const curve_constraints_t& constraints,
+ const real T_min, const real T_max)
{
- return wrapBezierConstructorConstraintsTemplate<bezier3_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, ub) ;
+ return wrapBezierConstructorConstraintsTemplate<bezier3_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, T_min, T_max) ;
}
/*END 3D constructors bezier */
/*6D constructors bezier */
@@ -102,17 +104,17 @@ bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
{
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array) ;
}
-bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real ub)
+bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real T_min, const real T_max)
{
- return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, ub) ;
+ return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, T_min, T_max) ;
}
bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints)
{
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints) ;
}
-bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints, const real ub)
+bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints, const real T_min, const real T_max)
{
- return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, ub) ;
+ return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, T_min, T_max) ;
}
/*END 6D constructors bezier */
diff --git a/python/python_variables.cpp b/python/python_variables.cpp
index 163898544fe672865c226c43071bce776202c171..8306be298b6c831cb0cd2f3037350c8647e1d88e 100644
--- a/python/python_variables.cpp
+++ b/python/python_variables.cpp
@@ -51,18 +51,17 @@ std::vector<variables_3_t> computeLinearControlPoints(const point_list_t& matric
bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors)
{
std::vector<variables_3_t> asVector = computeLinearControlPoints(matrices, vectors);
- return new bezier_linear_variable_t(asVector.begin(), asVector.end(), 1.) ;
+ return new bezier_linear_variable_t(asVector.begin(), asVector.end(), 0., 1.) ;
}
-bezier_linear_variable_t* wrapBezierLinearConstructorBounds(const point_list_t& matrices, const point_list_t& vectors, const real ub)
+bezier_linear_variable_t* wrapBezierLinearConstructorBounds(const point_list_t& matrices, const point_list_t& vectors, const real T_min, const real T_max)
{
std::vector<variables_3_t> asVector = computeLinearControlPoints(matrices, vectors);
- return new bezier_linear_variable_t(asVector.begin(), asVector.end(), ub) ;
+ return new bezier_linear_variable_t(asVector.begin(), asVector.end(), T_min, T_max) ;
}
-LinearControlPointsHolder*
- wayPointsToLists(const bezier_linear_variable_t& self)
+LinearControlPointsHolder* wayPointsToLists(const bezier_linear_variable_t& self)
{
typedef typename bezier_linear_variable_t::t_point_t t_point;
typedef typename bezier_linear_variable_t::t_point_t::const_iterator cit_point;
diff --git a/tests/Main.cpp b/tests/Main.cpp
index de6f6653547ad5eeeaa18ac926365b6e65ac79aa..fbf663155f6127ad9f655d54e4f11efbce9fe2c3 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -6,6 +6,7 @@
#include "curves/helpers/effector_spline_rotation.h"
#include "curves/curve_conversion.h"
#include "curves/cubic_hermite_spline.h"
+#include "curves/piecewise_polynomial_curve.h"
#include <string>
#include <iostream>
@@ -36,6 +37,9 @@ typedef cubic_hermite_spline <double, double, 3, true, point_t> cubic_hermite_sp
typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
typedef std::vector<pair_point_tangent_t,Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
+typedef piecewise_polynomial_curve <double, double, 3, true, point_t> piecewise_polynomial_curve_t;
+
+
bool QuasiEqual(const double a, const double b, const float margin)
{
if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0))
@@ -154,7 +158,7 @@ void BezierCurveTest(bool& error)
std::vector<point_t> params;
params.push_back(a);
- // 1d curve
+ // 1d curve in [0,1]
bezier_curve_t cf1(params.begin(), params.end());
point_t res1;
res1 = cf1(0);
@@ -163,7 +167,7 @@ void BezierCurveTest(bool& error)
res1 = cf1(1);
ComparePoints(x10, res1, errMsg + "1(1) ", error);
- // 2d curve
+ // 2d curve in [0,1]
params.push_back(b);
bezier_curve_t cf(params.begin(), params.end());
res1 = cf(0);
@@ -174,7 +178,7 @@ void BezierCurveTest(bool& error)
res1 = cf(1);
ComparePoints(x21, res1, errMsg + "2(1) ", error);
- //3d curve
+ //3d curve in [0,1]
params.push_back(c);
bezier_curve_t cf3(params.begin(), params.end());
res1 = cf3(0);
@@ -183,14 +187,14 @@ void BezierCurveTest(bool& error)
res1 = cf3(1);
ComparePoints(c, res1, errMsg + "3(1) ", error);
- //4d curve
+ //4d curve in [1,2]
params.push_back(d);
- bezier_curve_t cf4(params.begin(), params.end(), 2);
+ bezier_curve_t cf4(params.begin(), params.end(), 1., 2.);
//testing bernstein polynomials
- bezier_curve_t cf5(params.begin(), params.end(),2.);
+ bezier_curve_t cf5(params.begin(), params.end(),1.,2.);
std::string errMsg2("In test BezierCurveTest ; Bernstein polynomials do not evaluate as analytical evaluation");
- for(double d = 0.; d <2.; d+=0.1)
+ for(double d = 1.; d <2.; d+=0.1)
{
ComparePoints( cf5.evalBernstein(d) , cf5 (d), errMsg2, error);
ComparePoints( cf5.evalHorner(d) , cf5 (d), errMsg2, error);
@@ -380,13 +384,15 @@ void BezierDerivativeCurveTimeReparametrizationTest(bool& error)
params.push_back(d);
params.push_back(e);
params.push_back(f);
- double T = 2.;
+ double Tmin = 0.;
+ double Tmax = 2.;
+ double diffT = Tmax-Tmin;
bezier_curve_t cf(params.begin(), params.end());
- bezier_curve_t cfT(params.begin(), params.end(),T);
+ bezier_curve_t cfT(params.begin(), params.end(),Tmin,Tmax);
ComparePoints(cf(0.5), cfT(1), errMsg, error);
- ComparePoints(cf.derivate(0.5,1), cfT.derivate(1,1) * T, errMsg, error);
- ComparePoints(cf.derivate(0.5,2), cfT.derivate(1,2) * T*T, errMsg, error);
+ ComparePoints(cf.derivate(0.5,1), cfT.derivate(1,1) * (diffT), errMsg, error);
+ ComparePoints(cf.derivate(0.5,2), cfT.derivate(1,2) * diffT*diffT, errMsg, error);
}
void BezierDerivativeCurveConstraintTest(bool& error)
@@ -455,6 +461,39 @@ void BezierToPolynomialConversionTest(bool& error)
}
}
+void CubicHermiteToPolynomialTest(bool& error)
+{
+ std::string errMsg("In test CubicHermiteToPolynomialTest ; unexpected result for x => ");
+ point_t p0(1,2,3);
+ point_t m0(2,3,4);
+ point_t p1(3,4,5);
+ point_t m1(3,6,7);
+
+ std::vector< double > time_control_points;
+ time_control_points.push_back(0.);
+ time_control_points.push_back(1.);
+
+ pair_point_tangent_t pair0(p0,m0);
+ pair_point_tangent_t pair1(p1,m1);
+
+ t_pair_point_tangent_t control_points;
+ control_points.push_back(pair0);
+ control_points.push_back(pair1);
+
+ cubic_hermite_spline_t ch(control_points.begin(), control_points.end(), time_control_points);
+ polynomial_t pol = polynom_from_hermite<cubic_hermite_spline_t, polynomial_t>(ch);
+
+ // Test derivative in t=0 and t=1
+ ComparePoints(ch.derivate(0.,1),pol.derivate(0.,1),errMsg, error, true);
+ ComparePoints(ch.derivate(1.,1),pol.derivate(1.,1),errMsg, error, true);
+
+ for(double i =0.; i<1.; i+=0.01)
+ {
+ ComparePoints(ch(i),pol(i),errMsg, error, true);
+ ComparePoints(ch(i),pol(i),errMsg, error, false);
+ }
+}
+
void CubicHermiteToCubicBezierConversionTest(bool& error)
{
std::string errMsg("In test CubicHermiteToCubicBezierConversionTest ; unexpected result for x => ");
@@ -486,6 +525,20 @@ void CubicHermiteToCubicBezierConversionTest(bool& error)
ComparePoints(ch(i),bc(i),errMsg, error, true);
ComparePoints(ch(i),bc(i),errMsg, error, false);
}
+
+ // Check if conversion to polynomial gives the same polynomial
+ polynomial_t pol_bc = polynom_from_bezier<bezier_curve_t,polynomial_t>(bc);
+ polynomial_t pol_ch = polynom_from_hermite<cubic_hermite_spline_t,polynomial_t>(ch);
+
+ // Test derivative in t=0 and t=1
+ ComparePoints(pol_ch.derivate(0.,1),pol_bc.derivate(0.,1),errMsg, error, true);
+ ComparePoints(pol_ch.derivate(1.,1),pol_bc.derivate(1.,1),errMsg, error, true);
+
+ for(double i =0.; i<1.; i+=0.01)
+ {
+ ComparePoints(pol_ch(i),pol_bc(i),errMsg, error, true);
+ ComparePoints(pol_ch(i),pol_bc(i),errMsg, error, false);
+ }
}
/*Exact Cubic Function tests*/
@@ -911,10 +964,11 @@ void BezierSplitCurve(bool& error)
{
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);
+ double t0 = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
+ double t1 = (rand()/(double)RAND_MAX )*(t_max-t0) + t0;
+ double ts = (rand()/(double)RAND_MAX )*(t1-t0)+t0;
- bezier_curve_t c(wps.begin(), wps.end(),t);
+ bezier_curve_t c(wps.begin(), wps.end(),t0, t1);
std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
//std::cout<<"split curve of duration "<<t<<" at "<<ts<<std::endl;
@@ -926,19 +980,19 @@ void BezierSplitCurve(bool& error)
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()))
+ if(c.max()-c.min() != (cs.first.max()-cs.first.min() + cs.second.max()-cs.second.min()))
{
error = true;
std::cout<<"Duration of the splitted curve doesn't correspond to the original"<<std::endl;
}
- if(c(0) != cs.first(0))
+ if(c(t0) != cs.first(t0))
{
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()))
+ if(c(t1) != cs.second(cs.second.max()))
{
error = true;
std::cout<<"final point of the splitted curve doesn't correspond to the original"<<std::endl;
@@ -950,16 +1004,19 @@ void BezierSplitCurve(bool& error)
std::cout<<"timing of the splitted curve doesn't correspond to the original"<<std::endl;
}
- if(cs.first(ts) != cs.second(0.))
+ if(cs.first(ts) != cs.second(ts))
{
error = true;
std::cout<<"splitting point of the splitted curve doesn't correspond to the original"<<std::endl;
}
+ std::cout<<"4"<<std::endl;
+
// check along curve :
- double ti = 0.;
+ double ti = t0;
while(ti <= ts)
{
+ std::cout<<"a "<<ti<<std::endl;
if((cs.first(ti) - c(ti)).norm() > 1e-14)
{
error = true;
@@ -967,14 +1024,17 @@ void BezierSplitCurve(bool& error)
}
ti += 0.01;
}
- while(ti <= t){
- if((cs.second(ti-ts) - c(ti)).norm() > 1e-14)
+ while(ti <= t1){
+ std::cout<<"b "<<ti<<std::endl;
+ if((cs.second(ti) - 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;
}
+
+ std::cout<<"5"<<std::endl;
}
}
@@ -988,42 +1048,42 @@ void CubicHermitePairsPositionDerivativeTest(bool& error)
std::vector< pair_point_tangent_t > control_points;
point_t res1;
- point_t P0(0.,0.,0.);
- point_t P1(1.,2.,3.);
- point_t P2(4.,4.,4.);
+ point_t p0(0.,0.,0.);
+ point_t p1(1.,2.,3.);
+ point_t p2(4.,4.,4.);
- tangent_t T0(0.5,0.5,0.5);
- tangent_t T1(0.1,0.2,-0.5);
- tangent_t T2(0.1,0.2,0.3);
+ tangent_t t0(0.5,0.5,0.5);
+ tangent_t t1(0.1,0.2,-0.5);
+ tangent_t t2(0.1,0.2,0.3);
std::vector< double > time_control_points;
// Two pairs
control_points.clear();
- control_points.push_back(pair_point_tangent_t(P0,T0));
+ control_points.push_back(pair_point_tangent_t(p0,t0));
time_control_points.push_back(0.); // Time at P0
- control_points.push_back(pair_point_tangent_t(P1,T1));
+ control_points.push_back(pair_point_tangent_t(p1,t1));
time_control_points.push_back(1.); // Time at P1
// Create cubic hermite spline
cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end(), time_control_points);
cubic_hermite_spline_1Pair.setTimeSplinesDefault();
//Check
res1 = cubic_hermite_spline_1Pair(0.); // t=0
- ComparePoints(P0, res1, errmsg1, error);
+ ComparePoints(p0, res1, errmsg1, error);
res1 = cubic_hermite_spline_1Pair(1.); // t=1
- ComparePoints(P1, res1, errmsg1, error);
+ ComparePoints(p1, res1, errmsg1, error);
res1 = cubic_hermite_spline_1Pair(0.5); // t=0.5
ComparePoints(point_t(0.55,1.0375,1.625), res1, errmsg2, error);
// Test derivative : two pairs
res1 = cubic_hermite_spline_1Pair.derivate(0.,1);
- ComparePoints(T0, res1, errmsg3, error);
+ ComparePoints(t0, res1, errmsg3, error);
res1 = cubic_hermite_spline_1Pair.derivate(0.5,1);
ComparePoints(point_t(1.35,2.825,4.5), res1, errmsg3, error);
res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
- ComparePoints(T1, res1, errmsg3, error);
+ ComparePoints(t1, res1, errmsg3, error);
// Three pairs
- control_points.push_back(pair_point_tangent_t(P2,T2));
+ control_points.push_back(pair_point_tangent_t(p2,t2));
time_control_points.clear();
time_control_points.push_back(0.); // Time at P0
time_control_points.push_back(2.); // Time at P1
@@ -1031,36 +1091,135 @@ void CubicHermitePairsPositionDerivativeTest(bool& error)
cubic_hermite_spline_t cubic_hermite_spline_2Pairs(control_points.begin(), control_points.end(), time_control_points);
//Check
res1 = cubic_hermite_spline_2Pairs(0.); // t=0
- ComparePoints(P0, res1, errmsg1, error);
+ ComparePoints(p0, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(2.); // t=2
- ComparePoints(P1, res1, errmsg2, error);
+ ComparePoints(p1, res1, errmsg2, error);
res1 = cubic_hermite_spline_2Pairs(5.); // t=5
- ComparePoints(P2, res1, errmsg1, error);
+ ComparePoints(p2, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(1.); // t=1.0 , same than in two pairs at t=0.5
ComparePoints(point_t(0.55,1.0375,1.625), res1, errmsg2, error);
// Test derivative : three pairs
res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
- ComparePoints(T0, res1, errmsg3, error);
+ ComparePoints(t0, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(2.,1);
- ComparePoints(T1, res1, errmsg3, error);
+ ComparePoints(t1, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(5.,1);
- ComparePoints(T2, res1, errmsg3, error);
+ ComparePoints(t2, res1, errmsg3, error);
// Test time control points by default => with N control points :
// Time at P0= 0. | Time at P1= 1.0/(N-1) | Time at P2= 2.0/(N-1) | ... | Time at P_(N-1)= (N-1)/(N-1)= 1.0
cubic_hermite_spline_2Pairs.setTimeSplinesDefault();
res1 = cubic_hermite_spline_2Pairs(0.); // t=0
- ComparePoints(P0, res1, errmsg1, error);
+ ComparePoints(p0, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
- ComparePoints(P1, res1, errmsg2, error);
+ ComparePoints(p1, res1, errmsg2, error);
res1 = cubic_hermite_spline_2Pairs(1.); // t=1
- ComparePoints(P2, res1, errmsg1, error);
+ ComparePoints(p2, res1, errmsg1, error);
// Test derivative : three pairs, time default
res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
- ComparePoints(T0, res1, errmsg3, error);
+ ComparePoints(t0, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(0.5,1);
- ComparePoints(T1, res1, errmsg3, error);
+ ComparePoints(t1, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(1.,1);
- ComparePoints(T2, res1, errmsg3, error);
+ ComparePoints(t2, res1, errmsg3, error);
+}
+
+
+void piecewisePolynomialCurveTest(bool& error)
+{
+ std::string errmsg1("in piecewise polynomial curve test, Error While checking value of point on curve : ");
+ point_t a(1,1,1); // in [0,1[
+ point_t b(2,1,1); // in [1,2[
+ point_t c(3,1,1); // in [2,3]
+ point_t res;
+ t_point_t vec1, vec2, vec3;
+ vec1.push_back(a); // x=1, y=1, z=1
+ vec2.push_back(b); // x=2, y=1, z=1
+ vec3.push_back(c); // x=3, y=1, z=1
+ polynomial_t pol1(vec1.begin(), vec1.end(), 0, 1);
+ polynomial_t pol2(vec2.begin(), vec2.end(), 1, 2);
+ polynomial_t pol3(vec3.begin(), vec3.end(), 2, 3);
+
+ // 1 polynomial in curve
+ piecewise_polynomial_curve_t ppc(pol1);
+ res = ppc(0.5);
+ ComparePoints(a,res,errmsg1,error);
+
+ // 3 polynomials in curve
+ ppc.add_polynomial_curve(pol2);
+ ppc.add_polynomial_curve(pol3);
+
+ // Check values on piecewise curve
+ res = ppc(0.);
+ ComparePoints(a,res,errmsg1,error);
+ res = ppc(0.5);
+ ComparePoints(a,res,errmsg1,error);
+ res = ppc(1.0);
+ ComparePoints(b,res,errmsg1,error);
+ res = ppc(1.5);
+ ComparePoints(b,res,errmsg1,error);
+ res = ppc(2.5);
+ ComparePoints(c,res,errmsg1,error);
+ res = ppc(3.0);
+ ComparePoints(c,res,errmsg1,error);
+
+ // piecewise curve C0
+ point_t a1(1,1,1);
+ t_point_t vec_C0;
+ vec_C0.push_back(a);
+ vec_C0.push_back(a1);
+ polynomial_t pol_a(vec_C0, 1.0, 2.0);
+ piecewise_polynomial_curve_t ppc_C0(pol1); // for t in [0,1[ : x=1 , y=1 , z=1
+ ppc_C0.add_polynomial_curve(pol_a); // for t in [1,2] : x=(t-1)+1 , y=(t-1)+1 , z=(t-1)+1
+ // Check value in t=0.5 and t=1.5
+ res = ppc_C0(0.5);
+ ComparePoints(a,res,errmsg1,error);
+ res = ppc_C0(1.5);
+ ComparePoints(point_t(1.5,1.5,1.5),res,errmsg1,error);
+
+ std::cout<<"here"<<std::endl;
+
+ // piecewise curve C1 from Hermite
+ point_t p0(0.,0.,0.);
+ point_t p1(1.,2.,3.);
+ point_t p2(4.,4.,4.);
+ tangent_t t0(0.5,0.5,0.5);
+ tangent_t t1(0.1,0.2,-0.5);
+ tangent_t t2(0.1,0.2,0.3);
+ std::vector< pair_point_tangent_t > control_points_0;
+ control_points_0.push_back(pair_point_tangent_t(p0,t0));
+ control_points_0.push_back(pair_point_tangent_t(p1,t1)); // control_points_0 = 1st piece of curve
+ std::vector< pair_point_tangent_t > control_points_1;
+ control_points_1.push_back(pair_point_tangent_t(p1,t1));
+ control_points_1.push_back(pair_point_tangent_t(p2,t2)); // control_points_1 = 2nd piece of curve
+ std::vector< double > time_control_points;
+ time_control_points.push_back(0.);
+ time_control_points.push_back(1.); // For both curves, time will be between [0,1]
+ cubic_hermite_spline_t chs0(control_points_0.begin(), control_points_0.end(), time_control_points);
+ cubic_hermite_spline_t chs1(control_points_1.begin(), control_points_1.end(), time_control_points);
+ // Convert to polynomial
+ polynomial_t pol_chs0 = polynom_from_hermite<cubic_hermite_spline_t, polynomial_t>(chs0);
+ polynomial_t pol_chs1 = polynom_from_hermite<cubic_hermite_spline_t, polynomial_t>(chs1);
+ piecewise_polynomial_curve_t ppc_C1(pol_chs0);
+ ppc_C1.add_polynomial_curve(pol_chs1);
+
+
+ // check C0 continuity
+ std::string errmsg2("in piecewise polynomial curve test, Error while checking continuity C0 ");
+ // Test 1 : not C0
+ bool isC0 = ppc.isContinuous(0);
+ if (isC0)
+ {
+ std::cout << errmsg2 << " ppc " << std::endl;
+ error = true;
+ }
+ // Test 1 : C0
+ isC0 = ppc_C0.isContinuous(0);
+ if (not isC0)
+ {
+ std::cout << errmsg2 << " ppc_C0 " << std::endl;
+ error = true;
+ }
+
}
@@ -1068,6 +1227,7 @@ int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "performing tests... \n";
bool error = false;
+
CubicFunctionTest(error);
ExactCubicNoErrorTest(error);
ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
@@ -1084,11 +1244,18 @@ int main(int /*argc*/, char** /*argv[]*/)
BezierDerivativeCurveConstraintTest(error);
BezierCurveTestCompareHornerAndBernstein(error);
BezierDerivativeCurveTimeReparametrizationTest(error);
- BezierToPolynomialConversionTest(error);
- CubicHermiteToCubicBezierConversionTest(error);
+ std::cout<<"here 1"<<std::endl;
BezierEvalDeCasteljau(error);
+ std::cout<<"here 2"<<std::endl;
BezierSplitCurve(error);
+ std::cout<<"here 3"<<std::endl;
CubicHermitePairsPositionDerivativeTest(error);
+ std::cout<<"here 4"<<std::endl;
+
+ BezierToPolynomialConversionTest(error);
+ CubicHermiteToCubicBezierConversionTest(error);
+ CubicHermiteToPolynomialTest(error);
+ piecewisePolynomialCurveTest(error);
if(error)
{
std::cout << "There were some errors\n";