bezier_curve.h 8.95 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 258 259
    /*const*/ time_t minBound_, maxBound_;
    /*const*/ std::size_t size_;
    /*const*/ std::size_t degree_;
    /*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