bezier_curve.h 12.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
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
        {
Steve T's avatar
Steve T committed
143
144
145
            if(Safe &! (0 <= t && t <= T_))
                throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
            return evalHorner(t);
146
	}
147

148
149
150
151
152
153
154
155
    ///  \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
156
            derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
157
        if(derived_wp.empty())
158
            derived_wp.push_back(point_t::Zero(Dim));
159
        bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_, mult_T_ * (1./T_) );
160
161
162
        return deriv.compute_derivate(order-1);
    }

163
164
165
166
167
168
    ///  \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
169
        num_t new_degree = (num_t)(degree_+1);
170
        t_point_t n_wp;
171
        point_t current_sum =  point_t::Zero(Dim);
172
173
174
175
176
177
178
179
        // 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);
        }
180
        bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_, mult_T_*T_);
181
182
183
        return integ.compute_primitive(order-1);
    }

184
185
186
187
188
189
    ///  \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)
190
    virtual point_t derivate(const time_t t, const std::size_t order) const
191
192
193
194
195
    {
        bezier_curve_t deriv =compute_derivate(order);
        return deriv(t);
    }

196
197
    ///
    /// \brief Evaluates all Bernstein polynomes for a certain degree
198
199
    /// Warning: the horner scheme is about 100 times faster than this method.
    /// This method will probably be removed in the future
200
    ///
stevet's avatar
stevet committed
201
    point_t evalBernstein(const Numeric t) const
202
    {
stevet's avatar
stevet committed
203
        const Numeric u = t/T_;
204
        point_t res = point_t::Zero(Dim);
205
206
207
        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)
stevet's avatar
stevet committed
208
            res += cit->operator()(u) * (*pts_it);
209
        return res*mult_T_;
210
211
    }

212
213
214
215

    ///
    /// \brief Evaluates all Bernstein polynomes for a certain degree using horner's scheme
    ///
stevet's avatar
stevet committed
216
    point_t evalHorner(const Numeric t) const
217
    {
stevet's avatar
stevet committed
218
        const Numeric u = t/T_;
219
        typename t_point_t::const_iterator pts_it = pts_.begin();
stevet's avatar
stevet committed
220
221
        Numeric u_op, bc, tn;
        u_op = 1.0 - u;
222
223
        bc = 1;
        tn = 1;
stevet's avatar
stevet committed
224
        point_t tmp =(*pts_it)*u_op; ++pts_it;
stevet's avatar
stevet committed
225
        for(unsigned int i=1; i<degree_; i++, ++pts_it)
226
        {
stevet's avatar
stevet committed
227
            tn = tn*u;
stevet's avatar
stevet committed
228
            bc = bc*((num_t)(degree_-i+1))/i;
stevet's avatar
stevet committed
229
            tmp = (tmp + tn*bc*(*pts_it))*u_op;
230
        }
stevet's avatar
stevet committed
231
        return (tmp + tn*u*(*pts_it))*mult_T_;
232
233
    }

Steve Tonneau's avatar
Steve Tonneau committed
234
    const t_point_t& waypoints() const {return pts_;}
Steve Tonneau's avatar
Steve Tonneau committed
235

236
237
238
239
240
241

    /**
     * @brief evalDeCasteljau evaluate the curve value at time t using deCasteljau algorithm
     * @param t unNormalized time
     * @return the point at time t
     */
stevet's avatar
stevet committed
242
    point_t evalDeCasteljau(const Numeric t) const {
243
        // normalize time :
stevet's avatar
stevet committed
244
245
        const Numeric u = t/T_;
        t_point_t pts = deCasteljauReduction(waypoints(),u);
246
        while(pts.size() > 1){
stevet's avatar
stevet committed
247
            pts = deCasteljauReduction(pts,u);
248
249
250
251
252
253
254
255
256
257
258
        }
        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
stevet's avatar
stevet committed
259
     * @param u the NORMALIZED time
260
261
     * @return the reduced list of point (size of pts - 1)
     */
stevet's avatar
stevet committed
262
263
264
    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]");
265
266
267
268
269
        if(pts.size() == 1)
            return pts;

        t_point_t new_pts;
        for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit){
stevet's avatar
stevet committed
270
            new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
271
272
273
274
275
        }
        return new_pts;
    }


276
277
278
279
280
    /**
     * @brief split split the curve in 2 at time t
     * @param t
     * @return
     */
stevet's avatar
stevet committed
281
282
    std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t){
        if (t == T_)
stevet's avatar
stevet committed
283
            throw std::runtime_error("can't split curve, interval range is equal to original curve");
284
        t_point_t wps_first(size_),wps_second(size_);
stevet's avatar
stevet committed
285
        const double u = t/T_;
286
287
288
289
290
        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){
stevet's avatar
stevet committed
291
            casteljau_pts = deCasteljauReduction(casteljau_pts,u);
292
293
294
295
296
            wps_first[id] = casteljau_pts.front();
            wps_second[degree_-id] = casteljau_pts.back();
            ++id;
        }

stevet's avatar
stevet committed
297
298
        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_);
299
300
301
302
303
304
        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");
stevet's avatar
stevet committed
305
306
        if(t1 == 0. &&  t2 == T_)
            return bezier_curve_t(waypoints().begin(), waypoints().end(), T_,mult_T_);
307
308
309
310
311
312
        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);
313
        return c_split.second.split(t2-t1).first;
314
    }
315

316
317
318
319
320
321
322
    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
323
324
325
326
        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;
327
328
329
330
331
332
333
334
335
336
337
338
339
340

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

341
/*Operations*/
342
343

/*Helpers*/
344
    public:
345
346
    virtual time_t min() const{return 0.;}
    virtual time_t max() const{return T_;}
347
348
349
/*Helpers*/

	public:
350
351
    /*const*/ time_t T_;
    /*const*/ time_t mult_T_;
352
353
354
    /*const*/ std::size_t size_;
    /*const*/ std::size_t degree_;
    /*const*/ std::vector<Bern<Numeric> > bernstein_;
355
	
356
357
358
    private:
    t_point_t  pts_;

t steve's avatar
t steve committed
359
360
361
362
    public:
    static bezier_curve_t zero(const time_t T=1.)
    {
        std::vector<point_t> ts;
363
        ts.push_back(point_t::Zero(Dim));
t steve's avatar
t steve committed
364
365
        return bezier_curve_t(ts.begin(), ts.end(),T);
    }
366
};
367
368
369
}
#endif //_CLASS_BEZIERCURVE