piecewise_curve.h 12.9 KB
 JasonChmn committed Jul 02, 2019 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 /** * \file piecewise_curve.h * \brief class allowing to create a piecewise curve. * \author Jason C. * \date 05/2019 */ #ifndef _CLASS_PIECEWISE_CURVE #define _CLASS_PIECEWISE_CURVE #include "curve_abc.h" #include "curve_conversion.h" namespace curves {  JasonChmn committed Sep 03, 2019 16 17 18 19 20 21 22 23 24 25 26 27 28  /// \class PiecewiseCurve. /// \brief Represent a piecewise curve. We can add some new curve, /// but the starting time of the curve to add should be equal to the ending time of the actual /// piecewise_curve.
\ 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 >  JasonChmn committed Sep 03, 2019 29  struct piecewise_curve : public curve_abc  JasonChmn committed Sep 03, 2019 30  {  JasonChmn committed Jul 03, 2019 31 32 33 34  typedef Point point_t; typedef T_Point t_point_t; typedef Time time_t; typedef Numeric num_t;  JasonChmn committed Jul 02, 2019 35 36 37 38  typedef Curve curve_t; typedef typename std::vector < curve_t > t_curve_t; typedef typename std::vector< Time > t_time_t;  JasonChmn committed Jul 03, 2019 39  public:  JasonChmn committed Sep 03, 2019 40 41  /// \brief Empty constructor. Add at least one curve to call other class functions. ///  JasonChmn committed Sep 03, 2019 42  piecewise_curve()  JasonChmn committed Sep 03, 2019 43  : size_(0), T_min_(0), T_max_(0)  JasonChmn committed Sep 03, 2019 44 45 46 47 48 49 50 51  {} /// \brief Constructor. /// Initialize a piecewise curve by giving the first curve. /// \param pol : a polynomial curve. /// piecewise_curve(const curve_t& cf) {  JasonChmn committed Jul 03, 2019 52 53  size_ = 0; add_curve(cf);  JasonChmn committed Sep 03, 2019 54  }  JasonChmn committed Jul 02, 2019 55   JasonChmn committed Sep 03, 2019 56 57  piecewise_curve(const t_curve_t list_curves) {  JasonChmn committed Sep 03, 2019 58 59 60  size_ = 0; for( std::size_t i=0; i MARGIN) { isContinuous = false; } i++;  JasonChmn committed Jul 03, 2019 151 152  } return isContinuous;  JasonChmn committed Sep 03, 2019 153  }  JasonChmn committed Jul 03, 2019 154   JasonChmn committed Sep 03, 2019 155 156 157  template piecewise_curve convert_piecewise_curve_to_bezier() {  JasonChmn committed Sep 03, 2019 158  check_if_not_empty();  JasonChmn committed Sep 03, 2019 159 160 161 162 163 164 165 166 167  typedef piecewise_curve piecewise_curve_out_t; // Get first curve (segment) curve_t first_curve = curves_.at(0); Bezier first_curve_output = bezier_from_curve(first_curve); // Create piecewise curve piecewise_curve_out_t pc_res(first_curve_output); // Convert and add all other curves (segments) for (std::size_t i=1; i(curves_.at(i)));  JasonChmn committed Sep 03, 2019 169 170  } return pc_res;  JasonChmn committed Sep 03, 2019 171  }  JasonChmn committed Sep 03, 2019 172   JasonChmn committed Sep 03, 2019 173 174 175  template piecewise_curve convert_piecewise_curve_to_cubic_hermite() {  JasonChmn committed Sep 03, 2019 176  check_if_not_empty();  JasonChmn committed Sep 03, 2019 177 178 179 180 181 182 183 184 185  typedef piecewise_curve piecewise_curve_out_t; // Get first curve (segment) curve_t first_curve = curves_.at(0); Hermite first_curve_output = hermite_from_curve(first_curve); // Create piecewise curve piecewise_curve_out_t pc_res(first_curve_output); // Convert and add all other curves (segments) for (std::size_t i=1; i(curves_.at(i)));  JasonChmn committed Sep 03, 2019 187 188  } return pc_res;  JasonChmn committed Sep 03, 2019 189  }  JasonChmn committed Sep 03, 2019 190   JasonChmn committed Sep 03, 2019 191 192 193  template piecewise_curve convert_piecewise_curve_to_polynomial() {  JasonChmn committed Sep 03, 2019 194  check_if_not_empty();  JasonChmn committed Sep 03, 2019 195 196 197 198 199 200 201 202 203  typedef piecewise_curve piecewise_curve_out_t; // Get first curve (segment) curve_t first_curve = curves_.at(0); Polynomial first_curve_output = polynomial_from_curve(first_curve); // Create piecewise curve piecewise_curve_out_t pc_res(first_curve_output); // Convert and add all other curves (segments) for (std::size_t i=1; i(curves_.at(i)));  JasonChmn committed Sep 03, 2019 205 206  } return pc_res;  JasonChmn committed Sep 03, 2019 207  }  JasonChmn committed Sep 03, 2019 208   JasonChmn committed Sep 03, 2019 209 210 211 212  template static piecewise_curve convert_discrete_points_to_polynomial(T_Point points, Time T_min, Time T_max) {  JasonChmn committed Sep 03, 2019 213 214  if(Safe &! (points.size()>1)) {  JasonChmn committed Sep 03, 2019 215 216  //std::cout<<"[Min,Max]=["< convert_discrete_points_to_polynomial, Error, less than 2 discrete points");  JasonChmn committed Sep 03, 2019 217 218  } typedef piecewise_curve piecewise_curve_out_t;  JasonChmn committed Sep 03, 2019 219  Time discretization_step = (T_max-T_min)/Time(points.size()-1);  JasonChmn committed Sep 03, 2019 220 221 222 223 224 225 226 227 228 229 230 231 232  Time time_actual = T_min; // Initialization at first points point_t actual_point = points[0]; point_t next_point = points[1]; point_t coeff_order_zero(actual_point); point_t coeff_order_one((next_point-actual_point)/discretization_step); t_point_t coeffs; coeffs.push_back(coeff_order_zero); coeffs.push_back(coeff_order_one); Polynomial pol(coeffs,time_actual,time_actual+discretization_step); piecewise_curve_out_t ppc(pol); time_actual += discretization_step; // Other points  JasonChmn committed Sep 03, 2019 233  for (std::size_t i=1; i time_curves_[size_-1]) {  JasonChmn committed Sep 03, 2019 273  return size_-1;  JasonChmn committed Jul 02, 2019 274 275 276 277 278 279  } std::size_t left_id = 0; std::size_t right_id = size_-1; while(left_id <= right_id) {  JasonChmn committed Sep 03, 2019 280 281 282 283 284 285 286 287 288 289 290 291 292  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; }  JasonChmn committed Jul 02, 2019 293 294  } return left_id-1;  JasonChmn committed Sep 03, 2019 295  }  JasonChmn committed Jul 02, 2019 296   JasonChmn committed Sep 03, 2019 297 298 299 300 301 302 303 304  void check_if_not_empty() const { if (curves_.size() == 0) { throw std::runtime_error("Error in piecewise curve : No curve added"); } }  JasonChmn committed Sep 03, 2019 305   JasonChmn committed Jul 02, 2019 306  /*Helpers*/  JasonChmn committed Jul 03, 2019 307  public:  JasonChmn committed Sep 03, 2019 308 309 310  /// \brief Get dimension of curve. /// \return dimension of curve. std::size_t virtual dim() const{return dim_;};  JasonChmn committed Sep 03, 2019 311 312 313 314 315 316 317 318 319 320  /// \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_;} std::size_t getNumberCurves() { return curves_.size(); } /*Helpers*/ /* Attributes */  JasonChmn committed Sep 03, 2019 321  std::size_t dim_; // Dim of curve  JasonChmn committed Sep 03, 2019 322 323 324 325 326 327 328 329 330 331 332 333  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_; static const double MARGIN; /* Attributes */ // Serialization of the class friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version){  JasonChmn committed Sep 03, 2019 334  if (version) {  JasonChmn committed Sep 03, 2019 335  // Do something depending on version ?  JasonChmn committed Sep 03, 2019 336  }  JasonChmn committed Sep 03, 2019 337  ar & boost::serialization::make_nvp("dim", dim_);  JasonChmn committed Sep 03, 2019 338 339 340 341 342  ar & boost::serialization::make_nvp("curves", curves_); ar & boost::serialization::make_nvp("time_curves", time_curves_); ar & boost::serialization::make_nvp("size", size_); ar & boost::serialization::make_nvp("T_min", T_min_); ar & boost::serialization::make_nvp("T_max", T_max_);  JasonChmn committed Sep 03, 2019 343 344  } }; // End struct piecewise curve  JasonChmn committed Jul 02, 2019 345   JasonChmn committed Sep 03, 2019 346 347  template const double piecewise_curve::MARGIN(0.001);  JasonChmn committed Jul 04, 2019 348   JasonChmn committed Jul 02, 2019 349 350 351 352 } // end namespace #endif // _CLASS_PIECEWISE_CURVE