bezier_curve.h 18.2 KB
Newer Older
1
/**
2
* \file bezier_curve.h
3
4
5
6
7
8
9
10
11
12
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/


#ifndef _CLASS_BEZIERCURVE
#define _CLASS_BEZIERCURVE

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

#include "MathDefs.h"

19
20
21
#include "serialization/archive.hpp"
#include "serialization/eigen-matrix.hpp"

22
#include <vector>
23
#include <stdexcept>
24

25
26
#include <iostream>

27
namespace curves
28
{
29
30
31
32
33
  /// \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.
  ///
34
35
  template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
           typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
36
  struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>
37
  {
JasonChmn's avatar
JasonChmn committed
38
39
40
    typedef Point   point_t;
    typedef Time    time_t;
    typedef Numeric num_t;
41
    typedef curve_constraints<point_t> curve_constraints_t;
42
    typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
43
    typedef typename t_point_t::const_iterator cit_point_t;
44
    typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
45

46
    /* Constructors - destructors */
JasonChmn's avatar
JasonChmn committed
47
    public:
48
49
      /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
      ///
50
51
52
      bezier_curve()
        : T_min_(0), T_max_(0)
      {}
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

      /// \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_))
      {
70
71
        assert(bernstein_.size() == size_);
        In it(PointsBegin);
72
        if(Safe && (size_<1 || T_max_ <= T_min_))
73
        {
74
          throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO
75
        }
76
        for(; it != PointsEnd; ++it)
77
        {
78
          control_points_.push_back(*it);
79
        }
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
      }

      /// \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_))
      {
99
        if(Safe && (size_<1 || T_max_ <= T_min_))
100
        {
101
          throw std::invalid_argument("can't create bezier min bound is higher than max bound");
102
        }
103
104
        t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
        for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
105
        {
106
          control_points_.push_back(*cit);
107
        }
108
109
110
111
112
113
114
115
116
117
118
119
      }

      bezier_curve(const bezier_curve& other)
        : 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()
      {
JasonChmn's avatar
JasonChmn committed
120
        // NOTHING
121
122
123
124
125
126
127
128
      }

      /*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
      {
129
        check_if_not_empty();
130
131
        if(Safe &! (T_min_ <= t && t <= T_max_))
        {
132
          throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
133
134
        }
        if (size_ == 1)
135
        {
136
          return mult_T_*control_points_[0];
137
138
        }
        else
139
140
141
        {
          return evalHorner(t);
        }
142
143
144
145
146
147
148
149
      }

      ///  \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.
      bezier_curve_t compute_derivate(const std::size_t order) const
      {
150
        check_if_not_empty();
151
152
        if(order == 0) 
        {
153
          return *this;
154
        }
155
        t_point_t derived_wp;
156
        for(typename t_point_t::const_iterator pit =  control_points_.begin(); pit != control_points_.end()-1; ++pit)
157
158
159
        {
          derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
        }
160
        if(derived_wp.empty())
161
162
163
        {
          derived_wp.push_back(point_t::Zero(Dim));
        }
164
        bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_min_, T_max_, mult_T_ * (1./(T_max_-T_min_)) );
165
        return deriv.compute_derivate(order-1);
166
167
168
169
170
171
172
173
174
      }

      ///  \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
      {
175
        check_if_not_empty();
176
177
        if(order == 0) 
        {
178
          return *this;
179
        }
Steve Tonneau's avatar
Steve Tonneau committed
180
        num_t new_degree = (num_t)(degree_+1);
181
        t_point_t n_wp;
182
        point_t current_sum =  point_t::Zero(Dim);
183
184
185
        // 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);
186
        for(typename t_point_t::const_iterator pit =  control_points_.begin(); pit != control_points_.end(); ++pit)
187
        {
188
189
          current_sum += *pit;
          n_wp.push_back(current_sum / new_degree);
190
        }
191
        bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_min_, T_max_, mult_T_*(T_max_-T_min_));
192
        return integ.compute_primitive(order-1);
193
194
195
196
197
198
199
200
201
202
203
      }

      ///  \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
      {
204
        bezier_curve_t deriv = compute_derivate(order);
205
        return deriv(t);
206
207
208
209
210
211
212
213
214
215
216
217
      }

      /// \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
      {
218
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
219
        point_t res = point_t::Zero(Dim);
220
        typename t_point_t::const_iterator control_points_it = control_points_.begin();
221
        for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
222
        cit !=bernstein_.end(); ++cit, ++control_points_it)
223
        {
224
          res += cit->operator()(u) * (*control_points_it);
225
        }
226
        return res*mult_T_;
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
      }

      /// \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
      {
243
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
244
        typename t_point_t::const_iterator control_points_it = control_points_.begin();
stevet's avatar
stevet committed
245
246
        Numeric u_op, bc, tn;
        u_op = 1.0 - u;
247
248
        bc = 1;
        tn = 1;
249
250
        point_t tmp =(*control_points_it)*u_op; ++control_points_it;
        for(unsigned int i=1; i<degree_; i++, ++control_points_it)
251
        {
252
253
254
          tn = tn*u;
          bc = bc*((num_t)(degree_-i+1))/i;
          tmp = (tmp + tn*bc*(*control_points_it))*u_op;
255
        }
256
        return (tmp + tn*u*(*control_points_it))*mult_T_;
257
258
259
260
261
262
263
264
265
266
267
268
269
      }

      const t_point_t& waypoints() const {return control_points_;}

      /// \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 
      {
270
        // normalize time :
271
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
stevet's avatar
stevet committed
272
        t_point_t pts = deCasteljauReduction(waypoints(),u);
273
274
        while(pts.size() > 1)
        {
275
          pts = deCasteljauReduction(pts,u);
276
277
        }
        return pts[0]*mult_T_;
278
      }
279

280

281
282
      t_point_t deCasteljauReduction(const Numeric t) const
      {
283
284
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
        return deCasteljauReduction(waypoints(),u);
285
286
287
288
289
290
291
292
293
294
295
296
297
      }

      /// \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
      {
stevet's avatar
stevet committed
298
        if(u < 0 || u > 1)
299
        {
300
          throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
301
        }
302
        if(pts.size() == 1)
303
        {
304
          return pts;
305
        }
306
307

        t_point_t new_pts;
308
309
        for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit)
        {
310
          new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
311
312
        }
        return new_pts;
313
314
315
316
317
318
319
320
321
      }

      /// \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.
      ///
      std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t)
      {
322
        check_if_not_empty();
323
        if (fabs(t-T_max_)<MARGIN)
324
        {
325
          throw std::runtime_error("can't split curve, interval range is equal to original curve");
326
        }
327
        t_point_t wps_first(size_),wps_second(size_);
328
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
329
330
        wps_first[0] = control_points_.front();
        wps_second[degree_] = control_points_.back();
331
332
        t_point_t casteljau_pts = waypoints();
        size_t id = 1;
333
334
        while(casteljau_pts.size() > 1)
        {
335
336
337
338
          casteljau_pts = deCasteljauReduction(casteljau_pts,u);
          wps_first[id] = casteljau_pts.front();
          wps_second[degree_-id] = casteljau_pts.back();
          ++id;
339
340
        }

341
342
        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_);
343
        return std::make_pair(c_first,c_second);
344
      }
345

346
      bezier_curve_t extract(const Numeric t1, const Numeric t2){
347
        if(t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_)
348
        {
349
          throw std::out_of_range("In Extract curve : times out of bounds");
350
        }
351
        if (fabs(t1-T_min_)<MARGIN && fabs(t2-T_max_)<MARGIN)
352
        {
353
          return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
354
        }
355
        if (fabs(t1-T_min_)<MARGIN)
356
        {
357
          return split(t2).first;
358
        }
359
        if (fabs(t2-T_max_)<MARGIN)
360
        {
361
          return split(t1).second;
362
        }
363
        std::pair<bezier_curve_t,bezier_curve_t> c_split = this->split(t1);
364
        return c_split.second.split(t2-t1).first;
365
      }
366

367
    private:
368
369
370
371

      template<typename In>
      t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
      {
372
        t_point_t res;
373
374
        num_t T = T_max_ - T_min_;
        num_t T_square = T*T;
375
376
        point_t P0, P1, P2, P_n_2, P_n_1, PN;
        P0 = *PointsBegin; PN = *(PointsEnd-1);
377
378
379
380
        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;
381
382
383
384
        res.push_back(P0);
        res.push_back(P1);
        res.push_back(P2);
        for(In it = PointsBegin+1; it != PointsEnd-1; ++it)
385
        {
386
          res.push_back(*it);
387
        }
388
389
390
391
        res.push_back(P_n_2);
        res.push_back(P_n_1);
        res.push_back(PN);
        return res;
392
      }
393
394
395
396
397
398
399
400

      void check_if_not_empty() const
      {
        if (control_points_.size() == 0)
        {
          throw std::runtime_error("Error in beziercurve : there is no control points set / did you use empty constructor ?");
        }
      }
401
      /*Operations*/
402

403
    public:
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
      /*Helpers*/
      /// \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_;}
      /*Helpers*/

      /* Attributes */
      /// 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 time_t T=1.)
      {
t steve's avatar
t steve committed
428
        std::vector<point_t> ts;
429
        ts.push_back(point_t::Zero(Dim));
430
        return bezier_curve_t(ts.begin(), ts.end(),0.,T);
431
      }
432

433
434
      // Serialization of the class
      friend class boost::serialization::access;
435

436
437
      template<class Archive>
      void serialize(Archive& ar, const unsigned int version){
438
        if (version) {
439
          // Do something depending on version ?
440
        }
441
442
443
444
445
446
447
        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_);
448
449
      }
  }; // End struct bezier_curve
450

451
452
  template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
  const double bezier_curve<Time, Numeric, Dim, Safe, Point>::MARGIN(0.001);
453

454
} // namespace curve
455
456
#endif //_CLASS_BEZIERCURVE