### Modification in bezier curve, now there is a T_min and T_max (Only T_max...

Modification in bezier curve, now there is a T_min and T_max (Only T_max before). More logic for curve_conversion. Also modified split bezier curve : before bezier curve defined on [0,T] and we splitted it in ts => bc1 was defined on [0,ts] and bc2 on [0,T-ts]. Now, bezier curve defined on [T_min,T_max] is splitted in ts => bc1 defined on [T_min,ts] and bc2 defined on [ts,T_max].
parent 69451c34
 ... ... @@ -11,6 +11,7 @@ SET(${PROJECT_NAME}_HEADERS quintic_spline.h linear_variable.h cubic_hermite_spline.h piecewise_polynomial_curve.h ) INSTALL(FILES ... ...  ... ... @@ -50,7 +50,8 @@ struct bezier_curve : public curve_abc /// template bezier_curve(In PointsBegin, In PointsEnd) : T_(1.) : T_min_(0.) , T_max_(1.) , mult_T_(1.) , size_(std::distance(PointsBegin, PointsEnd)) , degree_(size_-1) ... ... @@ -58,7 +59,7 @@ struct bezier_curve : public curve_abc { assert(bernstein_.size() == size_); In it(PointsBegin); if(Safe && (size_<1 || T_ <= 0.)) if(Safe && (size_<1 || T_max_ <= T_min_)) { throw std::out_of_range("can't create bezier min bound is higher than max bound"); } ... ... @@ -75,8 +76,9 @@ struct bezier_curve : public curve_abc /// \param T : upper bound of curve parameter which is between \f$[0;T]\f$(default \f$[0;1]\f$). /// template bezier_curve(In PointsBegin, In PointsEnd, const time_t T) : T_(T) bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min, const time_t T_max) : T_min_(T_min) , T_max_(T_max) , mult_T_(1.) , size_(std::distance(PointsBegin, PointsEnd)) , degree_(size_-1) ... ... @@ -84,7 +86,7 @@ struct bezier_curve : public curve_abc { assert(bernstein_.size() == size_); In it(PointsBegin); if(Safe && (size_<1 || T_ <= 0.)) if(Safe && (size_<1 || T_max_ <= T_min_)) { throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO } ... ... @@ -104,8 +106,9 @@ struct bezier_curve : public curve_abc /// \param mult_T : ... (default value is 1.0). /// template bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T) : T_(T) bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min, const time_t T_max, const time_t mult_T) : T_min_(T_min) , T_max_(T_max) , mult_T_(mult_T) , size_(std::distance(PointsBegin, PointsEnd)) , degree_(size_-1) ... ... @@ -113,7 +116,7 @@ struct bezier_curve : public curve_abc { assert(bernstein_.size() == size_); In it(PointsBegin); if(Safe && (size_<1 || T_ <= 0.)) if(Safe && (size_<1 || T_max_ <= T_min_)) { throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO } ... ... @@ -131,14 +134,16 @@ struct bezier_curve : public curve_abc /// \param constraints : constraints applying on start / end velocities and acceleration. /// template bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T=1.) : T_(T) bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T_min=0., const time_t T_max=1.) : T_min_(T_min) , T_max_(T_max) , mult_T_(1.) , size_(std::distance(PointsBegin, PointsEnd)+4) , degree_(size_-1) , bernstein_(curves::makeBernstein((unsigned int)degree_)) { if(Safe && (size_<1 || T_ <= 0.)) if(Safe && (size_<1 || T_max_ <= T_min_)) { throw std::out_of_range("can't create bezier min bound is higher than max bound"); } ... ... @@ -166,18 +171,18 @@ struct bezier_curve : public curve_abc /// \param t : time when to evaluate the curve. /// \return \f$x(t)\f$point corresponding on curve at time t. virtual point_t operator()(const time_t t) const { if(Safe &! (T_min_ <= t && t <= T_max_)) { throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO } if (size_ == 1) { if(Safe &! (0 <= t && t <= T_)) { throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO } if (size_ == 1) { return mult_T_*pts_; }else { return evalHorner(t); } return mult_T_*pts_; }else { return evalHorner(t); } } /// \brief Compute the derived curve at order N. ... ... @@ -195,7 +200,7 @@ struct bezier_curve : public curve_abc derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit))); if(derived_wp.empty()) derived_wp.push_back(point_t::Zero(Dim)); bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_, mult_T_ * (1./T_) ); bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_min_, T_max_, mult_T_ * (1./(T_max_-T_min_)) ); return deriv.compute_derivate(order-1); } ... ... @@ -221,7 +226,7 @@ struct bezier_curve : public curve_abc current_sum += *pit; n_wp.push_back(current_sum / new_degree); } bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_, mult_T_*T_); bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_min_, T_max_, mult_T_*(T_max_-T_min_)); return integ.compute_primitive(order-1); } ... ... @@ -248,7 +253,7 @@ struct bezier_curve : public curve_abc /// point_t evalBernstein(const Numeric t) const { const Numeric u = t/T_; const Numeric u = (t-T_min_)/(T_max_-T_min_); point_t res = point_t::Zero(Dim); typename t_point_t::const_iterator pts_it = pts_.begin(); for(typename std::vector >::const_iterator cit = bernstein_.begin(); ... ... @@ -273,7 +278,7 @@ struct bezier_curve : public curve_abc /// point_t evalHorner(const Numeric t) const { const Numeric u = t/T_; const Numeric u = (t-T_min_)/(T_max_-T_min_); typename t_point_t::const_iterator pts_it = pts_.begin(); Numeric u_op, bc, tn; u_op = 1.0 - u; ... ... @@ -300,7 +305,7 @@ struct bezier_curve : public curve_abc /// point_t evalDeCasteljau(const Numeric t) const { // normalize time : const Numeric u = t/T_; const Numeric u = (t-T_min_)/(T_max_-T_min_); t_point_t pts = deCasteljauReduction(waypoints(),u); while(pts.size() > 1) { ... ... @@ -311,7 +316,8 @@ struct bezier_curve : public curve_abc t_point_t deCasteljauReduction(const Numeric t) const{ return deCasteljauReduction(waypoints(),t/T_); const Numeric u = (t-T_min_)/(T_max_-T_min_); return deCasteljauReduction(waypoints(),u); } /// \brief Compute de Casteljau's reduction of the given list of points at time t. ... ... @@ -347,12 +353,13 @@ struct bezier_curve : public curve_abc /// \return pair containing the first element of both bezier curve obtained. /// std::pair split(const Numeric t){ if (t == T_) if (t == T_max_) { throw std::runtime_error("can't split curve, interval range is equal to original curve"); } t_point_t wps_first(size_),wps_second(size_); const double u = t/T_; const Numeric u = (t-T_min_)/(T_max_-T_min_); std::cout< public: /// \brief Get the minimum time for which the curve is defined /// \return \f$t_{min}\f$, lower bound of time range. virtual time_t min() const{return 0.;} virtual time_t min() const{return T_min_;} /// \brief Get the maximum time for which the curve is defined. /// \return \f$t_{max}\f\$, upper bound of time range. virtual time_t max() const{return T_;} virtual time_t max() const{return T_max_;} /*Helpers*/ public: /*const*/ time_t T_; /// Starting time of cubic hermite spline : T_min_ is equal to first time of control points. /*const*/ time_t T_min_; /// Ending time of cubic hermite spline : T_max_ is equal to last time of control points. /*const*/ time_t T_max_; /*const*/ time_t mult_T_; /*const*/ std::size_t size_; /*const*/ std::size_t degree_; ... ... @@ -446,7 +456,7 @@ struct bezier_curve : public curve_abc { std::vector 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 ... ...
 ... ... @@ -290,7 +290,7 @@ struct cubic_hermite_spline : public curve_abc h00 = 12.; h10 = 6.; h01 = -12.; h11 = 0.; h11 = 6.; } else { ... ...
 ... ... @@ -38,6 +38,25 @@ Polynomial polynom_from_bezier(const Bezier& curve) return Polynomial(coefficients,curve.min(),curve.max()); } template Polynomial polynom_from_hermite(const Hermite& curve) { typedef typename Polynomial::t_point_t t_point_t; typedef typename Polynomial::num_t num_t; assert (curve.min() == 0.); assert (curve.max() == 1.); t_point_t coefficients; Hermite current (curve); coefficients.push_back(curve(0.)); num_t fact = 1; for(std::size_t i = 1; i<= 3; ++i) { fact *= (num_t)i; coefficients.push_back(current.derivate(0.,i)/fact); } return Polynomial(coefficients,curve.min(),curve.max()); } /// \brief Converts a Cubic Hermite curve to a cubic bezier. /// \param curve : the cubic hermite curve defined between [0,1] to convert. /// \return the equivalent cubic bezier curve. ... ...
 #ifndef _CLASS_PIECEWISE_CURVE #define _CLASS_PIECEWISE_CURVE #include #include "curve_abc.h" #include "cubic_hermite_spline.h" #include "bezier_curve" #include "polynomial.h" #include "curve_conversion.h" ... ... @@ -17,30 +13,61 @@ namespace curves /// template, typename T_Point= std::vector, typename Polynomial= polynomial > > struct piecewise_polynomial_curve typename T_Point= std::vector > > struct piecewise_polynomial_curve : public curve_abc { typedef Point point_t; typedef T_Point t_point_t; typedef Time time_t; typedef Numeric num_t; typedef int Index; //typedef polynomial polynomial_t; typedef Polynomial polynomial_t; typedef std::vector < Polynomial > t_polynomial_t; typedef polynomial polynomial_t; typedef typename std::vector < polynomial_t > t_polynomial_t; typedef std::vector
 ... ... @@ -142,7 +142,7 @@ struct polynomial : public curve_abc { if((t < t_min_ || t > t_max_) && Safe) { throw std::out_of_range("time t to evaluate should be in range [Tmin, Tmax] of the curve"); throw std::out_of_range("error in polynomial : time t to evaluate should be in range [Tmin, Tmax] of the curve"); } time_t const dt (t-t_min_); point_t h = coefficients_.col(order_); ... ...
 ... ... @@ -65,17 +65,18 @@ using namespace boost::python; /* Template constructor bezier */ template 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(array); return new Bezier(asVector.begin(), asVector.end(), ub); return new Bezier(asVector.begin(), asVector.end(), T_min, T_max); } template 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(array); return new Bezier(asVector.begin(), asVector.end(), constraints, ub); return new Bezier(asVector.begin(), asVector.end(), constraints, T_min, T_max); } /* End Template constructor bezier */ ... ... @@ -84,17 +85,18 @@ bezier3_t* wrapBezierConstructor3(const point_list_t& array) { return wrapBezierConstructorTemplate(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(array, ub) ; return wrapBezierConstructorTemplate(array, T_min, T_max) ; } bezier3_t* wrapBezierConstructor3Constraints(const point_list_t& array, const curve_constraints_t& constraints) { return wrapBezierConstructorConstraintsTemplate(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(array, constraints, ub) ; return wrapBezierConstructorConstraintsTemplate(array, constraints, T_min, T_max) ; } /*END 3D constructors bezier */ /*6D constructors bezier */ ... ... @@ -102,17 +104,17 @@ bezier6_t* wrapBezierConstructor6(const point_list6_t& array) { return wrapBezierConstructorTemplate(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(array, ub) ; return wrapBezierConstructorTemplate(array, T_min, T_max) ; } bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints) { return wrapBezierConstructorConstraintsTemplate(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(array, constraints, ub) ; return wrapBezierConstructorConstraintsTemplate(array, constraints, T_min, T_max) ; } /*END 6D constructors bezier */ ... ...
 ... ... @@ -51,18 +51,17 @@ std::vector computeLinearControlPoints(const point_list_t& matric bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors) { std::vector 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 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; ... ...
 ... ... @@ -6,6 +6,7 @@ #include "curves/helpers/effector_spline_rotation.h" #include "curves/curve_conversion.h" #include "curves/cubic_hermite_spline.h" #include "curves/piecewise_polynomial_curve.h" #include #include ... ... @@ -36,6 +37,9 @@ typedef cubic_hermite_spline cubic_hermite_sp typedef std::pair pair_point_tangent_t; typedef std::vector > t_pair_point_tangent_t; typedef piecewise_polynomial_curve piecewise_polynomial_curve_t; bool QuasiEqual(const double a, const double b, const float margin) { if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0)) ... ... @@ -154,7 +158,7 @@ void BezierCurveTest(bool& error) std::vector params; params.push_back(a); // 1d curve // 1d curve in [0,1] bezier_curve_t cf1(params.begin(), params.end()); point_t res1; res1 = cf1(0); ... ... @@ -163,7 +167,7 @@ void BezierCurveTest(bool& error) res1 = cf1(1); ComparePoints(x10, res1, errMsg + "1(1) ", error); // 2d curve // 2d curve in [0,1] params.push_back(b); bezier_curve_t cf(params.begin(), params.end()); res1 = cf(0); ... ... @@ -174,7 +178,7 @@ void BezierCurveTest(bool& error) res1 = cf(1); ComparePoints(x21, res1, errMsg + "2(1) ", error);