diff --git a/include/curves/CMakeLists.txt b/include/curves/CMakeLists.txt
index c0f87f95b9555d1cbb879217c9f7ee0596bea727..8fefd30f211302e469c04d014d14f40909e44d9b 100644
--- a/include/curves/CMakeLists.txt
+++ b/include/curves/CMakeLists.txt
@@ -1,16 +1,17 @@
SET(${PROJECT_NAME}_HEADERS
bernstein.h
- bezier_polynom_conversion.h
+ curve_conversion.h
curve_abc.h
exact_cubic.h
MathDefs.h
- polynom.h
- spline_deriv_constraint.h
+ polynomial.h
bezier_curve.h
cubic_spline.h
curve_constraint.h
quintic_spline.h
linear_variable.h
+ cubic_hermite_spline.h
+ piecewise_polynomial_curve.h
)
INSTALL(FILES
diff --git a/include/curves/MathDefs.h b/include/curves/MathDefs.h
index 628898e60bf97ac176b28584fcbc8674ae8c65c8..18cced66cf22e1f7c39943e695a388b693daa5a7 100644
--- a/include/curves/MathDefs.h
+++ b/include/curves/MathDefs.h
@@ -35,7 +35,9 @@ void PseudoInverse(_Matrix_Type_& pinvmat)
for (long i=0; i<m_sigma.rows(); ++i)
{
if (m_sigma(i) > pinvtoler)
+ {
m_sigma_inv(i,i)=1.0/m_sigma(i);
+ }
}
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
}
diff --git a/include/curves/bernstein.h b/include/curves/bernstein.h
index 800ac3447c861899370eca24181d370e7ae03482..31c8aedff57f86da23f6b43ac24b89bed0b5a7de 100644
--- a/include/curves/bernstein.h
+++ b/include/curves/bernstein.h
@@ -20,23 +20,20 @@
namespace curves
{
-///
-/// \brief Computes factorial of a number.
-///
-inline unsigned int fact(const unsigned int n)
-{
- unsigned int res = 1;
- for (unsigned int i=2 ; i <= n ; ++i)
- res *= i;
- return res;
-}
-
-///
-/// \brief Computes a binomal coefficient.
+/// \brief Computes a binomial coefficient.
+/// \param n : an unsigned integer.
+/// \param k : an unsigned integer.
+/// \return \f$\binom{n}{k}f$
///
inline unsigned int bin(const unsigned int n, const unsigned int k)
{
- return fact(n) / (fact(k) * fact(n - k));
+ if(k > n)
+ throw std::runtime_error("binomial coefficient higher than degree");
+ if(k == 0)
+ return 1;
+ if(k > n/2)
+ return bin(n,n-k);
+ return n * bin(n-1,k-1) / k;
}
/// \class Bernstein.
@@ -62,8 +59,6 @@ Numeric i_;
Numeric bin_m_i_;
};
-
-///
/// \brief Computes all Bernstein polynomes for a certain degree.
///
template <typename Numeric>
diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index d5d6bf124e435bf0d8d94ba10394793be2ffa11f..56210193005728412381f4faaf3966de25f41005 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -44,60 +44,16 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
public:
/// \brief Constructor.
- /// Given the first and last point of a control points set, automatically create the bezier curve.
- /// \param PointsBegin : an iterator pointing to the first element of a control point container.
- /// \param PointsEnd : an iterator pointing to the last element of a control point container.
- ///
- template<typename In>
- bezier_curve(In PointsBegin, In PointsEnd)
- : T_(1.)
- , mult_T_(1.)
- , size_(std::distance(PointsBegin, PointsEnd))
- , degree_(size_-1)
- , bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
- {
- assert(bernstein_.size() == size_);
- In it(PointsBegin);
- if(Safe && (size_<1 || T_ <= 0.))
- throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
- for(; it != PointsEnd; ++it)
- pts_.push_back(*it);
- }
-
- /// \brief Constructor.
- /// Given the first and last point of a control points set, automatically create the bezier curve.
- /// \param PointsBegin : an iterator pointing to the first element of a control point container.
- /// \param PointsEnd : an iterator pointing to the last element of a control point container.
- /// \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)
- , mult_T_(1.)
- , size_(std::distance(PointsBegin, PointsEnd))
- , degree_(size_-1)
- , bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
- {
- assert(bernstein_.size() == size_);
- In it(PointsBegin);
- if(Safe && (size_<1 || T_ <= 0.))
- throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
- for(; it != PointsEnd; ++it)
- pts_.push_back(*it);
- }
-
-
-
- /// \brief Constructor.
- /// Given the first and last point of a control points set, automatically create the bezier curve.
+ /// Given the first and last point of a control points set, create the bezier curve.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param T : upper bound of time which is between \f$[0;T]\f$ (default \f$[0;1]\f$).
- /// \param mult_T :
+ /// \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=0., const time_t T_max=1., const time_t mult_T=1.)
+ : T_min_(T_min)
+ , T_max_(T_max)
, mult_T_(mult_T)
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
@@ -105,10 +61,14 @@ 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
+ }
for(; it != PointsEnd; ++it)
- pts_.push_back(*it);
+ {
+ control_points_.push_back(*it);
+ }
}
/// \brief Constructor
@@ -119,18 +79,24 @@ 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)
- , mult_T_(1.)
+ bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints,
+ const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
+ : T_min_(T_min)
+ , T_max_(T_max)
+ , mult_T_(mult_T)
, 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");
+ }
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
- pts_.push_back(*cit);
+ {
+ control_points_.push_back(*cit);
+ }
}
///\brief Destructor
@@ -148,141 +114,179 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
public:
/// \brief Evaluation of the bezier curve at time t.
/// \param t : time when to evaluate the curve.
- /// \return Point corresponding on curve at time t.
+ /// \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)
+ {
+ return mult_T_*control_points_[0];
+ }else
{
- 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 evalHorner(t);
+ }
}
- /// \brief Compute the derivative curve at order N.
- /// Computes the derivative at order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
- /// \param order : order of the derivative.
- /// \return Derivative \f$\frac{dx(t)}{dt}\f$.
+ /// \brief Compute the derived curve at order N.
+ /// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
+ /// \param order : order of derivative.
+ /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
bezier_curve_t compute_derivate(const std::size_t order) const
{
- if(order == 0) return *this;
+ 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)
+ for(typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end()-1; ++pit)
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);
}
/// \brief Compute the primitive of the curve at order N.
- /// Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. At order \f$N=1\f$,
- /// the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
+ /// Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
+ /// At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
/// \param order : order of the primitive.
- /// \return Primitive of x(t).
+ /// \return primitive at order N of x(t).
bezier_curve_t compute_primitive(const std::size_t order) const
{
- if(order == 0) return *this;
+ if(order == 0)
+ {
+ return *this;
+ }
num_t new_degree = (num_t)(degree_+1);
t_point_t n_wp;
point_t current_sum = point_t::Zero(Dim);
// 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)
+ for(typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end(); ++pit)
{
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);
}
- /// \brief Evaluate 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
+ /// \brief Evaluate the derivative order N of curve at time t.
+ /// If derivative is to be evaluated several times, it is
+ /// rather recommended to compute derived curve using compute_derivate.
+ /// \param order : order of derivative.
/// \param t : time when to evaluate the curve.
- /// \return Point corresponding on derivative curve at time t.
+ /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
+ ///
virtual point_t derivate(const time_t t, const std::size_t order) const
{
- bezier_curve_t deriv =compute_derivate(order);
+ bezier_curve_t deriv = compute_derivate(order);
return deriv(t);
}
/// \brief Evaluate all Bernstein polynomes for a certain degree.
- /// Warning: the horner scheme is about 100 times faster than this method.
- /// This method will probably be removed in the future.
- /// \param t : unNormalized time
- /// \return Point corresponding on curve at time t.
+ /// A bezier curve with N control points is represented by : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
+ /// with \f$ B_i^N(t) = \binom{N}{i}t^i (1-t)^{N-i} \f$.<br/>
+ /// Warning: the horner scheme is about 100 times faster than this method.<br>
+ /// This method will probably be removed in the future as the computation of bernstein polynomial is very costly.
+ /// \param t : time when to evaluate the curve.
+ /// \return \f$x(t)\f$ point corresponding on curve at time t.
///
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();
+ typename t_point_t::const_iterator control_points_it = control_points_.begin();
for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
- cit !=bernstein_.end(); ++cit, ++pts_it)
- res += cit->operator()(u) * (*pts_it);
+ cit !=bernstein_.end(); ++cit, ++control_points_it)
+ {
+ res += cit->operator()(u) * (*control_points_it);
+ }
return res*mult_T_;
}
- /// \brief Evaluate all Bernstein polynomes for a certain degree using horner's scheme.
- /// \param t : unNormalized time
- /// \return Point corresponding on curve at time t.
+ /// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
+ /// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.<br>
+ /// To evaluate the position on curve at time t,we can apply the Horner's scheme : <br>
+ /// \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.<br>
+ /// Horner's scheme : for a polynom of degree N expressed by : <br>
+ /// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
+ /// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$<br>
+ /// Using Horner's method, the polynom is transformed into : <br>
+ /// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
+ /// \param t : time when to evaluate the curve.
+ /// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t evalHorner(const Numeric t) const
{
- const Numeric u = t/T_;
- typename t_point_t::const_iterator pts_it = pts_.begin();
+ const Numeric u = (t-T_min_)/(T_max_-T_min_);
+ typename t_point_t::const_iterator control_points_it = control_points_.begin();
Numeric u_op, bc, tn;
u_op = 1.0 - u;
bc = 1;
tn = 1;
- point_t tmp =(*pts_it)*u_op; ++pts_it;
- for(unsigned int i=1; i<degree_; i++, ++pts_it)
+ point_t tmp =(*control_points_it)*u_op; ++control_points_it;
+ for(unsigned int i=1; i<degree_; i++, ++control_points_it)
{
tn = tn*u;
bc = bc*((num_t)(degree_-i+1))/i;
- tmp = (tmp + tn*bc*(*pts_it))*u_op;
+ tmp = (tmp + tn*bc*(*control_points_it))*u_op;
}
- return (tmp + tn*u*(*pts_it))*mult_T_;
+ return (tmp + tn*u*(*control_points_it))*mult_T_;
}
- const t_point_t& waypoints() const {return pts_;}
+ const t_point_t& waypoints() const {return control_points_;}
/// \brief Evaluate the curve value at time t using deCasteljau algorithm.
- /// \param t : unNormalized time
- /// \return Point corresponding on curve at time t.
+ /// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control points
+ /// of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier
+ /// curve at time \f$t\f$.
+ /// \param t : time when to evaluate the curve.
+ /// \return \f$x(t)\f$ point corresponding on curve at time t.
///
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){
+ while(pts.size() > 1)
+ {
pts = deCasteljauReduction(pts,u);
}
return pts[0]*mult_T_;
}
+
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.
+ /// For the list \f$pts\f$ of N points, compute a new list of points of size N-1 :<br>
+ /// \f$<br>( pts[0]*(1-t)+pts[1], pts[1]*(1-t)+pts[2], ..., pts[0]*(N-2)+pts[N-1] )\f$<br>
+ /// with t the time when to evaluate bezier curve.<br>\ The new list contains centroid of
+ /// parameters \f${t,1-t}\f$ of consecutive points in the list.
/// \param pts : list of points.
- /// \param u : NORMALIZED time.
- /// \return Reduced list of point (size of pts - 1).
+ /// \param u : NORMALIZED time when to evaluate the curve.
+ /// \return reduced list of point (size of pts - 1).
///
t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const{
if(u < 0 || u > 1)
+ {
throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
+ }
if(pts.size() == 1)
+ {
return pts;
+ }
t_point_t new_pts;
- for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit){
+ for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit)
+ {
new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
}
return new_pts;
@@ -291,38 +295,49 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Split the bezier curve in 2 at time t.
/// \param t : list of points.
/// \param u : unNormalized time.
- /// \return A pair containing the first element of both bezier curve obtained.
+ /// \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_;
- wps_first[0] = pts_.front();
- wps_second[degree_] = pts_.back();
+ const Numeric u = (t-T_min_)/(T_max_-T_min_);
+ wps_first[0] = control_points_.front();
+ wps_second[degree_] = control_points_.back();
t_point_t casteljau_pts = waypoints();
size_t id = 1;
- while(casteljau_pts.size() > 1){
+ while(casteljau_pts.size() > 1)
+ {
casteljau_pts = deCasteljauReduction(casteljau_pts,u);
wps_first[id] = casteljau_pts.front();
wps_second[degree_-id] = casteljau_pts.back();
++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_)
- return bezier_curve_t(waypoints().begin(), waypoints().end(), T_,mult_T_);
- if(t1 == 0.)
+ }
+ if(t1 == T_min_ && t2 == T_max_)
+ {
+ return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
+ }
+ if(t1 == T_min_)
+ {
return split(t2).first;
- if(t2 == T_)
+ }
+ if(t2 == T_max_)
+ {
return split(t1).second;
+ }
std::pair<bezier_curve_t,bezier_curve_t> c_split = this->split(t1);
return c_split.second.split(t2-t1).first;
@@ -333,19 +348,23 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
{
t_point_t res;
+ num_t T = T_max_ - T_min_;
+ num_t T_square = T*T;
point_t P0, P1, P2, P_n_2, P_n_1, PN;
P0 = *PointsBegin; PN = *(PointsEnd-1);
- P1 = P0+ constraints.init_vel / (num_t)degree_;
- P_n_1 = PN -constraints.end_vel / (num_t)degree_;
- P2 = constraints.init_acc / (num_t)(degree_ * (degree_-1)) + 2* P1 - P0;
- P_n_2 = constraints.end_acc / (num_t)(degree_ * (degree_-1)) + 2* P_n_1 - PN;
+ P1 = P0+ constraints.init_vel * T / (num_t)degree_;
+ P_n_1 = PN- constraints.end_vel * T / (num_t)degree_;
+ P2 = constraints.init_acc * T_square / (num_t)(degree_ * (degree_-1)) + 2* P1 - P0;
+ P_n_2 = constraints.end_acc * T_square / (num_t)(degree_ * (degree_-1)) + 2* P_n_1 - PN;
res.push_back(P0);
res.push_back(P1);
res.push_back(P2);
for(In it = PointsBegin+1; it != PointsEnd-1; ++it)
+ {
res.push_back(*it);
+ }
res.push_back(P_n_2);
res.push_back(P_n_1);
@@ -357,26 +376,31 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/
public:
- virtual time_t min() const{return 0.;}
- virtual time_t max() const{return T_;}
+ /// \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 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_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_;
/*const*/ std::vector<Bern<Numeric> > bernstein_;
-
- private:
- t_point_t pts_;
+ /*const*/ t_point_t control_points_;
public:
static bezier_curve_t zero(const time_t T=1.)
{
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/bezier_polynom_conversion.h b/include/curves/bezier_polynomial_conversion.h
similarity index 61%
rename from include/curves/bezier_polynom_conversion.h
rename to include/curves/bezier_polynomial_conversion.h
index 6829f98d0846be4359807dd6562b25f1b79bddf4..c45461dc98555c2f7e7ffca8f07fc97455da273a 100644
--- a/include/curves/bezier_polynom_conversion.h
+++ b/include/curves/bezier_polynomial_conversion.h
@@ -24,17 +24,17 @@
namespace curves
{
/// \brief Provides methods for converting a curve from a bernstein representation
-/// to a polynom representation.
+/// to a polynomial representation.
///
-/// \brief Converts a Bezier curve to a polynom.
-/// \param bezier: the Bezier curve to convert.
-/// \return The equivalent polynom.
-template<typename Bezier, typename Polynom>
-Polynom from_bezier(const Bezier& curve)
+/// \brief Converts a Bezier curve to a polynomial.
+/// \param bezier : the Bezier curve to convert.
+/// \return the equivalent polynomial.
+template<typename Bezier, typename Polynomial>
+Polynomial from_bezier(const Bezier& curve)
{
- typedef typename Polynom::t_point_t t_point_t;
- typedef typename Polynom::num_t num_t;
+ 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;
@@ -47,14 +47,15 @@ Polynom from_bezier(const Bezier& curve)
fact *= (num_t)i;
coefficients.push_back(current(0.)/fact);
}
- return Polynom(coefficients,curve.min(),curve.max());
+ return Polynomial(coefficients,curve.min(),curve.max());
}
-///\brief Converts a polynom to a Bezier curve
-///\param polynom: the polynom to be converted from
-///\return the equivalent Bezier curve
-/*template<typename Bezier, typename Polynom>
-Bezier from_polynom(const Polynom& polynom)
+/*
+/// \brief Converts a polynomial to a Bezier curve.
+/// \param polynomial : the polynomial to convert.
+/// \return the equivalent Bezier curve.
+template<typename Bezier, typename Polynomial>
+Bezier from_polynomial(const Polynomial& polynomial)
{
typedef Bezier::point_t point_t;
typedef Bezier::time_t time_t;
@@ -63,7 +64,8 @@ Bezier from_polynom(const Polynom& polynom)
typedef Bezier::t_point_t t_point_t;
typedef Bezier::cit_point_t cit_point_t;
typedef Bezier::bezier_curve_t bezier_curve_t;
-}*/
+}
+*/
} // namespace curves
#endif //_BEZIER_POLY_CONVERSION
diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd2118d5e1992d630626ce3e6fb9516e45518bff
--- /dev/null
+++ b/include/curves/cubic_hermite_spline.h
@@ -0,0 +1,371 @@
+/**
+* \file cubic_hermite_spline.h
+* \brief class allowing to create a cubic hermite spline of any dimension.
+* \author Justin Carpentier <jcarpent@laas.fr> modified by Jason Chemin <jchemin@laas.fr>
+* \date 05/2019
+*/
+
+#ifndef _CLASS_CUBICHERMITESPLINE
+#define _CLASS_CUBICHERMITESPLINE
+
+#include "curve_abc.h"
+#include "curve_constraint.h"
+
+#include "MathDefs.h"
+
+#include <vector>
+#include <stdexcept>
+
+#include <iostream>
+
+namespace curves
+{
+/// \class CubicHermiteSpline.
+/// \brief Represents a set of cubic hermite splines defining a continuous function \f$p(t)\f$.
+/// A hermite cubic spline is a minimal degree polynom interpolating a function in two
+/// points \f$P_i\f$ and \f$P_{i+1}\f$ with its tangent \f$m_i\f$ and \f$m_{i+1}\f$.<br>
+/// A hermite cubic spline :
+/// - crosses each of the waypoint given in its initialization (\f$P_0\f$, \f$P_1\f$,...,\f$P_N\f$).
+/// - has its derivatives on \f$P_i\f$ and \f$P_{i+1}\f$ are \f$p'(t_{P_i}) = m_i\f$ and \f$p'(t_{P_{i+1}}) = m_{i+1}\f$.
+///
+template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
+, typename Point= Eigen::Matrix<Numeric, Dim, 1>
+, typename Tangent= Eigen::Matrix<Numeric, Dim, 1>
+>
+struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
+{
+ typedef std::pair<Point, Tangent> pair_point_tangent_t;
+ typedef std::vector< pair_point_tangent_t ,Eigen::aligned_allocator<Point> > t_pair_point_tangent_t;
+ typedef std::vector<Time> vector_time_t;
+ typedef Numeric num_t;
+
+ /*Attributes*/
+ public:
+ /// Vector of pair < Point, Tangent >.
+ t_pair_point_tangent_t control_points_;
+ /// Vector of Time corresponding to time of each N control points : time at \f$P_0, P_1, P_2, ..., P_N\f$.
+ /// Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.
+ vector_time_t time_control_points_;
+
+ /// Vector of Time corresponding to time duration of each subspline.<br>
+ /// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
+ /// duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} )<br>
+ /// It contains \f$N-1\f$ durations.
+ vector_time_t duration_splines_;
+ /// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
+ /*const*/ Time T_min_;
+ /// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
+ /*const*/ Time T_max_;
+ /// Number of control points (pairs).
+ std::size_t size_;
+ /// Degree (Cubic so degree 3)
+ static const std::size_t degree_ = 3;
+ /*Attributes*/
+
+ public:
+ /// \brief Constructor.
+ /// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
+ /// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
+ /// \param time_control_points : vector containing time for each waypoint.
+ ///
+ template<typename In>
+ cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t & time_control_points)
+ {
+ // Check size of pairs container.
+ std::size_t const size(std::distance(PairsBegin, PairsEnd));
+ size_ = size;
+ if(Safe && size < 1)
+ {
+ throw std::length_error("can not create cubic_hermite_spline, number of pairs is inferior to 2.");
+ }
+ // Push all pairs in controlPoints
+ In it(PairsBegin);
+ for(; it != PairsEnd; ++it)
+ {
+ control_points_.push_back(*it);
+ }
+ setTime(time_control_points);
+ }
+
+ /// \brief Destructor.
+ virtual ~cubic_hermite_spline(){}
+
+ /*Operations*/
+ public:
+ /// \brief Evaluation of the cubic hermite spline at time t.
+ /// \param t : time when to evaluate the spline.
+ /// \return \f$p(t)\f$ point corresponding on spline at time t.
+ ///
+ virtual Point operator()(const Time t) const
+ {
+ if(Safe &! (T_min_ <= t && t <= T_max_))
+ {
+ throw std::out_of_range("can't evaluate cubic hermite spline, out of range");
+ }
+ if (size_ == 1)
+ {
+ return control_points_.front().first;
+ }
+ else
+ {
+ return evalCubicHermiteSpline(t, 0);
+ }
+ }
+
+ /// \brief Evaluate the derivative of order N of spline 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
+ {
+ return evalCubicHermiteSpline(t, order);
+ }
+
+ /// \brief Set time of each control point of cubic hermite spline.
+ /// Set duration of each spline, Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with
+ /// values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.<br>
+ /// \param time_control_points : Vector containing time for each control point.
+ ///
+ void setTime(const vector_time_t & time_control_points)
+ {
+ time_control_points_ = time_control_points;
+ T_min_ = time_control_points_.front();
+ T_max_ = time_control_points_.back();
+ if (time_control_points.size() != size())
+ {
+ throw std::length_error("size of time control points should be equal to number of control points");
+ }
+ computeDurationSplines();
+ if (!checkDurationSplines())
+ {
+ throw std::logic_error("time_splines not monotonous, all spline duration should be superior to 0");
+ }
+ }
+
+ /// \brief Get vector of pair (positition, derivative) corresponding to control points.
+ /// \return vector containing control points.
+ ///
+ t_pair_point_tangent_t getControlPoints()
+ {
+ return control_points_;
+ }
+
+ /// \brief Get vector of Time corresponding to Time for each control point.
+ /// \return vector containing time of each control point.
+ ///
+ vector_time_t getTime()
+ {
+ return time_control_points_;
+ }
+
+ /// \brief Get number of control points contained in the trajectory.
+ /// \return number of control points.
+ ///
+ std::size_t size() const { return size_; }
+
+ /// \brief Get number of intervals (subsplines) contained in the trajectory.
+ /// \return number of intervals (subsplines).
+ ///
+ std::size_t numIntervals() const { return size()-1; }
+
+
+ /// \brief Evaluate value of cubic hermite spline or its derivate at specified order at time \f$t\f$.
+ /// A cubic hermite spline on unit interval \f$[0,1]\f$ and given two control points defined by
+ /// their position and derivative \f$\{p_0,m_0\}\f$ and \f$\{p_1,m_1\}\f$, is defined by the polynom : <br>
+ /// \f$p(t)=h_{00}(t)P_0 + h_{10}(t)m_0 + h_{01}(t)p_1 + h_{11}(t)m_1\f$<br>
+ /// To extend this formula to a cubic hermite spline on any arbitrary interval,
+ /// we define \f$\alpha=(t-t_0)/(t_1-t_0) \in [0, 1]\f$ where \f$t \in [t_0, t_1]\f$.<br>
+ /// Polynom \f$p(t) \in [t_0, t_1]\f$ becomes \f$p(\alpha) \in [0, 1]\f$
+ /// and \f$p(\alpha) = p((t-t_0)/(t_1-t_0))\f$.
+ /// \param t : time when to evaluate the curve.
+ /// \param order_derivative : Order of derivate of cubic hermite spline (set value to 0 if you do not want derivate)
+ /// \return point corresponding \f$p(t)\f$ on spline at time t or its derivate order N \f$\frac{d^Np(t)}{dt^N}\f$.
+ ///
+ Point evalCubicHermiteSpline(const Numeric t, std::size_t order_derivative) const
+ {
+ const std::size_t id = findInterval(t);
+ // ID is on the last control point
+ if(id == size_-1)
+ {
+ if (order_derivative==0)
+ {
+ return control_points_.back().first;
+ }
+ else if (order_derivative==1)
+ {
+ return control_points_.back().second;
+ }
+ else
+ {
+ return control_points_.back().first*0.; // To modify, create a new Tangent ininitialized with 0.
+ }
+ }
+ const pair_point_tangent_t Pair0 = control_points_.at(id);
+ const pair_point_tangent_t Pair1 = control_points_.at(id+1);
+ const Time & t0 = time_control_points_[id];
+ const Time & t1 = time_control_points_[id+1];
+ // Polynom for a cubic hermite spline defined on [0., 1.] is :
+ // p(t) = h00(t)*p0 + h10(t)*m0 + h01(t)*p1 + h11(t)*m1 with t in [0., 1.]
+ //
+ // For a cubic hermite spline defined on [t0, t1], we define alpha=(t-t0)/(t1-t0) in [0., 1.].
+ // Polynom p(t) defined on [t0, t1] becomes p(alpha) defined on [0., 1.]
+ // p(alpha) = p((t-t0)/(t1-t0))
+ //
+ const Time dt = (t1-t0);
+ const Time alpha = (t - t0)/dt;
+ assert(0. <= alpha <= 1. && "alpha must be in [0,1]");
+ Numeric h00, h10, h01, h11;
+ evalCoeffs(alpha,h00,h10,h01,h11,order_derivative);
+ //std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<" h11="<<h11<<std::endl;
+ Point p_ = (h00 * Pair0.first + h10 * dt * Pair0.second + h01 * Pair1.first + h11 * dt * Pair1.second);
+ // if derivative, divide by dt^order_derivative
+ for (std::size_t i=0; i<order_derivative; i++)
+ {
+ p_ /= dt;
+ }
+ return p_;
+ }
+
+ /// \brief Evaluate coefficient for polynom of cubic hermite spline.
+ /// Coefficients of polynom :<br>
+ /// - \f$h00(t)=2t^3-3t^2+1\f$;
+ /// - \f$h10(t)=t^3-2t^2+t\f$;
+ /// - \f$h01(t)=-2t^3+3t^2\f$;
+ /// - \f$h11(t)=t^3-t^2\f$.<br>
+ /// From it, we can calculate their derivate order N :
+ /// \f$\frac{d^Nh00(t)}{dt^N}\f$, \f$\frac{d^Nh10(t)}{dt^N}\f$,\f$\frac{d^Nh01(t)}{dt^N}\f$, \f$\frac{d^Nh11(t)}{dt^N}\f$.
+ /// \param t : time to calculate coefficients.
+ /// \param h00 : variable to store value of coefficient.
+ /// \param h10 : variable to store value of coefficient.
+ /// \param h01 : variable to store value of coefficient.
+ /// \param h11 : variable to store value of coefficient.
+ /// \param order_derivative : order of derivative.
+ ///
+ static void evalCoeffs(const Numeric t, Numeric & h00, Numeric & h10, Numeric & h01, Numeric & h11, std::size_t order_derivative)
+ {
+ Numeric t_square = t*t;
+ Numeric t_cube = t_square*t;
+ if (order_derivative==0)
+ {
+ h00 = 2*t_cube -3*t_square +1.;
+ h10 = t_cube -2*t_square +t;
+ h01 = -2*t_cube +3*t_square;
+ h11 = t_cube -t_square;
+ }
+ else if (order_derivative==1)
+ {
+ h00 = 6*t_square -6*t;
+ h10 = 3*t_square -4*t +1.;
+ h01 = -6*t_square +6*t;
+ h11 = 3*t_square -2*t;
+ }
+ else if (order_derivative==2)
+ {
+ h00 = 12*t -6.;
+ h10 = 6*t -4.;
+ h01 = -12*t +6.;
+ h11 = 6*t -2.;
+ }
+ else if (order_derivative==3)
+ {
+ h00 = 12.;
+ h10 = 6.;
+ h01 = -12.;
+ h11 = 6.;
+ }
+ else
+ {
+ h00 = 0.;
+ h10 = 0.;
+ h01 = 0.;
+ h11 = 0.;
+ }
+ }
+
+ private:
+ /// \brief Get index of the interval (subspline) corresponding to time t for the interpolation.
+ /// \param t : time where to look for interval.
+ /// \return Index of interval for time t.
+ ///
+ std::size_t findInterval(const Numeric t) const
+ {
+ // time before first control point time.
+ if(t < time_control_points_[0])
+ {
+ return 0;
+ }
+ // time is after last control point time
+ if(t > time_control_points_[size_-1])
+ {
+ return size_-1;
+ }
+
+ std::size_t left_id = 0;
+ std::size_t right_id = size_-1;
+ while(left_id <= right_id)
+ {
+ const std::size_t middle_id = left_id + (right_id - left_id)/2;
+ if(time_control_points_.at(middle_id) < t)
+ {
+ left_id = middle_id+1;
+ }
+ else if(time_control_points_.at(middle_id) > t)
+ {
+ right_id = middle_id-1;
+ }
+ else
+ {
+ return middle_id;
+ }
+ }
+ return left_id-1;
+ }
+
+ /// \brief compute duration of each spline.
+ /// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
+ /// Duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} ).
+ ///
+ void computeDurationSplines() {
+ duration_splines_.clear();
+ Time actual_time;
+ Time prev_time = *(time_control_points_.begin());
+ std::size_t i = 0;
+ for (i=0; i<size()-1; i++)
+ {
+ actual_time = time_control_points_.at(i+1);
+ duration_splines_.push_back(actual_time-prev_time);
+ prev_time = actual_time;
+ }
+ }
+
+ /// \brief Check if duration of each subspline is strictly positive.
+ /// \return true if all duration of strictly positive, false otherwise.
+ ///
+ bool checkDurationSplines() const
+ {
+ std::size_t i = 0;
+ bool is_positive = true;
+ while (is_positive && i<duration_splines_.size())
+ {
+ is_positive = (duration_splines_.at(i)>0.);
+ i++;
+ }
+ return is_positive;
+ }
+ /*Operations*/
+
+ /*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 time_control_points_.front();}
+ /// \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 time_control_points_.back();}
+ /*Helpers*/
+
+};
+
+} // namespace curve
+#endif //_CLASS_CUBICHERMITESPLINE
\ No newline at end of file
diff --git a/include/curves/cubic_spline.h b/include/curves/cubic_spline.h
index 4e2efed0e94f9240d8d5e86ef86be7d3957c074a..20def8256486cfd0f4cb893c95e2b0934c64896c 100644
--- a/include/curves/cubic_spline.h
+++ b/include/curves/cubic_spline.h
@@ -16,15 +16,16 @@
#include "MathDefs.h"
-#include "polynom.h"
+#include "polynomial.h"
#include <stdexcept>
namespace curves
{
/// \brief Creates coefficient vector of a cubic spline defined on the interval
-/// \f$[t_{min}, t_{max}]\f$. It follows the equation :
-/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$.
+/// \f$[t_{min}, t_{max}]\f$. It follows the equation : <br>
+/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$
+/// with a, b, c and d the control points.
///
template<typename Point, typename T_Point>
T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const &d)
@@ -35,11 +36,11 @@ T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point
}
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point>
-polynom<Time,Numeric,Dim,Safe,Point,T_Point> create_cubic(Point const& a, Point const& b, Point const& c, Point const &d,
+polynomial<Time,Numeric,Dim,Safe,Point,T_Point> create_cubic(Point const& a, Point const& b, Point const& c, Point const &d,
const Time t_min, const Time t_max)
{
T_Point coeffs = make_cubic_vector<Point, T_Point>(a,b,c,d);
- return polynom<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max);
+ return polynomial<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max);
}
} // namespace curves
#endif //_STRUCT_CUBICSPLINE
diff --git a/include/curves/curve_abc.h b/include/curves/curve_abc.h
index a943eb8911214e942ecebaef070dabb54d653932..d6afacb9b3c342e037434eff4a07ec94779e4671 100644
--- a/include/curves/curve_abc.h
+++ b/include/curves/curve_abc.h
@@ -18,9 +18,9 @@
namespace curves
{
-/// \struct curve_abc
-/// \brief Represents a curve of dimension Dim
-/// is Safe is false, no verification is made on the evaluation of the curve.
+/// \struct curve_abc.
+/// \brief Represents a curve of dimension Dim.
+/// If value of parameter Safe is false, no verification is made on the evaluation of the curve.
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct curve_abc : std::unary_function<Time, Point>
@@ -30,33 +30,35 @@ struct curve_abc : std::unary_function<Time, Point>
/* Constructors - destructors */
public:
- ///\brief Constructor
+ /// \brief Constructor.
curve_abc(){}
- ///\brief Destructor
+ /// \brief Destructor.
virtual ~curve_abc(){}
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
- /// \param t : the time when to evaluate the spine
- /// \param return : the value x(t)
+ /// \param t : time when to evaluate the spine
+ /// \return \f$x(t)\f$, point corresponding on curve at time 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)
+ /// \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^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t.
virtual point_t derivate(const time_t t, const std::size_t order) const = 0;
/*Operations*/
/*Helpers*/
public:
- /// \brief Returns the minimum time for wich curve is defined
+ /// \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 = 0;
- /// \brief Returns the maximum time for wich curve is defined
+ /// \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 = 0;
std::pair<time_t, time_t> timeRange() {return std::make_pair(min(), max());}
diff --git a/include/curves/curve_constraint.h b/include/curves/curve_constraint.h
index d3710c9fb40e396bce15fcc55989d7f2bb074623..eb88d1cd7026a9ea65aead04ddc442ecd932a911 100644
--- a/include/curves/curve_constraint.h
+++ b/include/curves/curve_constraint.h
@@ -26,11 +26,12 @@ struct curve_constraints
curve_constraints():
init_vel(point_t::Zero()),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
- ~curve_constraints(){}
- point_t init_vel;
- point_t init_acc;
- point_t end_vel;
- point_t end_acc;
+ ~curve_constraints(){}
+
+ point_t init_vel;
+ point_t init_acc;
+ point_t end_vel;
+ point_t end_acc;
};
} // namespace curves
#endif //_CLASS_CUBICZEROVELACC
diff --git a/include/curves/curve_conversion.h b/include/curves/curve_conversion.h
new file mode 100644
index 0000000000000000000000000000000000000000..98bf1ca8b3b867851138c05727d63ed32e8d3705
--- /dev/null
+++ b/include/curves/curve_conversion.h
@@ -0,0 +1,117 @@
+#ifndef _CLASS_CURVE_CONVERSION
+#define _CLASS_CURVE_CONVERSION
+
+#include "curve_abc.h"
+#include "bernstein.h"
+#include "curve_constraint.h"
+
+#include "MathDefs.h"
+
+#include <vector>
+#include <stdexcept>
+
+#include <iostream>
+
+namespace curves
+{
+
+/// \brief Converts a cubic hermite spline or a bezier curve to a polynomial.
+/// \param curve : the bezier curve/cubic hermite spline defined between [Tmin,Tmax] to convert.
+/// \return the equivalent polynomial.
+template<typename Polynomial, typename curveTypeToConvert>
+Polynomial polynomial_from_curve(const curveTypeToConvert& curve)
+{
+ typedef typename Polynomial::t_point_t t_point_t;
+ typedef typename Polynomial::num_t num_t;
+ t_point_t coefficients;
+ curveTypeToConvert current (curve);
+ coefficients.push_back(curve(curve.min()));
+ num_t T = curve.max()-curve.min();
+ num_t T_div = 1.0;
+ num_t fact = 1;
+ for(std::size_t i = 1; i<= curve.degree_; ++i)
+ {
+ fact *= (num_t)i;
+ coefficients.push_back(current.derivate(current.min(),i)/fact);
+ }
+ return Polynomial(coefficients,curve.min(),curve.max());
+}
+
+
+/// \brief Converts a cubic hermite spline or polynomial to a cubic bezier curve.
+/// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert.
+/// \return the equivalent cubic bezier curve.
+template<typename Bezier, typename curveTypeToConvert>
+Bezier bezier_from_curve(const curveTypeToConvert& curve)
+{
+ typedef typename Bezier::point_t point_t;
+ typedef typename Bezier::t_point_t t_point_t;
+ typedef typename Bezier::num_t num_t;
+ curveTypeToConvert current (curve);
+
+ num_t T_min = current.min();
+ num_t T_max = current.max();
+ num_t T = T_max-T_min;
+
+ // Positions and derivatives
+ point_t p0 = current(T_min);
+ point_t p1 = current(T_max);
+ point_t m0 = current.derivate(T_min,1);
+ point_t m1 = current.derivate(T_max,1);
+
+ // Convert to bezier control points
+ // for t in [Tmin,Tmax] and T=Tmax-Tmin : x'(0)=3(b_p1-b_p0)/T and x'(1)=3(b_p3-b_p2)/T
+ // so : m0=3(b_p1-b_p0)/T and m1=3(b_p3-b_p2)/T
+ // <=> b_p1=T(m0/3)+b_p0 and b_p2=-T(m1/3)+b_p3
+ point_t b_p0 = p0;
+ point_t b_p3 = p1;
+ point_t b_p1 = T*m0/3+b_p0;
+ point_t b_p2 = -T*m1/3+b_p3;
+
+
+ t_point_t control_points;
+ control_points.push_back(b_p0);
+ control_points.push_back(b_p1);
+ control_points.push_back(b_p2);
+ control_points.push_back(b_p3);
+
+ return Bezier(control_points.begin(), control_points.end(), current.min(), current.max());
+}
+
+/// \brief Converts a polynomial of order 3 or less/cubic bezier curve to a cubic hermite spline.
+/// \param curve : the polynomial of order 3 or less/cubic bezier curve defined between [Tmin,Tmax] to convert.
+/// \return the equivalent cubic hermite spline.
+template<typename Hermite, typename curveTypeToConvert>
+Hermite hermite_from_curve(const curveTypeToConvert& curve)
+{
+ typedef typename Hermite::pair_point_tangent_t pair_point_tangent_t;
+ typedef typename Hermite::t_pair_point_tangent_t t_pair_point_tangent_t;
+ typedef typename Hermite::point_t point_t;
+ typedef typename Hermite::num_t num_t;
+ curveTypeToConvert current (curve);
+
+ num_t T_min = current.min();
+ num_t T_max = current.max();
+ num_t T = T_max-T_min;
+
+ // Positions and derivatives
+ point_t p0 = current(T_min);
+ point_t p1 = current(T_max);
+ point_t m0 = current.derivate(T_min,1);
+ point_t m1 = current.derivate(T_max,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);
+
+ std::vector< double > time_control_points;
+ time_control_points.push_back(T_min);
+ time_control_points.push_back(T_max);
+
+ return Hermite(control_points.begin(), control_points.end(), time_control_points);
+}
+
+} // namespace curve
+#endif //_CLASS_CURVE_CONVERSION
\ No newline at end of file
diff --git a/include/curves/exact_cubic.h b/include/curves/exact_cubic.h
index 693109c02c72b580fb0fdbcf7dc3f47e9620632b..cbbcbf52ed934813c02792c4bcf87ecce7322e40 100644
--- a/include/curves/exact_cubic.h
+++ b/include/curves/exact_cubic.h
@@ -23,6 +23,7 @@
#include "curve_abc.h"
#include "cubic_spline.h"
#include "quintic_spline.h"
+#include "curve_constraint.h"
#include "MathDefs.h"
@@ -37,12 +38,13 @@ 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 SplineBase=polynom<Time, Numeric, Dim, Safe, Point, T_Point> >
+, typename SplineBase=polynomial<Time, Numeric, Dim, Safe, Point, T_Point> >
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+ typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
typedef Time time_t;
typedef Numeric num_t;
typedef SplineBase spline_t;
@@ -50,6 +52,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef typename t_spline_t::iterator it_spline_t;
typedef typename t_spline_t::const_iterator cit_spline_t;
typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
+ typedef curve_constraints<point_t> spline_constraints;
/* Constructors - destructors */
public:
@@ -61,6 +64,14 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
: curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
+ /// \brief Constructor.
+ /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
+ /// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
+ /// \param constraints : constraints on the init and end velocity / accelerations of the spline.
+ ///
+ template<typename In>
+ exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
+ : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
/// \brief Constructor.
/// \param subSplines: vector of subsplines.
@@ -74,14 +85,30 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Destructor.
virtual ~exact_cubic(){}
+ std::size_t getNumberSplines()
+ {
+ return subSplines_.size();
+ }
+
+ spline_t getSplineAt(std::size_t index)
+ {
+ return subSplines_.at(index);
+ }
+
private:
+ /// \brief Compute polynom of exact cubic spline from waypoints.
+ /// Compute the coefficients of polynom as in paper : "Task-Space Trajectories via Cubic Spline Optimization".<br>
+ /// \f$x_i(t)=a_i+b_i(t-t_i)+c_i(t-t_i)^2\f$<br>
+ /// with \f$a=x\f$, \f$H_1b=H_2x\f$, \f$c=H_3x+H_4b\f$, \f$d=H_5x+H_6b\f$.<br>
+ /// The matrices \f$H\f$ are defined as in the paper in Appendix A.
+ ///
template<typename In>
t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
{
std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
if(Safe && size < 1)
{
- throw; // TODO
+ throw std::length_error("size of waypoints must be superior to 0") ; // TODO
}
t_spline_t subSplines; subSplines.reserve(size);
@@ -103,6 +130,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
In it(wayPointsBegin), next(wayPointsBegin);
++next;
+ // Fill the matrices H as specified in the paper.
for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
{
num_t const dTi((*next).first - (*it).first);
@@ -131,25 +159,94 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
h2(i+1, i+2) = 6 / dTi_1sqr;
}
x.row(i)= (*it).second.transpose();
- }
+ }
// adding last x
x.row(size-1)= (*it).second.transpose();
- a= x;
+ // Compute coefficients of polynom.
+ a = x;
PseudoInverse(h1);
b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
c = h3 * x + h4 * b;
d = h5 * x + h6 * b;
+ // create splines along waypoints.
it= wayPointsBegin, next=wayPointsBegin; ++ next;
for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
{
subSplines.push_back(
create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first));
}
+ /*
subSplines.push_back(
create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
+ */
return subSplines;
}
+ template<typename In>
+ t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const
+ {
+ std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
+ if(Safe && size < 1)
+ {
+ throw std::length_error("number of waypoints should be superior to one"); // TODO
+ }
+ t_spline_t subSplines;
+ subSplines.reserve(size-1);
+ spline_constraints cons = constraints;
+ In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd-1);
+ ++next;
+ for(std::size_t i(0); next != end; ++next, ++it, ++i)
+ {
+ compute_one_spline<In>(it, next, cons, subSplines);
+ }
+ compute_end_spline<In>(it, next,cons, subSplines);
+ return subSplines;
+ }
+
+ template<typename In>
+ void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
+ {
+ const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
+ const point_t& b0 = constraints.init_vel , c0 = constraints.init_acc / 2.;
+ const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
+ const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
+ const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
+ subSplines.push_back(create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>
+ (a0,b0,c0,d0,init_t, end_t));
+ constraints.init_vel = subSplines.back().derivate(end_t, 1);
+ constraints.init_acc = subSplines.back().derivate(end_t, 2);
+ }
+
+ template<typename In>
+ void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
+ {
+ const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
+ const point_t& b0 = constraints.init_vel, b1 = constraints.end_vel,
+ c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
+ const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
+ const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt, dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
+ //solving a system of four linear eq with 4 unknows: d0, e0
+ const point_t alpha_0 = a1 - a0 - b0 *dt - c0 * dt_2;
+ const point_t alpha_1 = b1 - b0 - 2 *c0 * dt;
+ const point_t alpha_2 = c1 - 2 *c0;
+ const num_t x_d_0 = dt_3, x_d_1 = 3*dt_2, x_d_2 = 6*dt;
+ const num_t x_e_0 = dt_4, x_e_1 = 4*dt_3, x_e_2 = 12*dt_2;
+ const num_t x_f_0 = dt_5, x_f_1 = 5*dt_4, x_f_2 = 20*dt_3;
+
+ point_t d, e, f;
+ MatrixX rhs = MatrixX::Zero(3,Dim);
+ rhs.row(0) = alpha_0; rhs.row(1) = alpha_1; rhs.row(2) = alpha_2;
+ Matrix3 eq = Matrix3::Zero(3,3);
+ eq(0,0) = x_d_0; eq(0,1) = x_e_0; eq(0,2) = x_f_0;
+ eq(1,0) = x_d_1; eq(1,1) = x_e_1; eq(1,2) = x_f_1;
+ eq(2,0) = x_d_2; eq(2,1) = x_e_2; eq(2,2) = x_f_2;
+ rhs = eq.inverse().eval() * rhs;
+ d = rhs.row(0); e = rhs.row(1); f = rhs.row(2);
+
+ subSplines.push_back(create_quintic<Time,Numeric,Dim,Safe,Point,T_Point>
+ (a0,b0,c0,d,e,f, init_t, end_t));
+ }
+
private:
//exact_cubic& operator=(const exact_cubic&);
/* Constructors - destructors */
@@ -158,32 +255,42 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : time when to evaluate the spline
- /// \return Point corresponding on spline at time t.
+ /// \return \f$x(t)\f$ point corresponding on spline at time t.
///
virtual point_t operator()(const time_t t) const
{
- if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
+ if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max()))
+ {
+ throw std::out_of_range("time t to evaluate should be in range [Tmin, Tmax] of the spline");
+ }
for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
if( (t >= (it->min()) && t <= (it->max())) || it+1 == subSplines_.end())
+ {
return it->operator()(t);
+ }
}
// this should not happen
throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
}
- /// \brief Evaluation of the derivative spline at time t.
+ /// \brief Evaluate the derivative of order N of spline at time t.
/// \param t : time when to evaluate the spline.
- /// \param order : order of the derivative.
- /// \return Point corresponding on derivative spline at time t.
+ /// \param order : order of derivative.
+ /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
///
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");}
+ if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max()))
+ {
+ throw std::out_of_range("time t to evaluate should be in range [Tmin, Tmax] of the spline");
+ }
for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
if( (t >= (it->min()) && t <= (it->max())) || it+1 == subSplines_.end())
+ {
return it->derivate(t, order);
+ }
}
// this should not happen
throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
@@ -192,7 +299,11 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/
public:
+ /// \brief Get the minimum time for which the curve is defined
+ /// \return \f$t_{min}\f$ lower bound of time range.
num_t virtual min() const{return subSplines_.front().min();}
+ /// \brief Get the maximum time for which the curve is defined.
+ /// \return \f$t_{max}\f$ upper bound of time range.
num_t virtual max() const{return subSplines_.back().max();}
/*Helpers*/
diff --git a/include/curves/helpers/effector_spline.h b/include/curves/helpers/effector_spline.h
index 9d9cd4216a63b14c83515aa334203cabbbcb68de..2a8fb59784816f2d28fb3e25d25ec1c9b49b7b77 100644
--- a/include/curves/helpers/effector_spline.h
+++ b/include/curves/helpers/effector_spline.h
@@ -20,7 +20,7 @@
#ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE
-#include "curves/spline_deriv_constraint.h"
+#include "curves/exact_cubic.h"
namespace curves
{
@@ -32,26 +32,24 @@ typedef Eigen::Matrix<Numeric, 3, 1> Point;
typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Point;
typedef std::pair<double, Point> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
-typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point> spline_deriv_constraint_t;
-typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
-typedef spline_deriv_constraint_t::exact_cubic_t exact_cubic_t;
-typedef spline_deriv_constraint_t::t_spline_t t_spline_t;
-typedef spline_deriv_constraint_t::spline_t spline_t;
+typedef exact_cubic<Time, Numeric, 3, true, Point, T_Point> exact_cubic_t;
+typedef exact_cubic_t::spline_constraints spline_constraints_t;
+typedef exact_cubic_t::t_spline_t t_spline_t;
+typedef exact_cubic_t::spline_t spline_t;
+/// \brief Compute time such that the equation from source to offsetpoint is necessarily a line.
Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset)
{
- //compute time such that the equation from source to offsetpoint is necessarily a line
Numeric norm = normal.norm();
assert(norm>0.);
return std::make_pair(source.first + time_offset,(source.second + normal / norm* offset));
}
-
+/// \brief Compute spline from land way point to end point.
+/// Constraints are null velocity and acceleration.
spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset,
const Time init_time, const Time time_offset)
{
- // compute spline from land way point to end point
- // constraints are null velocity and acceleration
Numeric norm = normal.norm();
assert(norm>0.);
Point n = normal / norm;
@@ -68,9 +66,9 @@ spline_t make_end_spline(const Point& normal, const Point& from, const Numeric o
return spline_t(points.begin(), points.end(), init_time, init_time+time_offset);
}
+/// \brief Compute end velocity : along landing normal and respecting time.
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline, const Time /*time_offset*/)
{
- //computing end velocity: along landing normal and respecting time
spline_constraints_t constraints;
constraints.end_acc = end_spline.derivate(end_spline.min(),2);
constraints.end_vel = end_spline.derivate(end_spline.min(),1);
@@ -112,7 +110,7 @@ exact_cubic_t* effector_spline(
//specifying end velocity constraint such that landing will be in straight line
spline_t end_spline=make_end_spline(land_normal,landWaypoint.second,land_offset,landWaypoint.first,land_offset_duration);
spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline,land_offset_duration);
- spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(),constraints);
+ exact_cubic_t all_but_end(waypoints.begin(), waypoints.end(),constraints);
t_spline_t splines = all_but_end.subSplines_;
splines.push_back(end_spline);
return new exact_cubic_t(splines);
diff --git a/include/curves/helpers/effector_spline_rotation.h b/include/curves/helpers/effector_spline_rotation.h
index 9d7ff42239394273ba0bd11ffa70678f79db0c10..38e9266975fdbcc3543249ba59d2eb5de56d7b9a 100644
--- a/include/curves/helpers/effector_spline_rotation.h
+++ b/include/curves/helpers/effector_spline_rotation.h
@@ -37,7 +37,7 @@ typedef curve_abc<Time, Numeric, 4, false, quat_t> curve_abc_quat_t;
typedef std::pair<Numeric, quat_t > waypoint_quat_t;
typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
-typedef spline_deriv_constraint <Numeric, Numeric, 1, false, point_one_dim_t> spline_deriv_constraint_one_dim;
+typedef exact_cubic <Numeric, Numeric, 1, false, point_one_dim_t> exact_cubic_constraint_one_dim;
typedef std::pair<Numeric, point_one_dim_t > waypoint_one_dim_t;
typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
@@ -60,7 +60,7 @@ class rotation_spline: public curve_abc_quat_t
quat_from_ = from.quat_from_;
quat_to_ = from.quat_to_;
min_ =from.min_; max_ = from.max_;
- time_reparam_ = spline_deriv_constraint_one_dim(from.time_reparam_);
+ time_reparam_ = exact_cubic_constraint_one_dim(from.time_reparam_);
return *this;
}
/* Copy Constructors / operator=*/
@@ -80,13 +80,13 @@ class rotation_spline: public curve_abc_quat_t
throw std::runtime_error("TODO quaternion spline does not implement derivate");
}
- spline_deriv_constraint_one_dim computeWayPoints() const
+ /// \brief Initialize time reparametrization for spline.
+ exact_cubic_constraint_one_dim computeWayPoints() const
{
- // initializing time reparametrization for spline
t_waypoint_one_dim_t waypoints;
waypoints.push_back(std::make_pair(0,point_one_dim_t::Zero()));
waypoints.push_back(std::make_pair(1,point_one_dim_t::Ones()));
- return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end());
+ return exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end());
}
virtual time_t min() const{return min_;}
@@ -97,7 +97,7 @@ class rotation_spline: public curve_abc_quat_t
Eigen::Quaterniond quat_to_; //const
double min_; //const
double max_; //const
- spline_deriv_constraint_one_dim time_reparam_; //const
+ exact_cubic_constraint_one_dim time_reparam_; //const
};
@@ -223,7 +223,9 @@ class effector_spline_rotation
exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const
{
if(std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1)
+ {
return simple_quat_spline();
+ }
exact_cubic_quat_t::t_spline_t splines;
InQuat it(quatWayPointsBegin);
Time init = time_lift_offset_;
diff --git a/include/curves/linear_variable.h b/include/curves/linear_variable.h
index 5a1dc4961e2fa0f14060d95f9dc4bafc253e31bc..14ea20ed19a520e9b68ed19dd95115283b310dbd 100644
--- a/include/curves/linear_variable.h
+++ b/include/curves/linear_variable.h
@@ -75,13 +75,16 @@ struct variables{
variables& operator+=(const variables& w1)
{
if(variables_.size() == 0)
+ {
variables_ = w1.variables_;
- else if (w1.variables_.size() !=0)
+ } else if (w1.variables_.size() !=0)
{
assert(variables_.size() == w1.variables_.size());
CIT_var_t cit = w1.variables_.begin();
for(IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit)
+ {
(*it)+=(*cit);
+ }
}
return *this;
}
@@ -89,13 +92,16 @@ struct variables{
variables& operator-=(const variables& w1)
{
if(variables_.size() == 0)
+ {
variables_ = w1.variables_;
- else if (w1.variables_.size() !=0)
+ } else if (w1.variables_.size() !=0)
{
assert(variables_.size() == w1.variables_.size());
CIT_var_t cit = w1.variables_.begin();
for(IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit)
+ {
(*it)-=(*cit);
+ }
}
return *this;
}
@@ -137,14 +143,20 @@ template<typename V>
variables<V> operator+(const variables<V>& w1, const variables<V>& w2)
{
if(w2.variables_.size() == 0)
+ {
return w1;
+ }
if(w1.variables_.size() == 0)
+ {
return w2;
+ }
variables<V> res;
assert(w2.variables_.size() == w1.variables_.size());
typename variables<V>::CIT_var_t cit = w1.variables_.begin();
for(typename variables<V>::CIT_var_t cit2 = w2.variables_.begin(); cit2 != w2.variables_.end(); ++cit, ++cit2)
+ {
res.variables_.push_back((*cit)+(*cit2));
+ }
return res;
}
@@ -152,14 +164,20 @@ template<typename V>
variables<V> operator-(const variables<V>& w1, const variables<V>& w2)
{
if(w2.variables_.size() == 0)
+ {
return w1;
+ }
if(w1.variables_.size() == 0)
+ {
return w2;
+ }
variables<V> res;
assert(w2.variables_.size() == w1.variables_.size());
typename variables<V>::CIT_var_t cit = w1.variables_.begin();
for(typename variables<V>::CIT_var_t cit2 = w2.variables_.begin(); cit2 != w2.variables_.end(); ++cit, ++cit2)
+ {
res.variables_.push_back((*cit)-(*cit2));
+ }
return res;
}
@@ -167,10 +185,14 @@ template<typename V>
variables<V> operator*(const double k, const variables<V>& w)
{
if(w.variables_.size() == 0)
+ {
return w;
+ }
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
+ {
res.variables_.push_back(k*(*cit));
+ }
return res;
}
@@ -178,10 +200,14 @@ template<typename V>
variables<V> operator*(const variables<V>& w,const double k)
{
if(w.variables_.size() == 0)
+ {
return w;
+ }
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
+ {
res.variables_.push_back((*cit)*k);
+ }
return res;
}
@@ -189,10 +215,14 @@ template<typename V>
variables<V> operator/(const variables<V>& w,const double k)
{
if(w.variables_.size() == 0)
+ {
return w;
+ }
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
+ {
res.variables_.push_back((*cit)/k);
+ }
return res;
}
diff --git a/include/curves/optimization/OptimizeSpline.h b/include/curves/optimization/OptimizeSpline.h
index 1107a582fceaec594cab7aec490d2f76201425cd..85fbb644750ed000830be72f588b28be3a8e88cf 100644
--- a/include/curves/optimization/OptimizeSpline.h
+++ b/include/curves/optimization/OptimizeSpline.h
@@ -125,7 +125,7 @@ inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
int const size((int)std::distance(wayPointsBegin, wayPointsEnd));
if(Safe && size < 1)
{
- throw; // TODO
+ throw std::length_error("can not generate optimizedCurve, number of waypoints should be superior to one");; // TODO
}
// refer to the paper to understand all this.
MatrixX h1 = MatrixX::Zero(size, size);
@@ -258,7 +258,7 @@ inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
for(int j = 0; j<Dim; ++j)
{
int id = i + j;
- h3x_h4x.block(id, j*size , 1, size) = h3.row(i%3);
+ h3x_h4x.block(id, j*size , 1, size) = h3.row(i%3);
h3x_h4x.block(id, j*size + ptOff , 1, size) = h4.row(i%3);
h3x_h4x.block(id, j*size + ptptOff, 1, size) = MatrixX::Ones(1, size) * -2;
}
@@ -430,56 +430,67 @@ inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
/* Append 'numcon' empty constraints.
The constraints will initially have no bounds. */
if ( r == MSK_RES_OK )
+ {
r = MSK_appendcons(task,numcon);
+ }
/* Append 'numvar' variables.
The variables will initially be fixed at zero (x=0). */
if ( r == MSK_RES_OK )
+ {
r = MSK_appendvars(task,numvar);
+ }
for(int j=0; j<numvar && r == MSK_RES_OK; ++j)
{
- /* Set the linear term c_j in the objective.*/
- if(r == MSK_RES_OK)
- r = MSK_putcj(task,j,0);
-
+ /* Set the linear term c_j in the objective.*/
+ if(r == MSK_RES_OK)
+ {
+ r = MSK_putcj(task,j,0);
+ }
/* Set the bounds on variable j.
blx[j] <= x_j <= bux[j] */
if(r == MSK_RES_OK)
- r = MSK_putvarbound(task,
- j, /* Index of variable.*/
- bkx[j], /* Bound key.*/
- blx[j], /* Numerical value of lower bound.*/
- bux[j]); /* Numerical value of upper bound.*/
-
+ {
+ r = MSK_putvarbound(task,
+ j, /* Index of variable.*/
+ bkx[j], /* Bound key.*/
+ blx[j], /* Numerical value of lower bound.*/
+ bux[j]); /* Numerical value of upper bound.*/
+ }
/* Input column j of A */
if(r == MSK_RES_OK)
- r = MSK_putacol(task,
- j, /* Variable (column) index.*/
- aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
- asub+aptrb[j], /* Pointer to row indexes of column j.*/
- aval+aptrb[j]); /* Pointer to Values of column j.*/
+ {
+ r = MSK_putacol(task,
+ j, /* Variable (column) index.*/
+ aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
+ asub+aptrb[j], /* Pointer to row indexes of column j.*/
+ aval+aptrb[j]); /* Pointer to Values of column j.*/
+ }
}
/* Set the bounds on constraints.
for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */
for(int i=0; i<numcon && r == MSK_RES_OK; ++i)
+ {
r = MSK_putconbound(task,
i, /* Index of constraint.*/
bkc[i], /* Bound key.*/
blc[i], /* Numerical value of lower bound.*/
buc[i]); /* Numerical value of upper bound.*/
+ }
/* Maximize objective function. */
if (r == MSK_RES_OK)
+ {
r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
+ }
if ( r==MSK_RES_OK )
{
- /* Input the Q for the objective. */
-
- r = MSK_putqobj(task,numqz,qsubi,qsubj,qval);
+ /* Input the Q for the objective. */
+ r = MSK_putqobj(task,numqz,qsubi,qsubj,qval);
}
if ( r==MSK_RES_OK )
diff --git a/include/curves/piecewise_polynomial_curve.h b/include/curves/piecewise_polynomial_curve.h
new file mode 100644
index 0000000000000000000000000000000000000000..203bbd5b934f32e0fcfc4aa47dbfbd3c6eb6750f
--- /dev/null
+++ b/include/curves/piecewise_polynomial_curve.h
@@ -0,0 +1,195 @@
+/**
+* \file piecewise_polynomial_curve.h
+* \brief class allowing to create a piecewise polynomial curve.
+* \author Jason C.
+* \date 05/2019
+*/
+
+#ifndef _CLASS_PIECEWISE_CURVE
+#define _CLASS_PIECEWISE_CURVE
+
+#include "curve_abc.h"
+#include "polynomial.h"
+#include "curve_conversion.h"
+
+namespace curves
+{
+/// \class PiecewiseCurve.
+/// \brief Represent a piecewise polynomial curve. We can add some new polynomials to the curve,
+/// but the starting time of the polynomial to add should be equal to the ending time of the
+/// piecewise_polynomial_curve.<br>\ Example : A piecewise polynomial curve composed of three polynomials pol_0,
+/// pol_1 and pol_2 where pol_0 is defined between \f$[T0_{min},T0_{max}]\f$, pol_1 between
+/// \f$[T0_{max},T1_{max}]\f$ and pol_2 between \f$[T1_{max},T2_{max}]\f$.
+/// On the piecewise polynomial curve, pol_0 is located between \f$[T0_{min},T0_{max}[\f$,
+/// pol_1 between \f$[T0_{max},T1_{max}[\f$ and pol_2 between \f$[T1_{max},T2_{max}]\f$.
+///
+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> >
+ >
+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 polynomial <double, double, 3, true, point_t, t_point_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:
+
+ /// \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;
+ add_polynomial_curve(pol);
+ time_polynomial_curves_.push_back(pol.min());
+ T_min_ = pol.min();
+ }
+
+ virtual ~piecewise_polynomial_curve(){}
+
+ virtual Point operator()(const Time t) const
+ {
+ if(Safe &! (T_min_ <= t && t <= T_max_))
+ {
+ //std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
+ throw std::out_of_range("can't evaluate piecewise curve, out of range");
+ }
+ return polynomial_curves_.at(find_interval(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(find_interval(t))).derivate(t, order);
+ }
+
+ /// \brief Add a new polynomial to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$
+ /// is equal to \f$T_{max}\f$ of the actual piecewise curve.
+ /// \param pol : polynomial to add.
+ ///
+ void add_polynomial_curve(polynomial_t pol)
+ {
+ // Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
+ if (size_!=0 && pol.min()!=T_max_)
+ {
+ throw std::out_of_range("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
+ }
+ polynomial_curves_.push_back(pol);
+ size_ = polynomial_curves_.size();
+ T_max_ = pol.max();
+ time_polynomial_curves_.push_back(T_max_);
+ }
+
+ /// \brief Check if the curve is continuous of order given.
+ /// \param order : order of continuity we want to check.
+ /// \return True if the curve is continuous of order given.
+ ///
+ bool is_continuous(const std::size_t order)
+ {
+ double margin = 0.001;
+ bool isContinuous = true;
+ std::size_t 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(next.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.
+ ///
+ std::size_t find_interval(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;
+ }
+
+ std::size_t left_id = 0;
+ std::size_t right_id = size_-1;
+ while(left_id <= right_id)
+ {
+ const std::size_t 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 ]
+ std::size_t size_; // Number of segments in piecewise curve = size of polynomial_curves_
+ Time T_min_, T_max_;
+};
+
+} // end namespace
+
+
+#endif // _CLASS_PIECEWISE_CURVE
\ No newline at end of file
diff --git a/include/curves/polynom.h b/include/curves/polynomial.h
similarity index 54%
rename from include/curves/polynom.h
rename to include/curves/polynomial.h
index 2fd55ae14a0a1194b1d6b3a39be8460cd0e1503c..b6ef4c3d876785df075ce82d0121c8fdfeb2dea4 100644
--- a/include/curves/polynom.h
+++ b/include/curves/polynomial.h
@@ -1,18 +1,18 @@
/**
-* \file polynom.h
+* \file polynomial.h
* \brief Definition of a cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
-* This file contains definitions for the polynom struct.
+* This file contains definitions for the polynomial struct.
* It allows the creation and evaluation of natural
* smooth splines of arbitrary dimension and order
*/
-#ifndef _STRUCT_POLYNOM
-#define _STRUCT_POLYNOM
+#ifndef _STRUCT_POLYNOMIAL
+#define _STRUCT_POLYNOMIAL
#include "MathDefs.h"
@@ -25,14 +25,15 @@
namespace curves
{
-/// \class polynom
-/// \brief Represents a polynomf arbitrary order defined on the interval
-/// [tBegin, tEnd]. It follows the equation
-/// x(t) = a + b(t - t_min_) + ... + d(t - t_min_)^N, where N is the order
+/// \class polynomial.
+/// \brief Represents a polynomial of an arbitrary order defined on the interval
+/// \f$[t_{min}, t_{max}]\f$. It follows the equation :<br>
+/// \f$ x(t) = a + b(t - t_{min}) + ... + d(t - t_{min})^N \f$<br>
+/// where N is the order and \f$ t \in [t_{min}, t_{max}] \f$.
///
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> > >
-struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
+struct polynomial : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
@@ -44,26 +45,26 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
/* Constructors - destructors */
public:
- ///\brief Constructor
- ///\param coefficients : a reference to an Eigen matrix where each column is a coefficient,
+ /// \brief Constructor.
+ /// \param coefficients : a reference to an Eigen matrix where each column is a coefficient,
/// from the zero order coefficient, up to the highest order. Spline order is given
/// by the number of the columns -1.
- ///\param min: LOWER bound on interval definition of the spline
- ///\param max: UPPER bound on interval definition of the spline
- polynom(const coeff_t& coefficients, const time_t min, const time_t max)
+ /// \param min : LOWER bound on interval definition of the curve.
+ /// \param max : UPPER bound on interval definition of the curve.
+ polynomial(const coeff_t& coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(coefficients), dim_(Dim), order_(coefficients_.cols()-1), t_min_(min), t_max_(max)
{
safe_check();
}
- ///\brief Constructor
- ///\param coefficients : a container containing all coefficients of the spline, starting
- /// with the zero order coefficient, up to the highest order. Spline order is given
- /// by the size of the coefficients
- ///\param min: LOWER bound on interval definition of the spline
- ///\param max: UPPER bound on interval definition of the spline
- polynom(const T_Point& coefficients, const time_t min, const time_t max)
+ /// \brief Constructor
+ /// \param coefficients : a container containing all coefficients of the spline, starting
+ /// with the zero order coefficient, up to the highest order. Spline order is given
+ /// by the size of the coefficients.
+ /// \param min : LOWER bound on interval definition of the spline.
+ /// \param max : UPPER bound on interval definition of the spline.
+ polynomial(const T_Point& coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
dim_(Dim), order_(coefficients_.cols()-1), t_min_(min), t_max_(max)
@@ -71,33 +72,33 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
safe_check();
}
- ///\brief Constructor
- ///\param zeroOrderCoefficient : an iterator pointing to the first element of a structure containing the coefficients
- /// it corresponds to the zero degree coefficient
- ///\param out : an iterator pointing to the last element of a structure ofcoefficients
- ///\param min: LOWER bound on interval definition of the spline
- ///\param max: UPPER bound on interval definition of the spline
+ /// \brief Constructor.
+ /// \param zeroOrderCoefficient : an iterator pointing to the first element of a structure containing the coefficients
+ /// it corresponds to the zero degree coefficient.
+ /// \param out : an iterator pointing to the last element of a structure ofcoefficients.
+ /// \param min : LOWER bound on interval definition of the spline.
+ /// \param max : UPPER bound on interval definition of the spline.
template<typename In>
- polynom(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
+ polynomial(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
:coefficients_(init_coeffs(zeroOrderCoefficient, out)),
dim_(Dim), order_(coefficients_.cols()-1), t_min_(min), t_max_(max)
{
safe_check();
}
- ///\brief Destructor
- ~polynom()
+ /// \brief Destructor
+ ~polynomial()
{
// NOTHING
}
- polynom(const polynom& other)
+ polynomial(const polynomial& other)
: coefficients_(other.coefficients_),
dim_(other.dim_), order_(other.order_), t_min_(other.t_min_), t_max_(other.t_max_){}
- //polynom& operator=(const polynom& other);
+ //polynomial& operator=(const polynomial& other);
private:
void safe_check()
@@ -105,9 +106,13 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
if(Safe)
{
if(t_min_ > t_max_)
- std::out_of_range("TODO");
+ {
+ std::out_of_range("Tmin should be inferior to Tmax");
+ }
if(coefficients_.size() != int(order_+1))
+ {
std::runtime_error("Spline order and coefficients do not match");
+ }
}
}
@@ -117,7 +122,7 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
public:
/*/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
- /// \param return : the value x(t)
+ /// \param return \f$x(t)\f$, point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const
{
if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
@@ -131,31 +136,41 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Evaluation of the cubic spline at time t using horner's scheme.
- /// \param t : the time when to evaluate the spine
- /// \param return : the value x(t)
+ /// \param t : time when to evaluate the spline.
+ /// \return \f$x(t)\f$ point corresponding on spline at time t.
virtual point_t operator()(const time_t t) const
{
- if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
+ if((t < t_min_ || t > t_max_) && Safe)
+ {
+ 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_);
for(int i=(int)(order_-1); i>=0; i--)
+ {
h = dt*h + coefficients_.col(i);
+ }
return h;
}
- /// \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)
+ /// \brief Evaluation of the derivative of order N of spline at time t.
+ /// \param t : the time when to evaluate the spline.
+ /// \param order : order of derivative.
+ /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t.
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");}
+ if((t < t_min_ || t > t_max_) && Safe)
+ {
+ throw std::out_of_range("error in polynomial : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
+ }
time_t const dt (t-t_min_);
time_t cdt(1);
point_t currentPoint_ = point_t::Zero();
- for(int i = (int)(order); i < (int)(order_+1); ++i, cdt*=dt)
+ for(int i = (int)(order); i < (int)(order_+1); ++i, cdt*=dt)
+ {
currentPoint_ += cdt *coefficients_.col(i) * fact(i, order);
+ }
return currentPoint_;
}
@@ -164,7 +179,9 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
num_t res(1);
for(std::size_t i = 0; i < order; ++i)
+ {
res *= (num_t)(n-i);
+ }
return res;
}
@@ -172,9 +189,11 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/
public:
- /// \brief Returns the minimum time for wich curve is defined
+ /// \brief Get the minimum time for which the curve is defined
+ /// \return \f$t_{min}\f$ lower bound of time range.
num_t virtual min() const {return t_min_;}
- /// \brief Returns the maximum time for wich curve is defined
+ /// \brief Get the maximum time for which the curve is defined.
+ /// \return \f$t_{max}\f$ upper bound of time range.
num_t virtual max() const {return t_max_;}
/*Helpers*/
@@ -195,10 +214,12 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
coeff_t res = coeff_t(Dim, size); int i = 0;
for(In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; ++cit, ++i)
+ {
res.col(i) = *cit;
+ }
return res;
}
-}; //class polynom
+}; //class polynomial
} // namespace curves
-#endif //_STRUCT_POLYNOM
+#endif //_STRUCT_POLYNOMIAL
diff --git a/include/curves/quintic_spline.h b/include/curves/quintic_spline.h
index 3ff0cf585a4f70976ccb35ea2cac46625ca84a3b..4bfa35a48a3f0bcef05dd3cb38d6f341b61bd96c 100644
--- a/include/curves/quintic_spline.h
+++ b/include/curves/quintic_spline.h
@@ -16,15 +16,15 @@
#include "MathDefs.h"
-#include "polynom.h"
+#include "polynomial.h"
#include <stdexcept>
namespace curves
{
/// \brief Creates coefficient vector of a quintic spline defined on the interval
-/// \f$[t_{min}, t_{max}]\f$. It follows the equation :
-/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 + e(t - t_{min})^4 + f(t - t_{min})^5 \f$
+/// \f$[t_{min}, t_{max}]\f$. It follows the equation :<br>
+/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 + e(t - t_{min})^4 + f(t - t_{min})^5 \f$ <br>
/// where \f$ t \in [t_{min}, t_{max}] \f$.
///
template<typename Point, typename T_Point>
@@ -38,11 +38,11 @@ T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c,
}
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point>
-polynom<Time,Numeric,Dim,Safe,Point,T_Point> create_quintic(Point const& a, Point const& b, Point const& c, Point const &d, Point const &e, Point const &f,
+polynomial<Time,Numeric,Dim,Safe,Point,T_Point> create_quintic(Point const& a, Point const& b, Point const& c, Point const &d, Point const &e, Point const &f,
const Time t_min, const Time t_max)
{
T_Point coeffs = make_quintic_vector<Point, T_Point>(a,b,c,d,e,f);
- return polynom<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max);
+ return polynomial<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max);
}
} // namespace curves
#endif //_STRUCT_QUINTIC_SPLINE
diff --git a/include/curves/spline_deriv_constraint.h b/include/curves/spline_deriv_constraint.h
deleted file mode 100644
index 9f5c332371a37d89713e7d187cdbe6e3e587c87c..0000000000000000000000000000000000000000
--- a/include/curves/spline_deriv_constraint.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/**
-* \file exact_cubic.h
-* \brief class allowing to create an Exact cubic spline.
-* \author Steve T.
-* \version 0.1
-* \date 06/17/2013
-*
-* This file contains definitions for the ExactCubic class.
-* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
-* cubic splines fulfulling those 4 restrictions :
-* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
-* - x_i(t_i+1) = x_i+1* ;
-* - its derivative is continous at t_i+1
-* - its acceleration is continous at t_i+1
-* more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
-* By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
-*/
-
-
-#ifndef _CLASS_CUBICZEROVELACC
-#define _CLASS_CUBICZEROVELACC
-
-#include "exact_cubic.h"
-#include "curve_constraint.h"
-
-#include "MathDefs.h"
-
-#include <functional>
-#include <vector>
-
-namespace curves
-{
-/// \class spline_deriv_constraint.
-/// \brief Represents a set of cubic splines defining a continuous function
-/// crossing each of the waypoint given in its initialization. Additional constraints
-/// are used to increase the order of the last spline, to start and finish
-/// trajectory with user defined velocity and acceleration.
-///
-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 SplineBase=polynom<Time, Numeric, Dim, Safe, Point, T_Point> >
-struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase>
-{
- typedef Point point_t;
- typedef T_Point t_point_t;
- typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
- typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
- typedef Time time_t;
- typedef Numeric num_t;
- typedef polynom<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
- typedef exact_cubic<time_t, Numeric, Dim, Safe, point_t, t_point_t> exact_cubic_t;
- typedef typename std::vector<spline_t> t_spline_t;
- typedef typename t_spline_t::iterator it_spline_t;
- typedef typename t_spline_t::const_iterator cit_spline_t;
- typedef curve_constraints<point_t> spline_constraints;
-
- /* Constructors - destructors */
- public:
- /// \brief Constructor.
- /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
- /// \param wayPointsEnd : an iterator pointing to the last element of a waypoint container.
- /// \param constraints : constraints on the init and end velocity / accelerations of the spline.
- ///
- template<typename In>
- spline_deriv_constraint(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints = spline_constraints())
- : exact_cubic_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
-
- ///\brief Destructor
- virtual ~spline_deriv_constraint(){}
-
- ///\brief Copy Constructor
- spline_deriv_constraint(const spline_deriv_constraint& other)
- : exact_cubic_t(other.subSplines_) {}
-
- private:
- template<typename In>
- void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
- {
- const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
- const point_t& b0 = constraints.init_vel , c0 = constraints.init_acc / 2.;
- const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
- const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
- const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
- subSplines.push_back(create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>
- (a0,b0,c0,d0,init_t, end_t));
- constraints.init_vel = subSplines.back().derivate(end_t, 1);
- constraints.init_acc = subSplines.back().derivate(end_t, 2);
- }
-
- template<typename In>
- void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
- {
- const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
- const point_t& b0 = constraints.init_vel, b1 = constraints.end_vel,
- c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
- const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
- const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt, dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
- //solving a system of four linear eq with 4 unknows: d0, e0
- const point_t alpha_0 = a1 - a0 - b0 *dt - c0 * dt_2;
- const point_t alpha_1 = b1 - b0 - 2 *c0 * dt;
- const point_t alpha_2 = c1 - 2 *c0;
- const num_t x_d_0 = dt_3, x_d_1 = 3*dt_2, x_d_2 = 6*dt;
- const num_t x_e_0 = dt_4, x_e_1 = 4*dt_3, x_e_2 = 12*dt_2;
- const num_t x_f_0 = dt_5, x_f_1 = 5*dt_4, x_f_2 = 20*dt_3;
-
- point_t d, e, f;
- MatrixX rhs = MatrixX::Zero(3,Dim);
- rhs.row(0) = alpha_0; rhs.row(1) = alpha_1; rhs.row(2) = alpha_2;
- Matrix3 eq = Matrix3::Zero(3,3);
- eq(0,0) = x_d_0; eq(0,1) = x_e_0; eq(0,2) = x_f_0;
- eq(1,0) = x_d_1; eq(1,1) = x_e_1; eq(1,2) = x_f_1;
- eq(2,0) = x_d_2; eq(2,1) = x_e_2; eq(2,2) = x_f_2;
- rhs = eq.inverse().eval() * rhs;
- d = rhs.row(0); e = rhs.row(1); f = rhs.row(2);
-
- subSplines.push_back(create_quintic<Time,Numeric,Dim,Safe,Point,T_Point>
- (a0,b0,c0,d,e,f, init_t, end_t));
- }
-
- private:
- template<typename In>
- t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const
- {
- std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
- if(Safe && size < 1) throw; // TODO
- t_spline_t subSplines; subSplines.reserve(size-1);
- spline_constraints cons = constraints;
- In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd-1);
- ++next;
- for(std::size_t i(0); next != end; ++next, ++it, ++i)
- compute_one_spline<In>(it, next, cons, subSplines);
- compute_end_spline<In>(it, next,cons, subSplines);
- return subSplines;
- }
-
- private:
- /* Constructors - destructors */
-};
-} // namespace curves
-#endif //_CLASS_CUBICZEROVELACC
-
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 6c843d025a6396377ac5d5220c9409cd2681877a..182f6beb18a047b70130ba09bc7c1ca910fd3dd5 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -1,10 +1,11 @@
#include "curves/bezier_curve.h"
-#include "curves/polynom.h"
+#include "curves/polynomial.h"
#include "curves/exact_cubic.h"
-#include "curves/spline_deriv_constraint.h"
#include "curves/curve_constraint.h"
-#include "curves/bezier_polynom_conversion.h"
+#include "curves/curve_conversion.h"
#include "curves/bernstein.h"
+#include "curves/cubic_hermite_spline.h"
+#include "curves/piecewise_polynomial_curve.h"
#include "python_definitions.h"
#include "python_variables.h"
@@ -20,6 +21,8 @@
using namespace curves;
typedef double real;
typedef Eigen::Vector3d point_t;
+typedef Eigen::Vector3d tangent_t;
+typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> point6_t;
typedef Eigen::Matrix<double, 3, 1, 0, 3, 1> ret_point_t;
typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> ret_point6_t;
@@ -32,19 +35,20 @@ typedef std::pair<real, point_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef std::pair<real, point6_t> Waypoint6;
typedef std::vector<Waypoint6> T_Waypoint6;
+typedef std::vector<pair_point_tangent_t,Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
typedef curves::bezier_curve <real, real, 3, true, point_t> bezier3_t;
typedef curves::bezier_curve <real, real, 6, true, point6_t> bezier6_t;
-typedef curves::polynom <real, real, 3, true, point_t, t_point_t> polynom_t;
+typedef curves::polynomial <real, real, 3, true, point_t, t_point_t> polynomial_t;
+typedef curves::piecewise_polynomial_curve <real, real, 3, true, point_t, t_point_t> piecewise_polynomial_curve_t;
typedef curves::exact_cubic <real, real, 3, true, point_t, t_point_t> exact_cubic_t;
-typedef polynom_t::coeff_t coeff_t;
+typedef curves::cubic_hermite_spline <real, real, 3, true, point_t> cubic_hermite_spline_t;
+typedef polynomial_t::coeff_t coeff_t;
typedef std::pair<real, point_t> waypoint_t;
typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t;
typedef curves::Bern<double> bernstein_t;
-
-typedef curves::spline_deriv_constraint <real, real, 3, true, point_t, t_point_t> spline_deriv_constraint_t;
typedef curves::curve_constraints<point_t> curve_constraints_t;
typedef curves::curve_constraints<point6_t> curve_constraints6_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
@@ -52,77 +56,119 @@ typedef curves::curve_constraints<point6_t> curve_constraints6_t;
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynom_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynomial_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t)
namespace curves
{
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 */
-/*3D constructors */
+/*3D constructors bezier */
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 */
-/*6D constructors */
+/*END 3D constructors bezier */
+/*6D constructors bezier */
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, T_min, T_max) ;
+}
+/*END 6D constructors bezier */
+
+/* Wrap Cubic hermite spline */
+t_pair_point_tangent_t getPairsPointTangent(const point_list_t& points, const point_list_t& tangents)
{
- return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, ub) ;
+ t_pair_point_tangent_t res;
+ if (points.size() != tangents.size())
+ {
+ throw std::length_error("size of points and tangents must be the same");
+ }
+ for(int i =0;i<points.cols();++i)
+ {
+ res.push_back(pair_point_tangent_t(tangents.col(i), tangents.col(i)));
+ }
+ return res;
}
-/*END 6D constructors */
-polynom_t* wrapSplineConstructor(const coeff_t& array)
+cubic_hermite_spline_t* wrapCubicHermiteSplineConstructor(const point_list_t& points, const point_list_t& tangents, const time_waypoints_t& time_pts)
{
- return new polynom_t(array, 0., 1.);
+ t_pair_point_tangent_t ppt = getPairsPointTangent(points, tangents);
+ std::vector<real> time_control_pts;
+ for( int i =0; i<time_pts.size(); ++i)
+ {
+ time_control_pts.push_back(time_pts[i]);
+ }
+ return new cubic_hermite_spline_t(ppt.begin(), ppt.end(), time_control_pts);
}
+/* End wrap Cubic hermite spline */
+/* Wrap polynomial */
+polynomial_t* wrapSplineConstructor(const coeff_t& array)
+{
+ return new polynomial_t(array, 0., 1.);
+}
+/* End wrap polynomial */
+/* Wrap piecewise polynomial curve */
+piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveConstructor(const polynomial_t& pol)
+{
+ return new piecewise_polynomial_curve_t(pol);
+}
+/* end wrap piecewise polynomial curve */
+
+/* Wrap exact cubic spline */
t_waypoint_t getWayPoints(const coeff_t& array, const time_waypoints_t& time_wp)
{
t_waypoint_t res;
- for(int i =0;i<array.cols();++i)
+ for(int i =0;i<array.cols();++i)
+ {
res.push_back(std::make_pair(time_wp(i), array.col(i)));
+ }
return res;
}
@@ -135,7 +181,9 @@ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> wayPointsToList(const Bezier
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> res (dim, wps.size());
int col = 0;
for(cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++col)
+ {
res.block<dim,1>(0,col) = *cit;
+ }
return res;
}
@@ -145,17 +193,10 @@ exact_cubic_t* wrapExactCubicConstructor(const coeff_t& array, const time_waypoi
return new exact_cubic_t(wps.begin(), wps.end());
}
-
-spline_deriv_constraint_t* wrapSplineDerivConstraint(const coeff_t& array, const time_waypoints_t& time_wp, const curve_constraints_t& constraints)
-{
- t_waypoint_t wps = getWayPoints(array, time_wp);
- return new spline_deriv_constraint_t(wps.begin(), wps.end(),constraints);
-}
-
-spline_deriv_constraint_t* wrapSplineDerivConstraintNoConstraints(const coeff_t& array, const time_waypoints_t& time_wp)
+exact_cubic_t* wrapExactCubicConstructorConstraint(const coeff_t& array, const time_waypoints_t& time_wp, const curve_constraints_t& constraints)
{
t_waypoint_t wps = getWayPoints(array, time_wp);
- return new spline_deriv_constraint_t(wps.begin(), wps.end());
+ return new exact_cubic_t(wps.begin(), wps.end(), constraints);
}
point_t get_init_vel(const curve_constraints_t& c)
@@ -197,7 +238,7 @@ void set_end_acc(curve_constraints_t& c, const point_t& val)
{
c.end_acc = val;
}
-
+/* End wrap exact cubic spline */
BOOST_PYTHON_MODULE(curves)
@@ -286,27 +327,55 @@ BOOST_PYTHON_MODULE(curves)
/** END variable points bezier curve**/
- /** BEGIN spline curve function**/
- class_<polynom_t>("polynom", init<const polynom_t::coeff_t, const real, const real >())
+ /** BEGIN polynomial curve function**/
+ class_<polynomial_t>("polynomial", init<const polynomial_t::coeff_t, const real, const real >())
.def("__init__", make_constructor(&wrapSplineConstructor))
- .def("min", &polynom_t::min)
- .def("max", &polynom_t::max)
- .def("__call__", &polynom_t::operator())
- .def("derivate", &polynom_t::derivate)
+ .def("min", &polynomial_t::min)
+ .def("max", &polynomial_t::max)
+ .def("__call__", &polynomial_t::operator())
+ .def("derivate", &polynomial_t::derivate)
+ ;
+ /** END polynomial function**/
+
+ /** BEGIN piecewise polynomial curve function **/
+ class_<piecewise_polynomial_curve_t>
+ ("piecewise_polynomial_curve", no_init)
+ .def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor))
+ .def("min", &piecewise_polynomial_curve_t::min)
+ .def("max", &piecewise_polynomial_curve_t::max)
+ .def("__call__", &piecewise_polynomial_curve_t::operator())
+ .def("derivate", &piecewise_polynomial_curve_t::derivate)
+ .def("add_polynomial_curve", &piecewise_polynomial_curve_t::add_polynomial_curve)
+ .def("is_continuous", &piecewise_polynomial_curve_t::is_continuous)
;
- /** END cubic function**/
+ /** END piecewise polynomial curve function **/
/** BEGIN exact_cubic curve**/
class_<exact_cubic_t>
("exact_cubic", no_init)
.def("__init__", make_constructor(&wrapExactCubicConstructor))
+ .def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
.def("min", &exact_cubic_t::min)
.def("max", &exact_cubic_t::max)
.def("__call__", &exact_cubic_t::operator())
.def("derivate", &exact_cubic_t::derivate)
+ .def("getNumberSplines", &exact_cubic_t::getNumberSplines)
+ .def("getSplineAt", &exact_cubic_t::getSplineAt)
;
- /** END bezier curve**/
+ /** END exact_cubic curve**/
+
+
+ /** BEGIN cubic_hermite_spline **/
+ class_<cubic_hermite_spline_t>
+ ("cubic_hermite_spline", no_init)
+ .def("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
+ .def("min", &cubic_hermite_spline_t::min)
+ .def("max", &cubic_hermite_spline_t::max)
+ .def("__call__", &cubic_hermite_spline_t::operator())
+ .def("derivate", &cubic_hermite_spline_t::derivate)
+ ;
+ /** END cubic_hermite_spline **/
/** BEGIN curve constraints**/
@@ -319,29 +388,21 @@ BOOST_PYTHON_MODULE(curves)
;
/** END curve constraints**/
-
- /** BEGIN spline_deriv_constraints**/
- class_<spline_deriv_constraint_t>
- ("spline_deriv_constraint", no_init)
- .def("__init__", make_constructor(&wrapSplineDerivConstraint))
- .def("__init__", make_constructor(&wrapSplineDerivConstraintNoConstraints))
- .def("min", &exact_cubic_t::min)
- .def("max", &exact_cubic_t::max)
- .def("__call__", &exact_cubic_t::operator())
- .def("derivate", &exact_cubic_t::derivate)
- ;
- /** END spline_deriv_constraints**/
-
- /** BEGIN bernstein polynom**/
+ /** BEGIN bernstein polynomial**/
class_<bernstein_t>
("bernstein", init<const unsigned int, const unsigned int>())
.def("__call__", &bernstein_t::operator())
;
- /** END bernstein polynom**/
-
- /** BEGIN Bezier to polynom conversion**/
- def("from_bezier", from_bezier<bezier3_t,polynom_t>);
- /** END Bezier to polynom conversion**/
+ /** END bernstein polynomial**/
+
+ /** BEGIN curves conversion**/
+ def("polynomial_from_bezier", polynomial_from_curve<polynomial_t,bezier3_t>);
+ def("polynomial_from_hermite", polynomial_from_curve<polynomial_t,cubic_hermite_spline_t>);
+ def("bezier_from_hermite", bezier_from_curve<bezier3_t,cubic_hermite_spline_t>);
+ def("bezier_from_polynomial", bezier_from_curve<bezier3_t,polynomial_t>);
+ def("hermite_from_bezier", hermite_from_curve<cubic_hermite_spline_t, bezier3_t>);
+ def("hermite_from_polynomial", hermite_from_curve<cubic_hermite_spline_t, polynomial_t>);
+ /** END curves conversion**/
}
diff --git a/python/python_definitions.h b/python/python_definitions.h
index d1b13a0d007dbf6335a2ca0baf62070fc90567a2..8c4b3d674e6e965191f70b74243c187a356a379c 100644
--- a/python/python_definitions.h
+++ b/python/python_definitions.h
@@ -12,7 +12,9 @@ namespace curves
typedef double real;
typedef Eigen::Vector3d point_t;
+typedef Eigen::Vector3d tangent_t;
typedef Eigen::VectorXd vectorX_t;
+typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> point6_t;
typedef Eigen::Matrix<double, 3, 1, 0, 3, 1> ret_point_t;
typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> ret_point6_t;
@@ -33,7 +35,9 @@ T_Point vectorFromEigenArray(const PointList& array)
{
T_Point res;
for(int i =0;i<array.cols();++i)
+ {
res.push_back(array.col(i));
+ }
return res;
}
diff --git a/python/python_variables.cpp b/python/python_variables.cpp
index 715c0d9550847c4645aacf863eb1a598e8ce0f2e..8306be298b6c831cb0cd2f3037350c8647e1d88e 100644
--- a/python/python_variables.cpp
+++ b/python/python_variables.cpp
@@ -12,7 +12,9 @@ std::vector<linear_variable_3_t> matrix3DFromEigenArray(const point_list_t& matr
assert(vectors.cols() * 3 == matrices.cols() ) ;
std::vector<linear_variable_3_t> res;
for(int i =0;i<vectors.cols();++i)
+ {
res.push_back(linear_variable_3_t(matrices.block<3,3>(0,i*3), vectors.col(i)));
+ }
return res;
}
@@ -21,10 +23,14 @@ variables_3_t fillWithZeros(const linear_variable_3_t& var, const std::size_t to
variables_3_t res;
std::vector<linear_variable_3_t>& vars = res.variables_;
for (std::size_t idx = 0; idx < i; ++idx)
+ {
vars.push_back(linear_variable_3_t::Zero());
+ }
vars.push_back(var);
for (std::size_t idx = i+1; idx < totalvar; ++idx)
+ {
vars.push_back(linear_variable_3_t::Zero());
+ }
return res;
}
@@ -35,7 +41,9 @@ std::vector<variables_3_t> computeLinearControlPoints(const point_list_t& matric
// now need to fill all this with zeros...
std::size_t totalvar = variables.size();
for (std::size_t i = 0; i < totalvar; ++i)
+ {
res.push_back( fillWithZeros(variables[i],totalvar,i));
+ }
return res;
}
@@ -43,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/python/python_variables.h b/python/python_variables.h
index 393284204cb61610870e2a041f69445cec7a54e4..d369c20fb9a4dab6ac885d575fb7c175cc84afcc 100644
--- a/python/python_variables.h
+++ b/python/python_variables.h
@@ -21,7 +21,7 @@ typedef curves::bezier_curve <real, real, dim, true, variables_3_t> bezier_line
bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors);
bezier_linear_variable_t* wrapBezierLinearConstructorBounds
- (const point_list_t& matrices, const point_list_t& vectors, const real ub);
+ (const point_list_t& matrices, const point_list_t& vectors, const real T_min, const real T_max);
typedef std::pair<Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>,
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> > linear_points_t;
diff --git a/python/test/test.py b/python/test/test.py
index 4c93b48b0aaea71e1ffa3b640dcc58e30e822f01..49eb020fc5061060095e2711e8efe36be5b65550 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -9,8 +9,11 @@ from numpy.linalg import norm
import unittest
-from curves import bezier3, bezier6, curve_constraints, exact_cubic, from_bezier, polynom, spline_deriv_constraint
-
+from curves import bezier3, bezier6
+from curves import curve_constraints, polynomial, piecewise_polynomial_curve, exact_cubic, cubic_hermite_spline
+from curves import polynomial_from_bezier, polynomial_from_hermite
+from curves import bezier_from_hermite, bezier_from_polynomial
+from curves import hermite_from_bezier, hermite_from_polynomial
class TestCurves(unittest.TestCase):
#def print_str(self, inStr):
@@ -23,7 +26,7 @@ class TestCurves(unittest.TestCase):
# - Variables : degree, nbWayPoints
__EPS = 1e-6
waypoints = matrix([[1., 2., 3.]]).T
- a = bezier3(waypoints,2.)
+ a = bezier3(waypoints,0.,2.)
t = 0.
while t < 2.:
self.assertTrue (norm(a(t) - matrix([1., 2., 3.]).T) < __EPS)
@@ -33,7 +36,7 @@ class TestCurves(unittest.TestCase):
time_waypoints = matrix([0., 1.]).transpose()
# Create bezier6 and bezier3
a = bezier6(waypoints6)
- a = bezier3(waypoints, 3.)
+ a = bezier3(waypoints, 0., 3.)
# Test : Degree, min, max, derivate
#self.print_str(("test 1")
self.assertEqual (a.degree, a.nbWaypoints - 1)
@@ -58,7 +61,7 @@ class TestCurves(unittest.TestCase):
# Create new bezier3 curve
waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose()
a0 = bezier3(waypoints)
- a1 = bezier3(waypoints, 3.)
+ a1 = bezier3(waypoints, 0., 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)
# Check change in argument time_t of bezier3
@@ -82,12 +85,12 @@ class TestCurves(unittest.TestCase):
self.assertTrue (norm(a.derivate(1, 2) - c.end_acc) < 1e-10)
return
- def test_polynom(self):
+ def test_polynomial(self):
# To test :
# - Functions : constructor, min, max, derivate
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
- a = polynom(waypoints)
- a = polynom(waypoints, -1., 3.)
+ a = polynomial(waypoints) # Defined on [0.,1.]
+ a = polynomial(waypoints, -1., 3.) # Defined on [-1.,3.]
a.min()
a.max()
a(0.4)
@@ -95,9 +98,27 @@ class TestCurves(unittest.TestCase):
a.derivate(0.4, 2)
return
+ def test_piecewise_polynomial_curve(self):
+ # To test :
+ # - Functions : constructor, min, max, derivate, add_polynomial_curve, is_continuous
+ waypoints1 = matrix([[1., 1., 1.]]).transpose()
+ waypoints2 = matrix([[1., 1., 1.], [1., 1., 1.]]).transpose()
+ a = polynomial(waypoints1, 0.,1.)
+ b = polynomial(waypoints2, 1., 3.)
+ ppc = piecewise_polynomial_curve(a)
+ ppc.add_polynomial_curve(b)
+ ppc.min()
+ ppc.max()
+ ppc(0.4)
+ self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
+ ppc.derivate(0.4, 2)
+ ppc.is_continuous(0)
+ ppc.is_continuous(1)
+ return
+
def test_exact_cubic(self):
# To test :
- # - Functions : constructor, min, max, derivate
+ # - Functions : constructor, min, max, derivate, getNumberSplines, getSplineAt
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_waypoints = matrix([0., 1.]).transpose()
a = exact_cubic(waypoints, time_waypoints)
@@ -106,9 +127,11 @@ class TestCurves(unittest.TestCase):
a(0.4)
self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
+ a.getNumberSplines()
+ a.getSplineAt(0)
return
- def test_spline_deriv_constraint(self):
+ def test_exact_cubic_constraint(self):
# To test :
# - Functions : constructor, min, max, derivate
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
@@ -122,17 +145,46 @@ class TestCurves(unittest.TestCase):
c.end_vel = matrix([0., 1., 1.]).transpose()
c.init_acc = matrix([0., 1., 1.]).transpose()
c.end_acc = matrix([0., 1., 1.]).transpose()
- a = spline_deriv_constraint(waypoints, time_waypoints)
- a = spline_deriv_constraint(waypoints, time_waypoints, c)
+ a = exact_cubic(waypoints, time_waypoints)
+ a = exact_cubic(waypoints, time_waypoints, c)
+ return
+
+ def test_cubic_hermite_spline(self):
+ points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
+ tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
+ time_points = matrix([0., 1.]).transpose()
+ a = cubic_hermite_spline(points, tangents, time_points)
+ a.min()
+ a.max()
+ a(0.4)
+ self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
+ a.derivate(0.4, 2)
return
- def test_from_bezier(self):
- # converting bezier to polynom
+ def test_conversion_curves(self):
__EPS = 1e-6
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier3(waypoints)
- a_pol = from_bezier(a)
+ # converting bezier to polynomial
+ a_pol = polynomial_from_bezier(a)
self.assertTrue (norm(a(0.3) - a_pol(0.3)) < __EPS)
+ # converting polynomial to hermite
+ a_chs = hermite_from_polynomial(a_pol);
+ self.assertTrue (norm(a_chs(0.3) - a_pol(0.3)) < __EPS)
+ # converting hermite to bezier
+ a_bc = bezier_from_hermite(a_chs);
+ self.assertTrue (norm(a_chs(0.3) - a_bc(0.3)) < __EPS)
+ self.assertTrue (norm(a(0.3) - a_bc(0.3)) < __EPS)
+ # converting bezier to hermite
+ a_chs = hermite_from_bezier(a);
+ self.assertTrue (norm(a(0.3) - a_chs(0.3)) < __EPS)
+ # converting hermite to polynomial
+ a_pol = polynomial_from_hermite(a_chs)
+ self.assertTrue (norm(a_pol(0.3) - a_chs(0.3)) < __EPS)
+ # converting polynomial to bezier
+ a_bc = bezier_from_polynomial(a_pol)
+ self.assertTrue (norm(a_bc(0.3) - a_pol(0.3)) < __EPS)
+ self.assertTrue (norm(a(0.3) - a_bc(0.3)) < __EPS)
return
if __name__ == '__main__':
diff --git a/tests/Main.cpp b/tests/Main.cpp
index 162e20261e32065dc6b6ba9fa68835bd4e1767ed..20afa1fa6ecaa10d45d61484c76e871f0625a350 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -1,11 +1,12 @@
#include "curves/exact_cubic.h"
#include "curves/bezier_curve.h"
-#include "curves/polynom.h"
-#include "curves/spline_deriv_constraint.h"
+#include "curves/polynomial.h"
#include "curves/helpers/effector_spline.h"
#include "curves/helpers/effector_spline_rotation.h"
-#include "curves/bezier_polynom_conversion.h"
+#include "curves/curve_conversion.h"
+#include "curves/cubic_hermite_spline.h"
+#include "curves/piecewise_polynomial_curve.h"
#include <string>
#include <iostream>
@@ -16,22 +17,29 @@ using namespace std;
namespace curves
{
typedef Eigen::Vector3d point_t;
+typedef Eigen::Vector3d tangent_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
-typedef polynom <double, double, 3, true, point_t, t_point_t> polynom_t;
+typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
-typedef spline_deriv_constraint <double, double, 3, true, point_t> spline_deriv_constraint_t;
typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
-typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
+typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef std::pair<double, point_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef Eigen::Matrix<double,1,1> point_one;
-typedef polynom<double, double, 1, true, point_one> polynom_one;
+typedef polynomial<double, double, 1, true, point_one> polynom_one;
typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one;
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
+typedef cubic_hermite_spline <double, double, 3, true, point_t> cubic_hermite_spline_t;
+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))
@@ -65,8 +73,32 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const
}
}
-/*Cubic Function tests*/
+template<typename curve1, typename curve2>
+void compareCurves(curve1 c1, curve2 c2, const std::string& errMsg, bool& error)
+{
+ double T_min = c1.min();
+ double T_max = c1.max();
+ if (T_min!=c2.min() || T_max!=c2.max())
+ {
+ std::cout << "compareCurves, time min and max of curves does not match ["<<T_min<<","<<T_max<<"] "
+ << " and ["<<c2.min()<<","<<c2.max()<<"] "<<std::endl;
+ error = true;
+ }
+ else
+ {
+ // derivative in T_min and T_max
+ ComparePoints(c1.derivate(T_min,1),c2.derivate(T_min,1),errMsg, error, false);
+ ComparePoints(c1.derivate(T_max,1),c2.derivate(T_max,1),errMsg, error, false);
+ // Test values on curves
+ for(double i =T_min; i<T_max; i+=0.02)
+ {
+ ComparePoints(c1(i),c2(i),errMsg, error, false);
+ ComparePoints(c1(i),c2(i),errMsg, error, false);
+ }
+ }
+}
+/*Cubic Function tests*/
void CubicFunctionTest(bool& error)
{
std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
@@ -79,7 +111,7 @@ void CubicFunctionTest(bool& error)
vec.push_back(b);
vec.push_back(c);
vec.push_back(d);
- polynom_t cf(vec.begin(), vec.end(), 0, 1);
+ polynomial_t cf(vec.begin(), vec.end(), 0, 1);
point_t res1;
res1 =cf(0);
point_t x0(1,2,3);
@@ -98,7 +130,7 @@ void CubicFunctionTest(bool& error)
vec.push_back(b);
vec.push_back(c);
vec.push_back(d);
- polynom_t cf2(vec, 0.5, 1);
+ polynomial_t cf2(vec, 0.5, 1);
res1 = cf2(0.5);
ComparePoints(x0, res1, errMsg + "x3 ", error);
error = true;
@@ -130,68 +162,64 @@ void CubicFunctionTest(bool& error)
if(cf.max() != 1)
{
error = true;
- std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
+ std::cout << "Evaluation of cubic cf error, MaxBound should be equal to 1\n";
}
if(cf.min() != 0)
{
error = true;
- std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
+ std::cout << "Evaluation of cubic cf error, MinBound should be equal to 1\n";
}
}
/*bezier_curve Function tests*/
-
void BezierCurveTest(bool& error)
{
- std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
- point_t d(3,6,7);
+ std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+ point_t d(3,6,7);
- std::vector<point_t> params;
- params.push_back(a);
+ std::vector<point_t> params;
+ params.push_back(a);
- // 1d curve
- bezier_curve_t cf1(params.begin(), params.end());
- point_t res1;
- res1 = cf1(0);
- point_t x10 = a ;
+ // 1d curve in [0,1]
+ bezier_curve_t cf1(params.begin(), params.end());
+ point_t res1;
+ res1 = cf1(0);
+ point_t x10 = a ;
ComparePoints(x10, res1, errMsg + "1(0) ", error);
- res1 = cf1(1);
+ res1 = cf1(1);
ComparePoints(x10, res1, errMsg + "1(1) ", error);
- // 2d curve
- params.push_back(b);
- bezier_curve_t cf(params.begin(), params.end());
- res1 = cf(0);
- point_t x20 = a ;
+ // 2d curve in [0,1]
+ params.push_back(b);
+ bezier_curve_t cf(params.begin(), params.end());
+ res1 = cf(0);
+ point_t x20 = a ;
ComparePoints(x20, res1, errMsg + "2(0) ", error);
- point_t x21 = b;
- res1 = cf(1);
+ point_t x21 = b;
+ res1 = cf(1);
ComparePoints(x21, res1, errMsg + "2(1) ", error);
- //3d curve
- params.push_back(c);
- bezier_curve_t cf3(params.begin(), params.end());
- res1 = cf3(0);
+ //3d curve in [0,1]
+ params.push_back(c);
+ bezier_curve_t cf3(params.begin(), params.end());
+ res1 = cf3(0);
ComparePoints(a, res1, errMsg + "3(0) ", error);
- res1 = cf3(1);
+ res1 = cf3(1);
ComparePoints(c, res1, errMsg + "3(1) ", error);
- //4d curve
- params.push_back(d);
- bezier_curve_t cf4(params.begin(), params.end(), 2);
-
- res1 = cf4(2);
- ComparePoints(d, res1, errMsg + "3(1) ", error);
+ //4d curve in [1,2]
+ params.push_back(d);
+ bezier_curve_t cf4(params.begin(), params.end(), 1., 2.);
- //testing bernstein polynomes
- bezier_curve_t cf5(params.begin(), params.end(),2.);
- std::string errMsg2("In test BezierCurveTest ; Bernstein polynoms do not evaluate as analytical evaluation");
- for(double d = 0.; d <2.; d+=0.1)
+ //testing bernstein polynomials
+ 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 = 1.; d <2.; d+=0.1)
{
ComparePoints( cf5.evalBernstein(d) , cf5 (d), errMsg2, error);
ComparePoints( cf5.evalHorner(d) , cf5 (d), errMsg2, error);
@@ -200,11 +228,11 @@ void BezierCurveTest(bool& error)
}
bool error_in(true);
+
try
{
cf(-0.4);
- }
- catch(...)
+ } catch(...)
{
error_in = false;
}
@@ -214,11 +242,11 @@ void BezierCurveTest(bool& error)
error = true;
}
error_in = true;
+
try
{
cf(1.1);
- }
- catch(...)
+ } catch(...)
{
error_in = false;
}
@@ -230,17 +258,17 @@ void BezierCurveTest(bool& error)
if(cf.max() != 1)
{
error = true;
- std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
+ std::cout << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
}
if(cf.min() != 0)
{
error = true;
- std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
+ std::cout << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
}
}
#include <ctime>
-void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
+void BezierCurveTestCompareHornerAndBernstein(bool&) // error
{
using namespace std;
std::vector<double> values;
@@ -264,8 +292,9 @@ void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
params.push_back(c);
// 3d curve
- bezier_curve_t cf(params.begin(), params.end());
+ bezier_curve_t cf(params.begin(), params.end()); // defined in [0,1]
+ // Check all evaluation of bezier curve
clock_t s0,e0,s1,e1,s2,e2,s3,e3;
s0 = clock();
for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
@@ -295,14 +324,12 @@ void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
}
e3 = clock();
-
std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
- std::cout << "now with high order polynom " << std::endl;
-
+ std::cout << "now with high order polynomial " << std::endl;
params.push_back(d);
params.push_back(e);
@@ -341,8 +368,6 @@ void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
}
e3 = clock();
-
-
std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
@@ -352,7 +377,7 @@ void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
void BezierDerivativeCurveTest(bool& error)
{
- std::string errMsg("In test BezierDerivativeCurveTest ; unexpected result for x ");
+ std::string errMsg("In test BezierDerivativeCurveTest ;, Error While checking value of point on curve : ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
@@ -371,7 +396,7 @@ void BezierDerivativeCurveTest(bool& error)
void BezierDerivativeCurveTimeReparametrizationTest(bool& error)
{
- std::string errMsg("In test BezierDerivativeCurveTimeReparametrizationTest ; unexpected result for x ");
+ std::string errMsg("In test BezierDerivativeCurveTimeReparametrizationTest, Error While checking value of point on curve : ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
@@ -386,18 +411,20 @@ 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)
{
- std::string errMsg("In test BezierDerivativeCurveConstraintTest ; unexpected result for x ");
+ std::string errMsg("In test BezierDerivativeCurveConstraintTest, Error While checking value of point on curve : ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
@@ -411,26 +438,29 @@ void BezierDerivativeCurveConstraintTest(bool& error)
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(), constraints);
- assert(cf3.degree_ == params.size() + 3);
- assert(cf3.size_ == params.size() + 4);
+ bezier_curve_t::num_t T_min = 1.0;
+ bezier_curve_t::num_t T_max = 3.0;
+ bezier_curve_t cf(params.begin(), params.end(), constraints, T_min, T_max);
+
+ assert(cf.degree_ == params.size() + 3);
+ assert(cf.size_ == params.size() + 4);
- ComparePoints(a, cf3(0), errMsg, error);
- ComparePoints(c, cf3(1), errMsg, error);
- ComparePoints(constraints.init_vel, cf3.derivate(0.,1), errMsg, error);
- ComparePoints(constraints.end_vel , cf3.derivate(1.,1), errMsg, error);
- ComparePoints(constraints.init_acc, cf3.derivate(0.,2), errMsg, error);
- ComparePoints(constraints.end_vel, cf3.derivate(1.,1), errMsg, error);
- ComparePoints(constraints.end_acc, cf3.derivate(1.,2), errMsg, error);
+ ComparePoints(a, cf(T_min), errMsg, error);
+ ComparePoints(c, cf(T_max), errMsg, error);
+ ComparePoints(constraints.init_vel, cf.derivate(T_min,1), errMsg, error);
+ ComparePoints(constraints.end_vel , cf.derivate(T_max,1), errMsg, error);
+ ComparePoints(constraints.init_acc, cf.derivate(T_min,2), errMsg, error);
+ ComparePoints(constraints.end_vel, cf.derivate(T_max,1), errMsg, error);
+ ComparePoints(constraints.end_acc, cf.derivate(T_max,2), errMsg, error);
}
-void BezierToPolynomConversionTest(bool& error)
+void toPolynomialConversionTest(bool& error)
{
- std::string errMsg("In test BezierToPolynomConversionTest ; unexpected result for x ");
+ // bezier to polynomial
+ std::string errMsg("In test BezierToPolynomialConversionTest, Error While checking value of point on curve : ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
@@ -441,50 +471,120 @@ void BezierToPolynomConversionTest(bool& error)
point_t h(43,6,7);
point_t i(3,6,77);
- std::vector<point_t> params;
- params.push_back(a);
- params.push_back(b);
- params.push_back(c);
- params.push_back(d);
- params.push_back(e);
- params.push_back(f);
- params.push_back(g);
- params.push_back(h);
- params.push_back(i);
+ std::vector<point_t> control_points;
+ control_points.push_back(a);
+ control_points.push_back(b);
+ control_points.push_back(c);
+ control_points.push_back(d);
+ control_points.push_back(e);
+ control_points.push_back(f);
+ control_points.push_back(g);
+ control_points.push_back(h);
+ control_points.push_back(i);
+
+ bezier_curve_t::num_t T_min = 1.0;
+ bezier_curve_t::num_t T_max = 3.0;
+ bezier_curve_t bc(control_points.begin(), control_points.end(),T_min, T_max);
+ polynomial_t pol = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc);
+ compareCurves<polynomial_t, bezier_curve_t>(pol, bc, errMsg, error);
+}
- bezier_curve_t cf(params.begin(), params.end());
- polynom_t pol =from_bezier<bezier_curve_t, polynom_t>(cf);
- for(double i =0.; i<1.; i+=0.01)
- {
- ComparePoints(cf(i),pol(i),errMsg, error, true);
- ComparePoints(cf(i),pol(i),errMsg, error, false);
- }
+void cubicConversionTest(bool& error)
+{
+ std::string errMsg0("In test CubicConversionTest - convert hermite to, Error While checking value of point on curve : ");
+ std::string errMsg1("In test CubicConversionTest - convert bezier to, Error While checking value of point on curve : ");
+ std::string errMsg2("In test CubicConversionTest - convert polynomial to, Error While checking value of point on curve : ");
+
+ // Create cubic hermite spline : Test hermite to bezier/polynomial
+ point_t p0(1,2,3);
+ point_t m0(2,3,4);
+ point_t p1(3,4,5);
+ point_t m1(3,6,7);
+ 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);
+ std::vector< double > time_control_points;
+ polynomial_t::num_t T_min = 1.0;
+ polynomial_t::num_t T_max = 3.0;
+ time_control_points.push_back(T_min);
+ time_control_points.push_back(T_max);
+ cubic_hermite_spline_t chs0(control_points.begin(), control_points.end(), time_control_points);
+
+ // hermite to bezier
+ //std::cout<<"======================= \n";
+ //std::cout<<"hermite to bezier \n";
+ bezier_curve_t bc0 = bezier_from_curve<bezier_curve_t, cubic_hermite_spline_t>(chs0);
+ compareCurves<cubic_hermite_spline_t, bezier_curve_t>(chs0, bc0, errMsg0, error);
+
+ // hermite to pol
+ //std::cout<<"======================= \n";
+ //std::cout<<"hermite to polynomial \n";
+ polynomial_t pol0 = polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>(chs0);
+ compareCurves<cubic_hermite_spline_t, polynomial_t>(chs0, pol0, errMsg0, error);
+
+ // pol to hermite
+ //std::cout<<"======================= \n";
+ //std::cout<<"polynomial to hermite \n";
+ cubic_hermite_spline_t chs1 = hermite_from_curve<cubic_hermite_spline_t, polynomial_t>(pol0);
+ compareCurves<polynomial_t, cubic_hermite_spline_t>(pol0,chs1,errMsg2,error);
+
+ // pol to bezier
+ //std::cout<<"======================= \n";
+ //std::cout<<"polynomial to bezier \n";
+ bezier_curve_t bc1 = bezier_from_curve<bezier_curve_t, polynomial_t>(pol0);
+ compareCurves<bezier_curve_t, polynomial_t>(bc1, pol0, errMsg2, error);
+
+ // Bezier to pol
+ //std::cout<<"======================= \n";
+ //std::cout<<"bezier to polynomial \n";
+ polynomial_t pol1 = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc0);
+ compareCurves<bezier_curve_t, polynomial_t>(bc0, pol1, errMsg1, error);
+
+ // bezier => hermite
+ //std::cout<<"======================= \n";
+ //std::cout<<"bezier to hermite \n";
+ cubic_hermite_spline_t chs2 = hermite_from_curve<cubic_hermite_spline_t, bezier_curve_t>(bc0);
+ compareCurves<bezier_curve_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);
}
/*Exact Cubic Function tests*/
void ExactCubicNoErrorTest(bool& error)
{
+ // Create an exact cubic spline with 7 waypoints => 6 polynomials defined in [0.0,3.0]
curves::T_Waypoint waypoints;
- for(double i = 0; i <= 1; i = i + 0.2)
+ for(double i = 0.0; i <= 3.0; i = i + 0.5)
{
waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
}
exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
- point_t res1;
+
+ // Test number of polynomials in exact cubic
+ std::size_t numberSegments = exactCubic.getNumberSplines();
+ assert(numberSegments == 6);
+
+ // Test getSplineAt function
+ for (std::size_t i=0; i<numberSegments; i++)
+ {
+ exactCubic.getSplineAt(i);
+ }
+
+ // Other tests
try
{
- exactCubic(0);
- exactCubic(1);
+ exactCubic(0.0);
+ exactCubic(3.0);
}
catch(...)
{
error = true;
- std::cout << "Evaluation of ExactCubicNoErrorTest error\n";
+ std::cout << "Evaluation of ExactCubicNoErrorTest error when testing value on bounds\n";
}
error = true;
try
{
- exactCubic(1.2);
+ exactCubic(3.2);
}
catch(...)
{
@@ -492,26 +592,27 @@ void ExactCubicNoErrorTest(bool& error)
}
if(error)
{
- std::cout << "Evaluation of exactCubic cf error, 1.2 should be an out of range value\n";
+ std::cout << "Evaluation of exactCubic cf error, 3.2 should be an out of range value\n";
}
- if(exactCubic.max() != 1)
+
+ if(exactCubic.max() != 3.)
{
error = true;
- std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
+ std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 3\n";
}
- if(exactCubic.min() != 0)
+ if(exactCubic.min() != 0.)
{
error = true;
- std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
+ std::cout << "Evaluation of exactCubic error, MinBound should be equal to 0\n";
}
}
-
/*Exact Cubic Function tests*/
void ExactCubicTwoPointsTest(bool& error)
{
+ // Create an exact cubic spline with 2 waypoints => 1 polynomial defined in [0.0,1.0]
curves::T_Waypoint waypoints;
- for(double i = 0; i < 2; ++i)
+ for(double i = 0.0; i < 2.0; ++i)
{
waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
}
@@ -523,6 +624,14 @@ void ExactCubicTwoPointsTest(bool& error)
res1 = exactCubic(1);
ComparePoints(point_t(1,1,1), res1, errmsg, error);
+
+ // Test number of polynomials in exact cubic
+ std::size_t numberSegments = exactCubic.getNumberSplines();
+ assert(numberSegments == 1);
+
+ // Test getSplineAt
+ assert(exactCubic(0.5) == (exactCubic.getSplineAt(0))(0.5));
+
}
void ExactCubicOneDimTest(bool& error)
@@ -544,7 +653,7 @@ void ExactCubicOneDimTest(bool& error)
ComparePoints(one, res1, errmsg, error);
}
-void CheckWayPointConstraint(const std::string& errmsg, const double step, const curves::T_Waypoint& /*wayPoints*/, const exact_cubic_t* curve, bool& error )
+void CheckWayPointConstraint(const std::string& errmsg, const double step, const curves::T_Waypoint&, const exact_cubic_t* curve, bool& error )
{
point_t res1;
for(double i = 0; i <= 1; i = i + step)
@@ -583,7 +692,11 @@ void ExactCubicVelocityConstraintsTest(bool& error)
}
std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
spline_constraints_t constraints;
- spline_deriv_constraint_t exactCubic(waypoints.begin(), waypoints.end());
+ constraints.end_vel = point_t(0,0,0);
+ constraints.init_vel = point_t(0,0,0);
+ constraints.end_acc = point_t(0,0,0);
+ constraints.init_acc = point_t(0,0,0);
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end(), constraints);
// now check that init and end velocity are 0
CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
std::string errmsg3("Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
@@ -598,7 +711,7 @@ void ExactCubicVelocityConstraintsTest(bool& error)
constraints.end_acc = point_t(4,5,6);
constraints.init_acc = point_t(-4,-4,-6);
std::string errmsg2("Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)");
- spline_deriv_constraint_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
+ exact_cubic_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);
std::string errmsg4("Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
@@ -763,7 +876,7 @@ void EffectorSplineRotationWayPointRotationTest(bool& error)
void TestReparametrization(bool& error)
{
helpers::rotation_spline s;
- const helpers::spline_deriv_constraint_one_dim& sp = s.time_reparam_;
+ const helpers::exact_cubic_constraint_one_dim& sp = s.time_reparam_;
if(sp.min() != 0)
{
std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
@@ -794,18 +907,24 @@ void TestReparametrization(bool& error)
}
}
-point_t randomPoint(const double min, const double max ){
+point_t randomPoint(const double min, const double max )
+{
point_t p;
for(size_t i = 0 ; i < 3 ; ++i)
+ {
p[i] = (rand()/(double)RAND_MAX ) * (max-min) + min;
+ }
return p;
}
-void BezierEvalDeCasteljau(bool& error){
+void BezierEvalDeCasteljau(bool& error)
+{
using namespace std;
std::vector<double> values;
for (int i =0; i < 100000; ++i)
+ {
values.push_back(rand()/RAND_MAX);
+ }
//first compare regular evaluation (low dim pol)
point_t a(1,2,3);
@@ -828,7 +947,8 @@ void BezierEvalDeCasteljau(bool& error){
for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
{
- if(cf.evalDeCasteljau(*cit) != cf(*cit)){
+ if(cf.evalDeCasteljau(*cit) != cf(*cit))
+ {
error = true;
std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
}
@@ -845,7 +965,8 @@ void BezierEvalDeCasteljau(bool& error){
for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
{
- if(cf.evalDeCasteljau(*cit) != cf(*cit)){
+ if(cf.evalDeCasteljau(*cit) != cf(*cit))
+ {
error = true;
std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
}
@@ -853,88 +974,342 @@ void BezierEvalDeCasteljau(bool& error){
}
-
/**
* @brief BezierSplitCurve test the 'split' method of bezier curve
* @param error
*/
-void BezierSplitCurve(bool& error){
+void BezierSplitCurve(bool& error)
+{
// test for degree 5
- int n = 5;
+ size_t n = 5;
double t_min = 0.2;
double t_max = 10;
- for(size_t i = 0 ; i < 1 ; ++i){
+ for(size_t i = 0 ; i < 1 ; ++i)
+ {
// build a random curve and split it at random time :
//std::cout<<"build a random curve"<<std::endl;
point_t a;
std::vector<point_t> wps;
- for(size_t j = 0 ; j <= n ; ++j){
+ for(size_t j = 0 ; j <= n ; ++j)
+ {
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;
// test on splitted curves :
-
- if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) )){
+ if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) ))
+ {
error = true;
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;
}
- if(cs.first.max() != ts){
+ if(cs.first.max() != ts)
+ {
error = true;
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;
}
// check along curve :
- double ti = 0.;
- while(ti <= ts){
- if((cs.first(ti) - c(ti)).norm() > 1e-14){
+ double ti = t0;
+ while(ti <= ts)
+ {
+ if((cs.first(ti) - c(ti)).norm() > 1e-14)
+ {
error = true;
std::cout<<"first splitted curve and original curve doesn't correspond, error = "<<cs.first(ti) - c(ti) <<std::endl;
}
ti += 0.01;
}
- while(ti <= t){
- if((cs.second(ti-ts) - c(ti)).norm() > 1e-14){
+ while(ti <= t1)
+ {
+ 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;
}
+
}
}
+/* cubic hermite spline function test */
+void CubicHermitePairsPositionDerivativeTest(bool& error)
+{
+ std::string errmsg1("in Cubic Hermite 2 pairs (pos,vel), Error While checking that given wayPoints are crossed (expected / obtained) : ");
+ std::string errmsg2("in Cubic Hermite 2 points, Error While checking value of point on curve : ");
+ std::string errmsg3("in Cubic Hermite 2 points, Error While checking value of tangent on curve : ");
+ 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.);
+
+ 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, time_control_points_test;
+
+ // Two pairs
+ control_points.clear();
+ control_points.push_back(pair_point_tangent_t(p0,t0));
+ control_points.push_back(pair_point_tangent_t(p1,t1));
+ time_control_points.push_back(0.); // Time at P0
+ 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);
+ //Check
+ res1 = cubic_hermite_spline_1Pair(0.); // t=0
+ ComparePoints(p0, res1, errmsg1, error);
+ res1 = cubic_hermite_spline_1Pair(1.); // t=1
+ ComparePoints(p1, res1, errmsg1, error);
+ // Test derivative : two pairs
+ res1 = cubic_hermite_spline_1Pair.derivate(0.,1);
+ ComparePoints(t0, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
+ ComparePoints(t1, res1, errmsg3, error);
+
+ // Three pairs
+ 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
+ time_control_points.push_back(5.); // Time at P2
+ 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);
+ res1 = cubic_hermite_spline_2Pairs(2.); // t=2
+ ComparePoints(p1, res1, errmsg2, error);
+ res1 = cubic_hermite_spline_2Pairs(5.); // t=5
+ ComparePoints(p2, res1, errmsg1, error);
+ // Test derivative : three pairs
+ res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
+ ComparePoints(t0, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(2.,1);
+ ComparePoints(t1, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(5.,1);
+ ComparePoints(t2, res1, errmsg3, error);
+ // Test time control points by default [0,1] => 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
+ time_control_points_test.clear();
+ time_control_points_test.push_back(0.); // Time at P0
+ time_control_points_test.push_back(0.5); // Time at P1
+ time_control_points_test.push_back(1.0); // Time at P2
+ cubic_hermite_spline_2Pairs.setTime(time_control_points_test);
+ res1 = cubic_hermite_spline_2Pairs(0.); // t=0
+ ComparePoints(p0, res1, errmsg1, error);
+ res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
+ ComparePoints(p1, res1, errmsg2, error);
+ res1 = cubic_hermite_spline_2Pairs(1.); // t=1
+ ComparePoints(p2, res1, errmsg1, error);
+ // Test getTime
+ try
+ {
+ cubic_hermite_spline_2Pairs.getTime();
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Cubic hermite spline test, Error when calling getTime\n";
+ }
+ // Test derivative : three pairs, time default
+ res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
+ ComparePoints(t0, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(0.5,1);
+ ComparePoints(t1, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(1.,1);
+ 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
+ // Create three polynomials of constant value in the interval of definition
+ 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
+ // t in [0,1[ -> res=a
+ res = ppc(0.);
+ ComparePoints(a,res,errmsg1,error);
+ res = ppc(0.5);
+ ComparePoints(a,res,errmsg1,error);
+ // t in [1,2[ -> res=b
+ res = ppc(1.0);
+ ComparePoints(b,res,errmsg1,error);
+ res = ppc(1.5);
+ ComparePoints(b,res,errmsg1,error);
+ // t in [2,3] -> res=c
+ res = ppc(2.0);
+ ComparePoints(c,res,errmsg1,error);
+ res = ppc(3.0);
+ ComparePoints(c,res,errmsg1,error);
+
+ // Create 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);
+
+ // Create 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_points0, time_control_points1;
+ time_control_points0.push_back(0.);
+ time_control_points0.push_back(1.); // hermite 0 between [0,1]
+ time_control_points1.push_back(1.);
+ time_control_points1.push_back(3.); // hermite 1 between [1,3]
+ cubic_hermite_spline_t chs0(control_points_0.begin(), control_points_0.end(), time_control_points0);
+ cubic_hermite_spline_t chs1(control_points_1.begin(), control_points_1.end(), time_control_points1);
+ // Convert to polynomial
+ polynomial_t pol_chs0 = polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>(chs0);
+ polynomial_t pol_chs1 = polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>(chs1);
+ piecewise_polynomial_curve_t ppc_C1(pol_chs0);
+ ppc_C1.add_polynomial_curve(pol_chs1);
+
+ // Create piecewise curve C2
+ point_t a0(0,0,0);
+ point_t b0(1,1,1);
+ t_point_t veca, vecb;
+ // in [0,1[
+ veca.push_back(a0);
+ veca.push_back(b0); // x=t, y=t, z=t
+ // in [1,2]
+ vecb.push_back(b0);
+ vecb.push_back(b0); // x=(t-1)+1, y=(t-1)+1, z=(t-1)+1
+ polynomial_t pola(veca.begin(), veca.end(), 0, 1);
+ polynomial_t polb(vecb.begin(), vecb.end(), 1, 2);
+ piecewise_polynomial_curve_t ppc_C2(pola);
+ ppc_C2.add_polynomial_curve(polb);
+
+
+ // check C0 continuity
+ std::string errmsg2("in piecewise polynomial curve test, Error while checking continuity C0 on ");
+ std::string errmsg3("in piecewise polynomial curve test, Error while checking continuity C1 on ");
+ std::string errmsg4("in piecewise polynomial curve test, Error while checking continuity C2 on ");
+ // not C0
+ bool isC0 = ppc.is_continuous(0);
+ if (isC0)
+ {
+ std::cout << errmsg2 << " ppc " << std::endl;
+ error = true;
+ }
+ // C0
+ isC0 = ppc_C0.is_continuous(0);
+ if (not isC0)
+ {
+ std::cout << errmsg2 << " ppc_C0 " << std::endl;
+ error = true;
+ }
+ // not C1
+ bool isC1 = ppc_C0.is_continuous(1);
+ if (isC1)
+ {
+ std::cout << errmsg3 << " ppc_C0 " << std::endl;
+ error = true;
+ }
+ // C1
+ isC1 = ppc_C1.is_continuous(1);
+ if (not isC1)
+ {
+ std::cout << errmsg3 << " ppc_C1 " << std::endl;
+ error = true;
+ }
+ // not C2
+ bool isC2 = ppc_C1.is_continuous(2);
+ if (isC2)
+ {
+ std::cout << errmsg4 << " ppc_C1 " << std::endl;
+ error = true;
+ }
+ // C2
+ isC2 = ppc_C2.is_continuous(2);
+ if (not isC2)
+ {
+ std::cout << errmsg4 << " ppc_C2 " << std::endl;
+ error = true;
+ }
+}
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
@@ -951,15 +1326,18 @@ int main(int /*argc*/, char** /*argv[]*/)
BezierDerivativeCurveConstraintTest(error);
BezierCurveTestCompareHornerAndBernstein(error);
BezierDerivativeCurveTimeReparametrizationTest(error);
- BezierToPolynomConversionTest(error);
BezierEvalDeCasteljau(error);
BezierSplitCurve(error);
+ CubicHermitePairsPositionDerivativeTest(error);
+ piecewisePolynomialCurveTest(error);
+ toPolynomialConversionTest(error);
+ cubicConversionTest(error);
+
if(error)
{
std::cout << "There were some errors\n";
return -1;
- }
- else
+ } else
{
std::cout << "no errors found \n";
return 0;