piecewise_polynomial_curve.h 5.28 KB
Newer Older
1
2
3
4
5
6
7
/**
* \file piecewise_polynomial_curve.h
* \brief class allowing to create a piecewise polynomial curve.
* \author Jason C.
* \date 05/2019
*/

8
9
10
11
12
#ifndef _CLASS_PIECEWISE_CURVE
#define _CLASS_PIECEWISE_CURVE

#include "curve_abc.h"
#include "polynomial.h"
13
#include "curve_conversion.h"
14
15
16
17
18
19
20
21

namespace curves
{
/// \class PiecewiseCurve.
/// 
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
     typename Point= Eigen::Matrix<Numeric, Dim, 1>, 
22
23
24
     typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point> > 
     >
struct piecewise_polynomial_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
25
26
{

27
28
29
30
	typedef Point 	point_t;
	typedef T_Point t_point_t;
	typedef Time 	time_t;
    typedef Numeric	num_t;
31
    typedef int Index;
32
33

    //typedef polynomial  <double, double, 3, true, point_t, t_point_t> polynomial_t;
34
35
    typedef polynomial  <double, double, 3, true, point_t, t_point_t> polynomial_t;
    typedef typename std::vector < polynomial_t > t_polynomial_t;
36
37
38
    typedef std::vector<Time> t_vector_time_t;

	public:
39

40
41
42
43
44
	/// \brief Constructor.
    /// Initialize a piecewise polynomial curve by giving the first polynomial curve.
    /// \param pol   : a polynomial curve.
    ///
	piecewise_polynomial_curve(const polynomial_t& pol)
45
46
47
	{
		size_ = 0;
		add_polynomial_curve(pol);
48
49
        time_polynomial_curves_.push_back(pol.min());
        T_min_ = pol.min();
50
51
	}

52
53
54
55
56
57
	virtual ~piecewise_polynomial_curve(){}

	virtual Point operator()(const Time t) const
    {
        if(Safe &! (T_min_ <= t && t <= T_max_))
        {
58
        	//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
59
60
			throw std::out_of_range("can't evaluate piecewise curve, out of range");
        }
61
        return polynomial_curves_.at(find_interval(t))(t);
62
63
64
65
66
67
68
69
70
71
72
73
74
    }

    ///  \brief Evaluate the derivative of order N of curve at time t.
    ///  \param t : time when to evaluate the spline.
    ///  \param order : order of derivative.
    ///  \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
    ///
    virtual Point derivate(const Time t, const std::size_t order) const
    {   
        if(Safe &! (T_min_ <= t && t <= T_max_))
        {
			throw std::out_of_range("can't evaluate piecewise curve, out of range");
        }
75
        return (polynomial_curves_.at(find_interval(t))).derivate(t, order);
76
77
    }

78
79
80
81
82
83
84
85
86
87
88
89
	void add_polynomial_curve(polynomial_t pol)
	{
		// Check time continuity : Begin time of pol must be equal to T_max_ of actual piecewise curve.
		if (size_!=0 && pol.min()!=T_max_)
		{
			throw std::out_of_range("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
		}
		polynomial_curves_.push_back(pol);
		size_ = polynomial_curves_.size();
		T_max_ = pol.max();
		time_polynomial_curves_.push_back(T_max_);
	}
90

91
	bool is_continuous(const std::size_t order)
92
	{
93
		bool isContinuous = true;
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
    	Index i=0;
    	point_t value_end, value_start;
    	while (isContinuous && i<(size_-1))
    	{
    		polynomial_t current = polynomial_curves_.at(i);
    		polynomial_t next = polynomial_curves_.at(i+1);

    		if (order == 0)
    		{
    			value_end = current(current.max());
				value_start = next(next.min());
    		}
    		else
    		{
				value_end = current.derivate(current.max(),order);
109
				value_start = next.derivate(next.min(),order);
110
111
112
113
114
115
116
117
118
119
120
			}

    		if ((value_end-value_start).norm() > margin)
    		{
    			isContinuous = false;
    		}
    		i++;
    	}
    	return isContinuous;
	}

121
	private:
122
123
124
125
126

	/// \brief Get index of the interval corresponding to time t for the interpolation.
    /// \param t : time where to look for interval.
    /// \return Index of interval for time t.
    ///
127
    Index find_interval(const Numeric t) const
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
    {	
        // time before first control point time.
        if(t < time_polynomial_curves_[0])
        {
            return 0;
        }
        // time is after last control point time
        if(t > time_polynomial_curves_[size_-1])
        {
            return size_-1;
        }

        Index left_id = 0;
        Index right_id = size_-1;
        while(left_id <= right_id)
        {
            const Index middle_id = left_id + (right_id - left_id)/2;
            if(time_polynomial_curves_.at(middle_id) < t)
            {
                left_id = middle_id+1;
            }
            else if(time_polynomial_curves_.at(middle_id) > t)
            {
                right_id = middle_id-1;
            }
            else
            {
                return middle_id;
            }
        }
        return left_id-1;
    }

    /*Helpers*/
	public:
    /// \brief Get the minimum time for which the curve is defined
    /// \return \f$t_{min}\f$, lower bound of time range.
    Time virtual min() const{return T_min_;}
    /// \brief Get the maximum time for which the curve is defined.
    /// \return \f$t_{max}\f$, upper bound of time range.
    Time virtual max() const{return T_max_;}
	/*Helpers*/

    /* Variables */
172
	t_polynomial_t polynomial_curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
173
	t_vector_time_t time_polynomial_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
174
	Numeric size_; // Number of segments in piecewise curve = size of polynomial_curves_
175
	Time T_min_, T_max_;
176
177
	const double margin = 0.001;
};
178
179
180
181
182

} // end namespace


#endif // _CLASS_PIECEWISE_CURVE