bezier_curve.h 19.7 KB
 stonneau committed Jun 27, 2013 1 /**  Guilhem Saurel committed Sep 24, 2019 2 3 4 5 6 7  * \file bezier_curve.h * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3. * \author Steve T. * \version 0.1 * \date 06/17/2013 */  stonneau committed Jun 27, 2013 8 9 10 11  #ifndef _CLASS_BEZIERCURVE #define _CLASS_BEZIERCURVE  stonneau committed Jun 27, 2013 12 #include "curve_abc.h"  Steve Tonneau committed Mar 13, 2017 13 #include "bernstein.h"  Steve Tonneau committed Apr 05, 2017 14 #include "curve_constraint.h"  stevet committed Oct 11, 2019 15 #include "piecewise_curve.h"  stonneau committed Jun 27, 2013 16 17 18 19  #include "MathDefs.h" #include  Steve Tonneau committed Mar 13, 2017 20 #include  stonneau committed Jun 27, 2013 21   Steve Tonneau committed Apr 04, 2017 22 23 #include  Guilhem Saurel committed Sep 24, 2019 24 25 26 27 28 29 30 31 32 33 namespace curves { /// \class BezierCurve. /// \brief Represents a Bezier curve of arbitrary dimension and order. /// For degree lesser than 4, the evaluation is analitycal. Otherwise /// the bernstein polynoms are used to evaluate the spline at a given location. /// template > struct bezier_curve : public curve_abc { typedef Point point_t;  stevet committed Oct 11, 2019 34 35  typedef Eigen::Matrix vector_x_t; typedef Eigen::Ref vector_x_ref_t;  Guilhem Saurel committed Sep 24, 2019 36 37 38 39 40 41  typedef Time time_t; typedef Numeric num_t; typedef curve_constraints curve_constraints_t; typedef std::vector > t_point_t; typedef typename t_point_t::const_iterator cit_point_t; typedef bezier_curve bezier_curve_t;  Pierre Fernbach committed Dec 02, 2019 42  typedef boost::shared_ptr bezier_curve_ptr_t;  Pierre Fernbach committed Nov 30, 2019 43  typedef piecewise_curve piecewise_curve_t;  44  typedef curve_abc curve_abc_t; // parent class  45  typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;  Guilhem Saurel committed Sep 24, 2019 46 47 48 49  /* Constructors - destructors */ public: /// \brief Empty constructor. Curve obtained this way can not perform other class functions.  JasonChmn committed Sep 03, 2019 50  ///  Guilhem Saurel committed Sep 24, 2019 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146  bezier_curve() : dim_(0), T_min_(0), T_max_(0) {} /// \brief Constructor. /// 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 : ... (default value is 1.0). /// template 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), bernstein_(curves::makeBernstein((unsigned int)degree_)) { assert(bernstein_.size() == size_); In it(PointsBegin); if (Safe && (size_ < 1 || T_max_ <= T_min_)) { throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO } for (; it != PointsEnd; ++it) { control_points_.push_back(*it); } // set dim if (control_points_.size() != 0) { dim_ = PointsBegin->size(); } } /// \brief Constructor /// This constructor will add 4 points (2 after the first one, 2 before the last one) /// to ensure that velocity and acceleration constraints are respected. /// \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 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_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((unsigned int)degree_)) { if (Safe && (size_ < 1 || T_max_ <= T_min_)) { throw std::invalid_argument("can't create bezier min bound is higher than max bound"); } t_point_t updatedList = add_constraints(PointsBegin, PointsEnd, constraints); for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) { control_points_.push_back(*cit); } // set dim if (control_points_.size() != 0) { dim_ = PointsBegin->size(); } } bezier_curve(const bezier_curve& other) : dim_(other.dim_), T_min_(other.T_min_), T_max_(other.T_max_), mult_T_(other.mult_T_), size_(other.size_), degree_(other.degree_), bernstein_(other.bernstein_), control_points_(other.control_points_) {} ///\brief Destructor ~bezier_curve() { // NOTHING } /*Operations*/ /// \brief Evaluation of the bezier curve at time t. /// \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 { check_conditions(); if (Safe & !(T_min_ <= t && t <= T_max_)) { throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO } if (size_ == 1) { return mult_T_ * control_points_[0]; } else { return evalHorner(t); } } /// \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.  147  bezier_curve_t compute_derivate(const std::size_t order) const {  Guilhem Saurel committed Sep 24, 2019 148  check_conditions();  149  if (order == 0) {  150  return *this;  151  }  Guilhem Saurel committed Sep 24, 2019 152 153 154 155 156 157 158  t_point_t derived_wp; 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_)); }  159 160  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);  161  }  162   163 164 165 166 167  /// \brief Compute the derived curve at order N. /// \param order : order of derivative. /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve. bezier_curve_t* compute_derivate_ptr(const std::size_t order) const { return new bezier_curve_t(compute_derivate(order));  Guilhem Saurel committed Sep 24, 2019 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201  } /// \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$. /// \param order : order of the primitive. /// \return primitive at order N of x(t). bezier_curve_t compute_primitive(const std::size_t order) const { check_conditions(); 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 = 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_min_, T_max_, mult_T_ * (T_max_ - T_min_)); return integ.compute_primitive(order - 1); } /// \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 \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 {  202  return compute_derivate(order)(t);  Guilhem Saurel committed Sep 24, 2019 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256  } /// \brief Evaluate all Bernstein polynomes for a certain degree. /// 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$.
/// 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_min_) / (T_max_ - T_min_); point_t res = point_t::Zero(dim_); typename t_point_t::const_iterator control_points_it = control_points_.begin(); for (typename std::vector >::const_iterator cit = bernstein_.begin(); 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. /// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.
/// \f$x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.
/// \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$
/// \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_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 = (*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 * (*control_points_it)) * u_op; } return (tmp + tn * u * (*control_points_it)) * mult_T_; } const t_point_t& waypoints() const { return control_points_; } const point_t waypointAtIndex(const std::size_t index) const { point_t waypoint;  stevet committed Oct 08, 2019 257  if (index < control_points_.size()) {  Guilhem Saurel committed Sep 24, 2019 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312  waypoint = control_points_[index]; } return waypoint; } /// \brief Evaluate the curve value at time t using deCasteljau algorithm. /// 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_min_) / (T_max_ - T_min_); t_point_t pts = deCasteljauReduction(waypoints(), u); while (pts.size() > 1) { pts = deCasteljauReduction(pts, u); } return pts[0] * mult_T_; } t_point_t deCasteljauReduction(const Numeric t) const { 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 :
/// \f$( pts[0]*(1-t)+pts[1], pts[1]*(1-t)+pts[2], ..., pts[0]*(N-2)+pts[N-1] )\f$
\ 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 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) { new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1))); } return new_pts; } /// \brief Split the bezier curve in 2 at time t. /// \param t : list of points. /// \param u : unNormalized time. /// \return pair containing the first element of both bezier curve obtained. ///  Pierre Fernbach committed Dec 02, 2019 313  std::pair split(const Numeric t) const {  Guilhem Saurel committed Sep 24, 2019 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329  check_conditions(); if (fabs(t - T_max_) < MARGIN) { 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 Numeric u = (t - T_min_) / (T_max_ - T_min_); t_point_t casteljau_pts = waypoints(); wps_first[0] = casteljau_pts.front(); wps_second[degree_] = casteljau_pts.back(); size_t id = 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; }  Pierre Fernbach committed Dec 02, 2019 330 331  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_);  Guilhem Saurel committed Sep 24, 2019 332 333 334  return std::make_pair(c_first, c_second); }  stevet committed Oct 11, 2019 335  /// \brief Split the bezier curve in several curves, all accessible  Pierre Fernbach committed Nov 30, 2019 336  /// within a piecewise_curve_t.  stevet committed Oct 11, 2019 337  /// \param times : list of times of size n.  Pierre Fernbach committed Nov 30, 2019 338  /// \return a piecewise_curve_t comprising n+1 curves  stevet committed Oct 11, 2019 339  ///  Pierre Fernbach committed Nov 30, 2019 340  piecewise_curve_t split(const vector_x_t& times) const {  Pierre Fernbach committed Dec 02, 2019 341 342 343 344  std::vector curves; bezier_curve_t current = *this; for (int i = 0; i < times.rows(); ++i) { std::pair pairsplit = current.split(times[i]);  Guilhem Saurel committed Nov 01, 2019 345 346 347 348  curves.push_back(pairsplit.first); current = pairsplit.second; } curves.push_back(current);  Pierre Fernbach committed Dec 02, 2019 349 350 351 352 353 354  piecewise_curve_t res; for(typename std::vector::const_iterator cit = curves.begin(); cit != curves.end() ; ++cit){ typename piecewise_curve_t::curve_ptr_t ptr(new bezier_curve_t(*cit)); res.add_curve_ptr(ptr); } return res;  stevet committed Oct 11, 2019 355 356  }  Guilhem Saurel committed Sep 24, 2019 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429  /// \brief Extract a bezier curve defined between \f$[t_1,t_2]\f$ from the actual bezier curve /// defined between \f$[T_{min},T_{max}]\f$ with \f$T_{min} \leq t_1 \leq t_2 \leq T_{max}\f$. /// \param t1 : start time of bezier curve extracted. /// \param t2 : end time of bezier curve extracted. /// \return bezier curve extract defined between \f$[t_1,t_2]\f$. /// bezier_curve_t extract(const Numeric t1, const Numeric t2) { 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 (fabs(t1 - T_min_) < MARGIN && fabs(t2 - T_max_) < MARGIN) // t1=T_min and t2=T_max { return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_); } if (fabs(t1 - T_min_) < MARGIN) // t1=T_min { return split(t2).first; } if (fabs(t2 - T_max_) < MARGIN) // t2=T_max { return split(t1).second; } std::pair c_split = this->split(t1); return c_split.second.split(t2).first; } private: template 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 * 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); res.push_back(PN); return res; } void check_conditions() const { if (control_points_.size() == 0) { throw std::runtime_error( "Error in bezier curve : there is no control points set / did you use empty constructor ?"); } else if (dim_ == 0) { throw std::runtime_error( "Error in bezier curve : Dimension of points is zero / did you use empty constructor ?"); } } /*Operations*/ public: /*Helpers*/ /// \brief Get dimension of curve. /// \return dimension of curve. std::size_t virtual dim() const { return dim_; }; /// \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_; }  Pierre Fernbach committed Nov 30, 2019 430 431 432  /// \brief Get the degree of the curve. /// \return \f$degree\f$, the degree of the curve. virtual std::size_t degree() const {return degree_;}  Guilhem Saurel committed Sep 24, 2019 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463  /*Helpers*/ /* Attributes */ /// Dim of curve std::size_t dim_; /// 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 > bernstein_; /*const*/ t_point_t control_points_; static const double MARGIN; /* Attributes */ static bezier_curve_t zero(const std::size_t dim, const time_t T = 1.) { std::vector ts; ts.push_back(point_t::Zero(dim)); return bezier_curve_t(ts.begin(), ts.end(), 0., T); } // Serialization of the class friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version) { if (version) { // Do something depending on version ? }  464  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);  Guilhem Saurel committed Sep 24, 2019 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480  ar& boost::serialization::make_nvp("dim", dim_); ar& boost::serialization::make_nvp("T_min", T_min_); ar& boost::serialization::make_nvp("T_max", T_max_); ar& boost::serialization::make_nvp("mult_T", mult_T_); ar& boost::serialization::make_nvp("size", size_); ar& boost::serialization::make_nvp("degree", degree_); ar& boost::serialization::make_nvp("bernstein", bernstein_); ar& boost::serialization::make_nvp("control_points", control_points_); } }; // End struct bezier_curve template const double bezier_curve::MARGIN(0.001); } // namespace curves #endif //_CLASS_BEZIERCURVE