bezier_curve.h 10.4 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
    private:
    t_point_t  pts_;

t steve's avatar
t steve committed
304 305 306 307 308 309 310
    public:
    static bezier_curve_t zero(const time_t T=1.)
    {
        std::vector<point_t> ts;
        ts.push_back(point_t::Zero());
        return bezier_curve_t(ts.begin(), ts.end(),T);
    }
311
};
312 313 314
}
#endif //_CLASS_BEZIERCURVE