bezier_curve.h 17.8 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
37
38
  struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>,
                        public serialization::Serializable< bezier_curve<Time, Numeric, Dim, Safe, Point> >
  {
JasonChmn's avatar
JasonChmn committed
39
40
41
    typedef Point   point_t;
    typedef Time    time_t;
    typedef Numeric num_t;
42
    typedef curve_constraints<point_t> curve_constraints_t;
43
    typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
44
    typedef typename t_point_t::const_iterator cit_point_t;
45
    typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
46

47
    /* Constructors - destructors */
JasonChmn's avatar
JasonChmn committed
48
    public:
49

50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
      bezier_curve(){}

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

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

      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
118
        // NOTHING
119
120
121
122
123
124
125
126
      }

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

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

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

      ///  \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
      {
199
        bezier_curve_t deriv = compute_derivate(order);
200
        return deriv(t);
201
202
203
204
205
206
207
208
209
210
211
212
      }

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

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

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

275

276
277
      t_point_t deCasteljauReduction(const Numeric t) const
      {
278
279
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
        return deCasteljauReduction(waypoints(),u);
280
281
282
283
284
285
286
287
288
289
290
291
292
      }

      /// \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
293
        if(u < 0 || u > 1)
294
        {
295
          throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
296
        }
297
        if(pts.size() == 1)
298
        {
299
          return pts;
300
        }
301
302

        t_point_t new_pts;
303
304
        for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit)
        {
305
          new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
306
307
        }
        return new_pts;
308
309
310
311
312
313
314
315
316
      }

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

335
336
        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_);
337
        return std::make_pair(c_first,c_second);
338
      }
339

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

361
    private:
362
363
364
365

      template<typename In>
      t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
      {
366
        t_point_t res;
367
368
        num_t T = T_max_ - T_min_;
        num_t T_square = T*T;
369
370
        point_t P0, P1, P2, P_n_2, P_n_1, PN;
        P0 = *PointsBegin; PN = *(PointsEnd-1);
371
372
373
374
        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;
375
376
377
378
        res.push_back(P0);
        res.push_back(P1);
        res.push_back(P2);
        for(In it = PointsBegin+1; it != PointsEnd-1; ++it)
379
        {
380
          res.push_back(*it);
381
        }
382
383
384
385
        res.push_back(P_n_2);
        res.push_back(P_n_1);
        res.push_back(PN);
        return res;
386
387
      }
      /*Operations*/
388

389
    public:
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
      /*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
414
        std::vector<point_t> ts;
415
        ts.push_back(point_t::Zero(Dim));
416
        return bezier_curve_t(ts.begin(), ts.end(),0.,T);
417
      }
418

419
420
      // Serialization of the class
      friend class boost::serialization::access;
421

422
423
      template<class Archive>
      void serialize(Archive& ar, const unsigned int version){
424
        if (version) {
425
          // Do something depending on version ?
426
        }
427
428
429
430
431
432
433
        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_);
434
435
      }
  }; // End struct bezier_curve
436

437
438
  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);
439

440
} // namespace curve
441
442
#endif //_CLASS_BEZIERCURVE