Commit 4dd18a3a by JasonChmn

\ Example : A piecewise curve composed of three curves cf0, /// cf1 and cf2 where cf0 is defined between \f$[T0_{min},T0_{max}]\f$, cf1 between /// \f$[T0_{max},T1_{max}]\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$. /// On the piecewise polynomial curve, cf0 is located between \f$[T0_{min},T0_{max}[\f$, /// cf1 between \f$[T0_{max},T1_{max}[\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$. /// template, typename T_Point= std::vector >, typename Curve= curve_abc > struct piecewise_curve : public curve_abc { typedef Point point_t; typedef T_Point t_point_t; typedef Time time_t; typedef Numeric num_t; typedef Curve curve_t; typedef typename std::vector < curve_t > t_curve_t; typedef typename std::vector< Time > t_time_t; public: /// \brief Constructor. /// Initialize a piecewise curve by giving the first curve. /// \param pol : a polynomial curve. /// piecewise_curve(const curve_t& cf) { size_ = 0; add_curve(cf); time_curves_.push_back(cf.min()); T_min_ = cf.min(); } virtual ~piecewise_curve(){} virtual Point operator()(const Time t) const { if(Safe &! (T_min_ <= t && t <= T_max_)) { //std::cout<<"[Min,Max]=["< 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_curves_[0]) { return 0; } // time is after last control point time if(t > time_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_curves_.at(middle_id) < t) { left_id = middle_id+1; } else if(time_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_curve_t curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ] t_time_t time_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ] std::size_t size_; // Number of segments in piecewise curve = size of curves_ Time T_min_, T_max_; }; } // end namespace #endif // _CLASS_PIECEWISE_CURVE \ No newline at end of file