bezier_curve.h 21.2 KB
Newer Older
1
/**
Guilhem Saurel's avatar
Guilhem Saurel committed
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
 */
8
9
10
11

#ifndef _CLASS_BEZIERCURVE
#define _CLASS_BEZIERCURVE

stonneau's avatar
stonneau committed
12
#include "curve_abc.h"
13
#include "bernstein.h"
14
#include "curve_constraint.h"
stevet's avatar
stevet committed
15
#include "piecewise_curve.h"
16
17
18
19

#include "MathDefs.h"

#include <vector>
20
#include <stdexcept>
21

22
23
#include <iostream>

Guilhem Saurel's avatar
Guilhem Saurel committed
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 <typename Time = double, typename Numeric = Time, bool Safe = false,
          typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
  typedef Point point_t;
stevet's avatar
stevet committed
34
35
  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
  typedef Eigen::Ref<const vector_x_t> vector_x_ref_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
36
37
38
39
40
41
  typedef Time time_t;
  typedef Numeric num_t;
  typedef curve_constraints<point_t> curve_constraints_t;
  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
  typedef typename t_point_t::const_iterator cit_point_t;
  typedef bezier_curve<Time, Numeric, Safe, Point> bezier_curve_t;
42
  typedef boost::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
43
  typedef piecewise_curve<Time, Numeric, Safe, point_t> piecewise_curve_t;
44
  typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t;  // parent class
45
  typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
46
47
48
49

  /* Constructors - destructors */
 public:
  /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
50
  ///
Guilhem Saurel's avatar
Guilhem Saurel committed
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
  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 <typename In>
  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<num_t>((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 <typename In>
  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_max_ <= T_min_)) {
      throw std::invalid_argument("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) {
      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);
    }
  }

143
144
145
146
147
148
149
150
  /**
   * @brief isApprox check if other and *this are approximately equals.
   * Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
   * @param other the other curve to check
   * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
   * @return true is the two curves are approximately equals
   */
  bool isApprox(const bezier_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
151
152
    bool equal = curves::isApprox<num_t> (T_min_, other.min())
        && curves::isApprox<num_t> (T_max_, other.max())
Pierre Fernbach's avatar
Pierre Fernbach committed
153
154
155
156
157
        && dim_ == other.dim()
        && degree_ == other.degree()
        && size_ == other.size_
        && mult_T_ == other.mult_T_
        && bernstein_ == other.bernstein_;
Pierre Fernbach's avatar
Pierre Fernbach committed
158
159
160
161
162
163
164
165
166
167
    if(!equal)
      return false;
    for (size_t i = 0 ;i < size_;++i)
    {
      if(!control_points_.at(i).isApprox(other.control_points_.at(i),prec))
        return false;
    }
    return true;
  }

168
169
170
171
172
173
174
175
  virtual bool isApprox(const curve_abc_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
    const bezier_curve_t* other_cast = dynamic_cast<const bezier_curve_t*>(other);
    if(other_cast)
      return isApprox(*other_cast,prec);
    else
      return false;
  }

Pierre Fernbach's avatar
Pierre Fernbach committed
176
177
178

  virtual bool operator==(const bezier_curve_t& other) const {
    return isApprox(other);
Pierre Fernbach's avatar
Pierre Fernbach committed
179
180
181
182
183
184
185
  }

  virtual bool operator!=(const bezier_curve_t& other) const {
    return !(*this == other);
  }


Guilhem Saurel's avatar
Guilhem Saurel committed
186
187
188
189
  ///  \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.
190
  bezier_curve_t compute_derivate(const std::size_t order) const {
Guilhem Saurel's avatar
Guilhem Saurel committed
191
    check_conditions();
192
    if (order == 0) {
193
      return *this;
194
    }
Guilhem Saurel's avatar
Guilhem Saurel committed
195
196
197
198
199
200
201
    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_));
    }
202
203
    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);
204
  }
205

206
207
208
209
210
  ///  \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's avatar
Guilhem Saurel committed
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
  }

  ///  \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$. <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 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 {
245
    return compute_derivate(order)(t);
Guilhem Saurel's avatar
Guilhem Saurel committed
246
247
248
249
250
251
252
253
254
255
256
257
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
  }

  /// \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$.<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_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<Bern<Numeric> >::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$.<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_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;
300
    if (index < control_points_.size()) {
Guilhem Saurel's avatar
Guilhem Saurel committed
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
      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 :<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 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.
  ///
356
  std::pair<bezier_curve_t, bezier_curve_t> split(const Numeric t) const {
Guilhem Saurel's avatar
Guilhem Saurel committed
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
    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;
    }
373
374
    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's avatar
Guilhem Saurel committed
375
376
377
    return std::make_pair(c_first, c_second);
  }

stevet's avatar
stevet committed
378
  /// \brief Split the bezier curve in several curves, all accessible
379
  /// within a piecewise_curve_t.
stevet's avatar
stevet committed
380
  /// \param times : list of times of size n.
381
  /// \return a piecewise_curve_t comprising n+1 curves
stevet's avatar
stevet committed
382
  ///
383
  piecewise_curve_t split(const vector_x_t& times) const {
384
385
386
387
    std::vector<bezier_curve_t> curves;
    bezier_curve_t current = *this;
    for (int i = 0; i < times.rows(); ++i) {
      std::pair<bezier_curve_t, bezier_curve_t> pairsplit = current.split(times[i]);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
388
389
390
391
      curves.push_back(pairsplit.first);
      current = pairsplit.second;
    }
    curves.push_back(current);
392
393
394
395
396
397
    piecewise_curve_t res;
    for(typename std::vector<bezier_curve_t>::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's avatar
stevet committed
398
399
  }

Guilhem Saurel's avatar
Guilhem Saurel committed
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
430
431
432
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
464
465
466
467
468
469
470
471
472
  /// \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<bezier_curve_t, bezier_curve_t> c_split = this->split(t1);
    return c_split.second.split(t2).first;
  }

 private:
  template <typename In>
  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_; }
473
474
475
  /// \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's avatar
Guilhem Saurel committed
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
  /*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<Bern<Numeric> > 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<point_t> 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 <class Archive>
  void serialize(Archive& ar, const unsigned int version) {
    if (version) {
      // Do something depending on version ?
    }
507
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
Guilhem Saurel's avatar
Guilhem Saurel committed
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
    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 <typename Time, typename Numeric, bool Safe, typename Point>
const double bezier_curve<Time, Numeric, Safe, Point>::MARGIN(0.001);

}  // namespace curves
#endif  //_CLASS_BEZIERCURVE