bezier_curve.h 8.93 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>
stonneau's avatar
stonneau committed
49
	bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound=0, const time_t maxBound=1)
50
51
52
	: minBound_(minBound)
	, maxBound_(maxBound)
	, 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 || minBound >= maxBound))
59
            throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
        for(; it != PointsEnd; ++it)
            pts_.push_back(*it);
    }


    ///\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>
    bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t minBound=0, const time_t maxBound=1)
    : minBound_(minBound)
    , maxBound_(maxBound)
    , size_(std::distance(PointsBegin, PointsEnd)+4)
    , degree_(size_-1)
    , bernstein_(spline::makeBernstein<num_t>(degree_))
    {
        if(Safe && (size_<1 || minBound >= maxBound))
            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);
    }
85

86
	///\brief Destructor
stonneau's avatar
stonneau committed
87
	~bezier_curve()
88
89
90
	{
		// NOTHING
	}
91
92

	private:
93
94
//	bezier_curve(const bezier_curve&);
//  bezier_curve& operator=(const bezier_curve&);
95
96
97
98
99
100
/* Constructors - destructors */

/*Operations*/
	public:
	///  \brief Evaluation of the cubic spline at time t.
	///  \param t : the time when to evaluate the spine
101
	///  \param return : the value x(t)
102
    virtual point_t operator()(const time_t t) const
103
104
105
106
	{
		num_t nT = (t - minBound_) / (maxBound_ - minBound_);
		if(Safe &! (0 <= nT && nT <= 1))
		{
107
            throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
108
        }
109
		else
110
111
        {
            num_t dt = (1 - nT);
112
			switch(size_)
113
114
115
            {
                case 1 :
                    return pts_[0];
116
117
118
119
120
121
122
123
				case 2 :
					return pts_[0] * dt +  nT * pts_[1];
				break;
				case 3 :
					return 	pts_[0] * dt * dt 
                       				+ 2 * pts_[1] * nT * dt
						+ pts_[2] * nT * nT;
				break;
124
                case 4 :
125
126
127
128
					return 	pts_[0] * dt * dt * dt
						+ 3 * pts_[1] * nT * dt * dt 
						+ 3 * pts_[2] * nT * nT * dt 
						+ pts_[3] * nT * nT *nT;
129
                default :
130
                    return evalHorner(nT);
131
132
133
134
				break;
			}
		}
	}
135

136
137
138
139
140
141
142
143
    ///  \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
144
            derived_wp.push_back(degree_ * (*(pit+1) - (*pit)));
145
146
147
148
149
150
        if(derived_wp.empty())
            derived_wp.push_back(point_t::Zero());
        bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),minBound_,maxBound_);
        return deriv.compute_derivate(order-1);
    }

151
152
153
154
155
156
    ///  \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
157
        num_t new_degree = (num_t)(degree_+1);
158
159
160
161
162
163
164
165
166
167
168
169
170
171
        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);
        }
        bezier_curve_t integ(n_wp.begin(), n_wp.end(),minBound_,maxBound_);
        return integ.compute_primitive(order-1);
    }

172
173
174
175
176
177
    ///  \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)
178
    virtual point_t derivate(const time_t t, const std::size_t order) const
179
180
181
182
183
    {
        bezier_curve_t deriv =compute_derivate(order);
        return deriv(t);
    }

184
185
    ///
    /// \brief Evaluates all Bernstein polynomes for a certain degree
186
187
    /// Warning: the horner scheme is about 100 times faster than this method.
    /// This method will probably be removed in the future
188
189
190
191
192
193
194
195
196
197
198
    ///
    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;
    }

199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219

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

222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
    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;
    }

247
/*Operations*/
248
249

/*Helpers*/
250
    public:
stonneau's avatar
stonneau committed
251
252
	virtual time_t min() const{return minBound_;}
	virtual time_t max() const{return maxBound_;}
253
254
255
/*Helpers*/

	public:
256
257
    const time_t minBound_, maxBound_;
    const std::size_t size_;
Steve Tonneau's avatar
Steve Tonneau committed
258
    const std::size_t degree_;
259
    const std::vector<Bern<Numeric> > bernstein_;
260
	
261
262
263
264
    private:
    t_point_t  pts_;

    //storing bernstein polynoms, even in low dimension
265
};
266
267
268
}
#endif //_CLASS_BEZIERCURVE