bezier_curve.h 10.3 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
54
    , degree_(size_-1)
    , bernstein_(spline::makeBernstein<num_t>(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
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
    ///\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)
    , bernstein_(spline::makeBernstein<num_t>(degree_))
    {
        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)
    , bernstein_(spline::makeBernstein<num_t>(degree_))
    {
        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
116
117
    , size_(std::distance(PointsBegin, PointsEnd)+4)
    , degree_(size_-1)
    , bernstein_(spline::makeBernstein<num_t>(degree_))
    {
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)
Steve Tonneau's avatar
Steve Tonneau committed
183
            derived_wp.push_back(degree_ * (*(pit+1) - (*pit)));
184
185
        if(derived_wp.empty())
            derived_wp.push_back(point_t::Zero());
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
198
199
200
201
202
203
204
205
206
        t_point_t n_wp;
        point_t current_sum =  point_t::Zero();
        // 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
230
231
232
233
234
235
236
237
    ///
    point_t evalBernstein(const Numeric u) const
    {
        point_t res = point_t::Zero();
        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
250
251
252
253
254
255
256
257
258

    ///
    /// \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;
        for(int i=1; i<degree_; i++, ++pts_it)
        {
            tn = tn*t;
            bc = bc*(degree_-i+1)/i;
            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
    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);
        P1    = P0+ constraints.init_vel / degree_;
        P_n_1 = PN -constraints.end_vel  / degree_;
        P2    = constraints.init_acc / (degree_ * (degree_-1)) + 2* P1    - P0;
        P_n_2 = constraints.end_acc  / (degree_ * (degree_-1)) + 2* P_n_1 - PN;

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

286
/*Operations*/
287
288

/*Helpers*/
289
    public:
290
291
    virtual time_t min() const{return 0.;}
    virtual time_t max() const{return T_;}
292
293
294
/*Helpers*/

	public:
295
296
    /*const*/ time_t T_;
    /*const*/ time_t mult_T_;
297
298
299
    /*const*/ std::size_t size_;
    /*const*/ std::size_t degree_;
    /*const*/ std::vector<Bern<Numeric> > bernstein_;
300
	
301
302
303
304
    private:
    t_point_t  pts_;

    //storing bernstein polynoms, even in low dimension
305
};
306
307
308
}
#endif //_CLASS_BEZIERCURVE