bezier_curve.h 16.5 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
19

#include "MathDefs.h"

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

22
23
#include <iostream>

24
namespace curves
25
{
26
/// \class BezierCurve.
27
/// \brief Represents a Bezier curve of arbitrary dimension and order.
28
/// For degree lesser than 4, the evaluation is analitycal. Otherwise
29
/// the bernstein polynoms are used to evaluate the spline at a given location.
30
///
31
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
32
, typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
33
struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>
34
{
JasonChmn's avatar
JasonChmn committed
35
36
37
    typedef Point   point_t;
    typedef Time    time_t;
    typedef Numeric num_t;
38
    typedef curve_constraints<point_t> curve_constraints_t;
39
    typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
40
    typedef typename t_point_t::const_iterator cit_point_t;
41
    typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
42

43
/* Constructors - destructors */
JasonChmn's avatar
JasonChmn committed
44
    public:
45

46
    /// \brief Constructor.
JasonChmn's avatar
JasonChmn committed
47
    /// Given the first and last point of a control points set, create the bezier curve.
48
49
50
    /// \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$).
JasonChmn's avatar
JasonChmn committed
51
    /// \param mult_T        : ... (default value is 1.0).
52
53
    ///
    template<typename In>
54
    bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
55
56
    : T_min_(T_min)
    , T_max_(T_max)
57
58
59
    , mult_T_(mult_T)
    , size_(std::distance(PointsBegin, PointsEnd))
    , degree_(size_-1)
60
    , bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
61
62
63
    {
        assert(bernstein_.size() == size_);
        In it(PointsBegin);
64
        if(Safe && (size_<1 || T_max_ <= T_min_))
65
        {
66
            throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO
67
        }
68
        for(; it != PointsEnd; ++it)
69
        {
70
            control_points_.push_back(*it);
71
        }
72
    }
73

74
    /// \brief Constructor
75
    /// This constructor will add 4 points (2 after the first one, 2 before the last one)
76
77
78
79
    /// 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.
80
81
    ///
    template<typename In>
82
    bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, 
83
                const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
84
85
    : T_min_(T_min)
    , T_max_(T_max)
86
    , mult_T_(mult_T)
87
88
    , size_(std::distance(PointsBegin, PointsEnd)+4)
    , degree_(size_-1)
89
    , bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
90
    {
91
        if(Safe && (size_<1 || T_max_ <= T_min_))
92
        {
93
            throw std::invalid_argument("can't create bezier min bound is higher than max bound");
94
        }
95
96
        t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
        for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
97
        {
98
            control_points_.push_back(*cit);
99
        }
100
    }
101

102
    bezier_curve(const bezier_curve& other)
103
        : T_min_(other.T_min_), T_max_(other.T_max_), mult_T_(other.mult_T_), size_(other.size_),
JasonChmn's avatar
JasonChmn committed
104
          degree_(other.degree_), bernstein_(other.bernstein_), control_points_(other.control_points_)
105
106
          {}

JasonChmn's avatar
JasonChmn committed
107
108
109
110
111
    ///\brief Destructor
    ~bezier_curve()
    {
        // NOTHING
    }
112

JasonChmn's avatar
JasonChmn committed
113
114
    private:
//  bezier_curve(const bezier_curve&);
115
//  bezier_curve& operator=(const bezier_curve&);
116
117
118
/* Constructors - destructors */

/*Operations*/
JasonChmn's avatar
JasonChmn committed
119
120
121
122
    public:
    ///  \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.
123
    virtual point_t operator()(const time_t t) const
124
125
126
    {
        if(Safe &! (T_min_ <= t && t <= T_max_))
        {
127
            throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
128
129
        }
        if (size_ == 1)
130
        {
131
          return mult_T_*control_points_[0];
132
133
134
135
        }else
        {
          return evalHorner(t);
        }
JasonChmn's avatar
JasonChmn committed
136
    }
137

138
    ///  \brief Compute the derived curve at order N.
JasonChmn's avatar
JasonChmn committed
139
    ///  Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
140
    ///  \param order : order of derivative.
JasonChmn's avatar
JasonChmn committed
141
    ///  \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
142
143
    bezier_curve_t compute_derivate(const std::size_t order) const
    {
144
145
146
147
        if(order == 0) 
        {
            return *this;
        }
148
        t_point_t derived_wp;
149
        for(typename t_point_t::const_iterator pit =  control_points_.begin(); pit != control_points_.end()-1; ++pit)
stevet's avatar
stevet committed
150
            derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
151
        if(derived_wp.empty())
152
            derived_wp.push_back(point_t::Zero(Dim));
153
        bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_min_, T_max_, mult_T_ * (1./(T_max_-T_min_)) );
154
155
156
        return deriv.compute_derivate(order-1);
    }

157
    ///  \brief Compute the primitive of the curve at order N.
JasonChmn's avatar
JasonChmn committed
158
159
    ///  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$.
160
    ///  \param order : order of the primitive.
JasonChmn's avatar
JasonChmn committed
161
    ///  \return primitive at order N of x(t).
162
163
    bezier_curve_t compute_primitive(const std::size_t order) const
    {
164
165
166
167
        if(order == 0) 
        {
            return *this;
        }
Steve Tonneau's avatar
Steve Tonneau committed
168
        num_t new_degree = (num_t)(degree_+1);
169
        t_point_t n_wp;
170
        point_t current_sum =  point_t::Zero(Dim);
171
172
173
        // 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);
174
        for(typename t_point_t::const_iterator pit =  control_points_.begin(); pit != control_points_.end(); ++pit)
175
176
177
178
        {
            current_sum += *pit;
            n_wp.push_back(current_sum / new_degree);
        }
179
        bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_min_, T_max_, mult_T_*(T_max_-T_min_));
180
181
182
        return integ.compute_primitive(order-1);
    }

JasonChmn's avatar
JasonChmn committed
183
184
    ///  \brief Evaluate the derivative order N of curve at time t.
    ///  If derivative is to be evaluated several times, it is
185
    ///  rather recommended to compute derived curve using compute_derivate.
186
    ///  \param order : order of derivative.
187
    ///  \param t : time when to evaluate the curve.
188
    ///  \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
189
    ///
190
    virtual point_t derivate(const time_t t, const std::size_t order) const
191
    {
192
        bezier_curve_t deriv = compute_derivate(order);
193
194
195
        return deriv(t);
    }

196
    /// \brief Evaluate all Bernstein polynomes for a certain degree.
197
198
    /// 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/>
JasonChmn's avatar
JasonChmn committed
199
    /// Warning: the horner scheme is about 100 times faster than this method.<br>
200
    /// This method will probably be removed in the future as the computation of bernstein polynomial is very costly.
JasonChmn's avatar
JasonChmn committed
201
    /// \param t : time when to evaluate the curve.
202
    /// \return \f$x(t)\f$ point corresponding on curve at time t.
203
    ///
stevet's avatar
stevet committed
204
    point_t evalBernstein(const Numeric t) const
205
    {
206
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
207
        point_t res = point_t::Zero(Dim);
208
        typename t_point_t::const_iterator control_points_it = control_points_.begin();
209
        for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
210
            cit !=bernstein_.end(); ++cit, ++control_points_it)
211
        {
212
            res += cit->operator()(u) * (*control_points_it);
213
        }
214
        return res*mult_T_;
215
216
    }

217
    /// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
JasonChmn's avatar
JasonChmn committed
218
219
220
221
    /// 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>
222
    /// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
JasonChmn's avatar
JasonChmn committed
223
224
    /// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$<br>
    /// Using Horner's method, the polynom is transformed into : <br>
225
    /// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
JasonChmn's avatar
JasonChmn committed
226
    /// \param t : time when to evaluate the curve.
227
    /// \return \f$x(t)\f$ point corresponding on curve at time t.
228
    ///
stevet's avatar
stevet committed
229
    point_t evalHorner(const Numeric t) const
230
    {
231
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
232
        typename t_point_t::const_iterator control_points_it = control_points_.begin();
stevet's avatar
stevet committed
233
234
        Numeric u_op, bc, tn;
        u_op = 1.0 - u;
235
236
        bc = 1;
        tn = 1;
237
238
        point_t tmp =(*control_points_it)*u_op; ++control_points_it;
        for(unsigned int i=1; i<degree_; i++, ++control_points_it)
239
        {
stevet's avatar
stevet committed
240
            tn = tn*u;
stevet's avatar
stevet committed
241
            bc = bc*((num_t)(degree_-i+1))/i;
242
            tmp = (tmp + tn*bc*(*control_points_it))*u_op;
243
        }
244
        return (tmp + tn*u*(*control_points_it))*mult_T_;
245
246
    }

247
    const t_point_t& waypoints() const {return control_points_;}
Steve Tonneau's avatar
Steve Tonneau committed
248

249
    /// \brief Evaluate the curve value at time t using deCasteljau algorithm.
250
251
252
    /// 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$.
JasonChmn's avatar
JasonChmn committed
253
    /// \param t : time when to evaluate the curve.
254
    /// \return \f$x(t)\f$ point corresponding on curve at time t.
255
    ///
stevet's avatar
stevet committed
256
    point_t evalDeCasteljau(const Numeric t) const {
257
        // normalize time :
258
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
stevet's avatar
stevet committed
259
        t_point_t pts = deCasteljauReduction(waypoints(),u);
260
261
        while(pts.size() > 1)
        {
stevet's avatar
stevet committed
262
            pts = deCasteljauReduction(pts,u);
263
264
265
266
        }
        return pts[0]*mult_T_;
    }

267

268
    t_point_t deCasteljauReduction(const Numeric t) const{
269
270
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
        return deCasteljauReduction(waypoints(),u);
271
272
    }

273
    /// \brief Compute de Casteljau's reduction of the given list of points at time t.
274
275
    /// 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>
JasonChmn's avatar
JasonChmn committed
276
277
    /// 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.
278
    /// \param pts : list of points.
JasonChmn's avatar
JasonChmn committed
279
    /// \param u   : NORMALIZED time when to evaluate the curve.
JasonChmn's avatar
JasonChmn committed
280
    /// \return reduced list of point (size of pts - 1).
281
    ///
stevet's avatar
stevet committed
282
283
    t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const{
        if(u < 0 || u > 1)
284
        {
stevet's avatar
stevet committed
285
            throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
286
        }
287
        if(pts.size() == 1)
288
        {
289
            return pts;
290
        }
291
292

        t_point_t new_pts;
293
294
        for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit)
        {
stevet's avatar
stevet committed
295
            new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
296
297
298
299
        }
        return new_pts;
    }

300
301
302
    /// \brief Split the bezier curve in 2 at time t.
    /// \param t : list of points.
    /// \param u : unNormalized time.
JasonChmn's avatar
JasonChmn committed
303
    /// \return pair containing the first element of both bezier curve obtained.
304
    ///
stevet's avatar
stevet committed
305
    std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t){
306
        if (fabs(t-T_max_)<MARGIN)
307
        {
stevet's avatar
stevet committed
308
            throw std::runtime_error("can't split curve, interval range is equal to original curve");
309
        }
310
        t_point_t wps_first(size_),wps_second(size_);
311
        const Numeric u = (t-T_min_)/(T_max_-T_min_);
312
313
        wps_first[0] = control_points_.front();
        wps_second[degree_] = control_points_.back();
314
315
        t_point_t casteljau_pts = waypoints();
        size_t id = 1;
316
317
        while(casteljau_pts.size() > 1)
        {
stevet's avatar
stevet committed
318
            casteljau_pts = deCasteljauReduction(casteljau_pts,u);
319
320
321
322
323
            wps_first[id] = casteljau_pts.front();
            wps_second[degree_-id] = casteljau_pts.back();
            ++id;
        }

324
325
        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_);
326
327
328
329
        return std::make_pair(c_first,c_second);
    }

    bezier_curve_t extract(const Numeric t1, const Numeric t2){
330
        if(t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_)
331
        {
332
            throw std::out_of_range("In Extract curve : times out of bounds");
333
        }
334
        if (fabs(t1-T_min_)<MARGIN && fabs(t2-T_max_)<MARGIN)
335
        {
336
            return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
337
        }
338
        if (fabs(t1-T_min_)<MARGIN)
339
        {
340
            return split(t2).first;
341
        }
342
        if (fabs(t2-T_max_)<MARGIN)
343
        {
344
            return split(t1).second;
345
        }
346
347

        std::pair<bezier_curve_t,bezier_curve_t> c_split = this->split(t1);
348
        return c_split.second.split(t2-t1).first;
349
    }
350

351
352
353
354
355
    private:
    template<typename In>
    t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
    {
        t_point_t res;
356
357
        num_t T = T_max_ - T_min_;
        num_t T_square = T*T;
358
359
        point_t P0, P1, P2, P_n_2, P_n_1, PN;
        P0 = *PointsBegin; PN = *(PointsEnd-1);
360
361
362
363
        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;
364
365
366
367
368
369

        res.push_back(P0);
        res.push_back(P1);
        res.push_back(P2);

        for(In it = PointsBegin+1; it != PointsEnd-1; ++it)
370
        {
371
            res.push_back(*it);
372
        }
373
374
375
376
377
378
379

        res.push_back(P_n_2);
        res.push_back(P_n_1);
        res.push_back(PN);
        return res;
    }

380
/*Operations*/
381
382

/*Helpers*/
383
    public:
384
385
    /// \brief Get the minimum time for which the curve is defined
    /// \return \f$t_{min}\f$, lower bound of time range.
386
    virtual time_t min() const{return T_min_;}
387
388
    /// \brief Get the maximum time for which the curve is defined.
    /// \return \f$t_{max}\f$, upper bound of time range.
389
    virtual time_t max() const{return T_max_;}
390
391
/*Helpers*/

JasonChmn's avatar
JasonChmn committed
392
    public:
393
394
395
396
    /// 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_;
397
    /*const*/ time_t mult_T_;
398
399
400
    /*const*/ std::size_t size_;
    /*const*/ std::size_t degree_;
    /*const*/ std::vector<Bern<Numeric> > bernstein_;
401
    /*const*/ t_point_t  control_points_;
402
    static const double MARGIN;
403

t steve's avatar
t steve committed
404
405
406
407
    public:
    static bezier_curve_t zero(const time_t T=1.)
    {
        std::vector<point_t> ts;
408
        ts.push_back(point_t::Zero(Dim));
409
        return bezier_curve_t(ts.begin(), ts.end(),0.,T);
t steve's avatar
t steve committed
410
    }
411
};
412
413
414
415

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);

416
} // namespace curve
417
418
#endif //_CLASS_BEZIERCURVE