bezier_curve.h 6.38 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 16 17 18

#include "MathDefs.h"

#include <vector>
19
#include <stdexcept>
20

21 22
#include <iostream>

23 24
namespace spline
{
25
/// \class BezierCurve
26 27 28
/// \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.
29
///
30
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
31
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
32
struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
33 34 35
{
	typedef Point 	point_t;
	typedef Time 	time_t;
36 37
    typedef Numeric	num_t;
    typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
38
    typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
39

40 41
/* Constructors - destructors */
	public:
42 43
	///\brief Constructor
	///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
44
    ///
45
	template<typename In>
stonneau's avatar
stonneau committed
46
	bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound=0, const time_t maxBound=1)
47 48 49
	: minBound_(minBound)
	, maxBound_(maxBound)
	, size_(std::distance(PointsBegin, PointsEnd))
50
    , bernstein_(spline::makeBernstein<num_t>(size_-1))
51
	{
52
        assert(bernstein_.size() == size_);
53
		In it(PointsBegin);
54
        if(Safe && (size_<1 || minBound >= maxBound))
55
		{
56
            throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
57 58 59 60 61 62
		}
		for(; it != PointsEnd; ++it)
		{
			pts_.push_back(*it);
		}
	}
63

64
	///\brief Destructor
stonneau's avatar
stonneau committed
65
	~bezier_curve()
66 67 68
	{
		// NOTHING
	}
69 70

	private:
71 72
//	bezier_curve(const bezier_curve&);
//  bezier_curve& operator=(const bezier_curve&);
73 74 75 76 77 78
/* Constructors - destructors */

/*Operations*/
	public:
	///  \brief Evaluation of the cubic spline at time t.
	///  \param t : the time when to evaluate the spine
79
	///  \param return : the value x(t)
80
    virtual point_t operator()(const time_t t) const
81 82 83 84
	{
		num_t nT = (t - minBound_) / (maxBound_ - minBound_);
		if(Safe &! (0 <= nT && nT <= 1))
		{
85
            throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
86
        }
87 88 89 90
		else
		{
			num_t dt = (1 - nT);
			switch(size_)
91 92 93
            {
                case 1 :
                    return pts_[0];
94 95 96 97 98 99 100 101
				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;
102
                case 4 :
103 104 105 106
					return 	pts_[0] * dt * dt * dt
						+ 3 * pts_[1] * nT * dt * dt 
						+ 3 * pts_[2] * nT * nT * dt 
						+ pts_[3] * nT * nT *nT;
107 108
                default :
                    return evalBernstein(dt);
109 110 111 112
				break;
			}
		}
	}
113

114 115 116 117 118 119
    ///  \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;
120
        num_t degree = (num_t)(size_-1);
121 122
        t_point_t derived_wp;
        for(typename t_point_t::const_iterator pit =  pts_.begin(); pit != pts_.end()-1; ++pit)
123
            derived_wp.push_back(degree * (*(pit+1) - (*pit)));
124 125 126
        if(derived_wp.empty())
            derived_wp.push_back(point_t::Zero());
        bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),minBound_,maxBound_);
Steve Tonneau's avatar
Steve Tonneau committed
127 128
        std::cout << "deriv size" << deriv.size_ << std::endl;
        std::cout << "val size" << size_ << std::endl;
129
        assert(deriv.size_ +1 == this->size_);
130 131 132
        return deriv.compute_derivate(order-1);
    }

133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154
    ///  \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;
        num_t new_degree = (num_t)(size_);
        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_);
        assert(integ.size_== this->size_ +1 );
        return integ.compute_primitive(order-1);
    }

155 156 157 158 159 160 161 162 163 164 165 166
    ///  \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)
    virtual point_t     derivate(const time_t t, const std::size_t order) const
    {
        bezier_curve_t deriv =compute_derivate(order);
        return deriv(t);
    }

167 168 169 170 171 172 173 174 175 176 177 178 179 180 181
    ///
    /// \brief Evaluates all Bernstein polynomes for a certain degree
    ///
    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;
    }

Steve Tonneau's avatar
Steve Tonneau committed
182 183 184 185 186
    const t_point_t& waypoints() const
    {
        return pts_;
    }

187
/*Operations*/
188 189

/*Helpers*/
stonneau's avatar
stonneau committed
190 191
	virtual time_t min() const{return minBound_;}
	virtual time_t max() const{return maxBound_;}
192 193 194
/*Helpers*/

	public:
195 196 197
    const time_t minBound_, maxBound_;
    const std::size_t size_;
    const std::vector<Bern<Numeric> > bernstein_;
198
	
199 200 201 202
    private:
    t_point_t  pts_;

    //storing bernstein polynoms, even in low dimension
203
};
204 205 206
}
#endif //_CLASS_BEZIERCURVE