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

#include "MathDefs.h"

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

22
23
#include <iostream>

24
25
namespace spline
{
26
/// \class BezierCurve
27
28
29
/// \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.
30
///
31
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
32
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
33
struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
34
35
36
{
	typedef Point 	point_t;
	typedef Time 	time_t;
37
    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
44
/* Constructors - destructors */
	public:
45
46
	///\brief Constructor
	///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
47
    ///
48
	template<typename In>
49
50
51
    bezier_curve(In PointsBegin, In PointsEnd)
    : T_(1.)
    , mult_T_(1.)
52
	, size_(std::distance(PointsBegin, PointsEnd))
Steve Tonneau's avatar
Steve Tonneau committed
53
    , degree_(size_-1)
stevet's avatar
stevet committed
54
    , bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_))
55
    {
56
        assert(bernstein_.size() == size_);
57
		In it(PointsBegin);
58
        if(Safe && (size_<1 || T_ <= 0.))
59
            throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
60
61
62
63
        for(; it != PointsEnd; ++it)
            pts_.push_back(*it);
    }

64
65
66
67
68
69
70
71
72
    ///\brief Constructor
    ///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
    ///
    template<typename In>
    bezier_curve(In PointsBegin, In PointsEnd, const time_t T)
    : T_(T)
    , mult_T_(1.)
    , size_(std::distance(PointsBegin, PointsEnd))
    , degree_(size_-1)
stevet's avatar
stevet committed
73
    , bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_))
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
    {
        assert(bernstein_.size() == size_);
        In it(PointsBegin);
        if(Safe && (size_<1 || T_ <= 0.))
            throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
        for(; it != PointsEnd; ++it)
            pts_.push_back(*it);
    }



    ///\brief Constructor
    ///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
    ///
    template<typename In>
    bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T)
    : T_(T)
    , mult_T_(mult_T)
    , size_(std::distance(PointsBegin, PointsEnd))
    , degree_(size_-1)
stevet's avatar
stevet committed
94
    , bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_))
95
96
97
98
99
100
101
102
    {
        assert(bernstein_.size() == size_);
        In it(PointsBegin);
        if(Safe && (size_<1 || T_ <= 0.))
            throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
        for(; it != PointsEnd; ++it)
            pts_.push_back(*it);
    }
103
104
105
106
107
108
109
110

    ///\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, PointsEnd : the points parametering the Bezier curve
    ///\param constraints : constraints applying on start / end velocities and acceleration
    ///
    template<typename In>
111
112
113
    bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T=1.)
    : T_(T)
    , mult_T_(1.)
114
115
    , size_(std::distance(PointsBegin, PointsEnd)+4)
    , degree_(size_-1)
stevet's avatar
stevet committed
116
    , bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_))
117
    {
118
        if(Safe && (size_<1 || T_ <= 0.))
119
120
121
122
123
            throw std::out_of_range("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)
            pts_.push_back(*cit);
    }
124

125
	///\brief Destructor
stonneau's avatar
stonneau committed
126
	~bezier_curve()
127
128
129
	{
		// NOTHING
	}
130
131

	private:
132
133
//	bezier_curve(const bezier_curve&);
//  bezier_curve& operator=(const bezier_curve&);
134
135
136
137
138
139
/* Constructors - destructors */

/*Operations*/
	public:
	///  \brief Evaluation of the cubic spline at time t.
	///  \param t : the time when to evaluate the spine
140
	///  \param return : the value x(t)
141
    virtual point_t operator()(const time_t t) const
142
	{
143
        num_t nT = t /  T_;
144
145
		if(Safe &! (0 <= nT && nT <= 1))
		{
146
            throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
147
        }
148
		else
149
150
        {
            num_t dt = (1 - nT);
151
			switch(size_)
152
153
            {
                case 1 :
154
                    return mult_T_ * pts_[0];
155
				case 2 :
156
                    return mult_T_ * (pts_[0] * dt +  nT * pts_[1]);
157
158
				break;
				case 3 :
159
                    return 	mult_T_ * (pts_[0] * dt * dt
160
                       				+ 2 * pts_[1] * nT * dt
161
                        + pts_[2] * nT * nT);
162
				break;
163
                case 4 :
164
                    return 	mult_T_ * (pts_[0] * dt * dt * dt
165
166
						+ 3 * pts_[1] * nT * dt * dt 
						+ 3 * pts_[2] * nT * nT * dt 
167
                        + pts_[3] * nT * nT *nT);
168
                default :
169
                    return mult_T_ * evalHorner(nT);
170
171
172
173
				break;
			}
		}
	}
174

175
176
177
178
179
180
181
182
    ///  \brief Computes the derivative curve at order N.
    ///  \param order : order of the derivative
    ///  \param return : the value x(t)
    bezier_curve_t compute_derivate(const std::size_t order) const
    {
        if(order == 0) return *this;
        t_point_t derived_wp;
        for(typename t_point_t::const_iterator pit =  pts_.begin(); pit != pts_.end()-1; ++pit)
stevet's avatar
stevet committed
183
            derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
184
        if(derived_wp.empty())
185
            derived_wp.push_back(point_t::Zero(Dim));
186
        bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_, mult_T_ * (1./T_) );
187
188
189
        return deriv.compute_derivate(order-1);
    }

190
191
192
193
194
195
    ///  \brief Computes the primitive of the curve at order N.
    ///  \param constant : value of the primitive at t = 0
    ///  \param return : the curve x_1(t) such that d/dt(x_1(t)) = x_1(t)
    bezier_curve_t compute_primitive(const std::size_t order) const
    {
        if(order == 0) return *this;
Steve Tonneau's avatar
Steve Tonneau committed
196
        num_t new_degree = (num_t)(degree_+1);
197
        t_point_t n_wp;
198
        point_t current_sum =  point_t::Zero(Dim);
199
200
201
202
203
204
205
206
        // 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 =  pts_.begin(); pit != pts_.end(); ++pit)
        {
            current_sum += *pit;
            n_wp.push_back(current_sum / new_degree);
        }
207
        bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_, mult_T_*T_);
208
209
210
        return integ.compute_primitive(order-1);
    }

211
212
213
214
215
216
    ///  \brief Evaluates the derivative at order N of the curve.
    ///  If the derivative is to be evaluated several times, it is
    ///  rather recommended to compute the derivative curve using compute_derivate
    ///  \param order : order of the derivative
    ///  \param t : the time when to evaluate the spine
    ///  \param return : the value x(t)
217
    virtual point_t derivate(const time_t t, const std::size_t order) const
218
219
220
221
222
    {
        bezier_curve_t deriv =compute_derivate(order);
        return deriv(t);
    }

223
224
    ///
    /// \brief Evaluates all Bernstein polynomes for a certain degree
225
226
    /// Warning: the horner scheme is about 100 times faster than this method.
    /// This method will probably be removed in the future
227
228
229
    ///
    point_t evalBernstein(const Numeric u) const
    {
230
        point_t res = point_t::Zero(Dim);
231
232
233
234
235
236
237
        typename t_point_t::const_iterator pts_it = pts_.begin();
        for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
            cit !=bernstein_.end(); ++cit, ++pts_it)
            res += cit->operator()(u) * (*pts_it);
        return res;
    }

238
239
240
241
242
243
244
245
246
247
248
249

    ///
    /// \brief Evaluates all Bernstein polynomes for a certain degree using horner's scheme
    ///
    point_t evalHorner(const Numeric t) const
    {
        typename t_point_t::const_iterator pts_it = pts_.begin();
        Numeric u, bc, tn;
        u = 1.0 - t;
        bc = 1;
        tn = 1;
        point_t tmp =(*pts_it)*u; ++pts_it;
stevet's avatar
stevet committed
250
        for(unsigned int i=1; i<degree_; i++, ++pts_it)
251
252
        {
            tn = tn*t;
stevet's avatar
stevet committed
253
            bc = bc*((num_t)(degree_-i+1))/i;
254
255
256
257
258
            tmp = (tmp + tn*bc*(*pts_it))*u;
        }
        return (tmp + tn*t*(*pts_it));
    }

Steve Tonneau's avatar
Steve Tonneau committed
259
    const t_point_t& waypoints() const {return pts_;}
Steve Tonneau's avatar
Steve Tonneau committed
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

    /**
     * @brief evalDeCasteljau evaluate the curve value at time t using deCasteljau algorithm
     * @param t unNormalized time
     * @return the point at time t
     */
    point_t evalDeCasteljau(const Numeric T) const {
        // normalize time :
        const Numeric t = T/T_;
        t_point_t pts = deCasteljauReduction(waypoints(),t);
        while(pts.size() > 1){
            pts = deCasteljauReduction(pts,t);
        }
        return pts[0]*mult_T_;
    }

    t_point_t deCasteljauReduction(const Numeric t) const{
        return deCasteljauReduction(waypoints(),t/T_);
    }

    /**
     * @brief deCasteljauReduction compute the de Casteljau's reduction of the given list of points at time t
     * @param pts the original list of points
     * @param t the NORMALIZED time
     * @return the reduced list of point (size of pts - 1)
     */
    t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric t) const{
        if(t < 0 || t > 1)
            throw std::out_of_range("In deCasteljau reduction : t 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-t) * (*cit) + t*(*(cit+1)));
        }
        return new_pts;
    }


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
    /**
     * @brief split split the curve in 2 at time t
     * @param t
     * @return
     */
    std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric T){
        t_point_t wps_first(size_),wps_second(size_);
        const double t = T/T_;
        wps_first[0] = pts_.front();
        wps_second[degree_] = pts_.back();
        t_point_t casteljau_pts = waypoints();
        size_t id = 1;
        while(casteljau_pts.size() > 1){
            casteljau_pts = deCasteljauReduction(casteljau_pts,t);
            wps_first[id] = casteljau_pts.front();
            wps_second[degree_-id] = casteljau_pts.back();
            ++id;
        }

        bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T,mult_T_);
        bezier_curve_t c_second(wps_second.begin(), wps_second.end(), T_-T,mult_T_);
        return std::make_pair(c_first,c_second);
    }

    bezier_curve_t extract(const Numeric t1, const Numeric t2){
        if(t1 < 0. || t1 > T_ || t2 < 0. || t2 > T_)
            throw std::out_of_range("In Extract curve : times out of bounds");
        if(t1 == 0.)
            return split(t2).first;
        if(t2 == T_)
            return split(t1).second;

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

337
338
339
340
341
342
343
    private:
    template<typename In>
    t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
    {
        t_point_t res;
        point_t P0, P1, P2, P_n_2, P_n_1, PN;
        P0 = *PointsBegin; PN = *(PointsEnd-1);
stevet's avatar
stevet committed
344
345
346
347
        P1    = P0+ constraints.init_vel / (num_t)degree_;
        P_n_1 = PN -constraints.end_vel  / (num_t)degree_;
        P2    = constraints.init_acc / (num_t)(degree_ * (degree_-1)) + 2* P1    - P0;
        P_n_2 = constraints.end_acc  / (num_t)(degree_ * (degree_-1)) + 2* P_n_1 - PN;
348
349
350
351
352
353
354
355
356
357
358
359
360
361

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

362
/*Operations*/
363
364

/*Helpers*/
365
    public:
366
367
    virtual time_t min() const{return 0.;}
    virtual time_t max() const{return T_;}
368
369
370
/*Helpers*/

	public:
371
372
    /*const*/ time_t T_;
    /*const*/ time_t mult_T_;
373
374
375
    /*const*/ std::size_t size_;
    /*const*/ std::size_t degree_;
    /*const*/ std::vector<Bern<Numeric> > bernstein_;
376
	
377
378
379
    private:
    t_point_t  pts_;

t steve's avatar
t steve committed
380
381
382
383
    public:
    static bezier_curve_t zero(const time_t T=1.)
    {
        std::vector<point_t> ts;
384
        ts.push_back(point_t::Zero(Dim));
t steve's avatar
t steve committed
385
386
        return bezier_curve_t(ts.begin(), ts.end(),T);
    }
387
};
388
389
390
}
#endif //_CLASS_BEZIERCURVE