piecewise_curve.h 12.9 KB
Newer Older
JasonChmn's avatar
JasonChmn committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
/**
* \file piecewise_curve.h
* \brief class allowing to create a piecewise curve.
* \author Jason C.
* \date 05/2019
*/

#ifndef _CLASS_PIECEWISE_CURVE
#define _CLASS_PIECEWISE_CURVE

#include "curve_abc.h"
#include "curve_conversion.h"

namespace curves
{
16
17
18
19
20
21
22
23
24
25
26
27
28
  /// \class PiecewiseCurve.
  /// \brief Represent a piecewise curve. We can add some new curve,
  ///        but the starting time of the curve to add should be equal to the ending time of the actual
  ///        piecewise_curve.<br>\ Example : A piecewise curve composed of three curves cf0, 
  ///        cf1 and cf2 where cf0 is defined between \f$[T0_{min},T0_{max}]\f$, cf1 between 
  ///        \f$[T0_{max},T1_{max}]\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
  ///        On the piecewise polynomial curve, cf0 is located between \f$[T0_{min},T0_{max}[\f$,
  ///        cf1 between \f$[T0_{max},T1_{max}[\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
  ///
  template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
           typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>, 
           typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point> >,
           typename Curve= curve_abc<Time, Numeric, Safe, Point> >
29
  struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>
30
  {
JasonChmn's avatar
JasonChmn committed
31
32
33
34
    typedef Point   point_t;
    typedef T_Point t_point_t;
    typedef Time    time_t;
    typedef Numeric num_t;
JasonChmn's avatar
JasonChmn committed
35
36
37
38
    typedef Curve curve_t;
    typedef typename std::vector < curve_t > t_curve_t;
    typedef typename std::vector< Time > t_time_t;

JasonChmn's avatar
JasonChmn committed
39
    public:
40
41
      /// \brief Empty constructor. Add at least one curve to call other class functions.
      ///
42
      piecewise_curve()
43
        : size_(0), T_min_(0), T_max_(0)
44
45
46
47
48
49
50
51
      {}

      /// \brief Constructor.
      /// Initialize a piecewise curve by giving the first curve.
      /// \param pol   : a polynomial curve.
      ///
      piecewise_curve(const curve_t& cf)
      {
JasonChmn's avatar
JasonChmn committed
52
53
        size_ = 0;
        add_curve(cf);
54
      }
JasonChmn's avatar
JasonChmn committed
55

56
57
      piecewise_curve(const t_curve_t list_curves)
      {
58
59
60
        size_ = 0;
        for( std::size_t i=0; i<list_curves.size(); i++ )
        {
61
          add_curve(list_curves[i]);
62
        }
63
      }
64

65
      piecewise_curve(const piecewise_curve& other)
66
        : curves_(other.curves_), time_curves_(other.time_curves_), size_(other.size_),
67
68
        T_min_(other.T_min_), T_max_(other.T_max_)
      {}
69

70
      virtual ~piecewise_curve(){}
JasonChmn's avatar
JasonChmn committed
71

72
73
      virtual Point operator()(const Time t) const
      {
74
        check_if_not_empty();
JasonChmn's avatar
JasonChmn committed
75
76
        if(Safe &! (T_min_ <= t && t <= T_max_))
        {
77
78
          //std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
          throw std::out_of_range("can't evaluate piecewise curve, out of range");
JasonChmn's avatar
JasonChmn committed
79
80
        }
        return curves_.at(find_interval(t))(t);
81
82
83
84
85
86
87
88
      }

      ///  \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
89
90
      { 
        check_if_not_empty();  
JasonChmn's avatar
JasonChmn committed
91
92
        if(Safe &! (T_min_ <= t && t <= T_max_))
        {
93
          throw std::invalid_argument("can't evaluate piecewise curve, out of range");
JasonChmn's avatar
JasonChmn committed
94
95
        }
        return (curves_.at(find_interval(t))).derivate(t, order);
96
97
98
99
100
101
102
103
104
      }

      ///  \brief Add a new curve to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$
      ///         is equal to \f$T_{max}\f$ of the actual piecewise curve. The curve added should be of type Curve as defined
      ///         in the template.
      ///  \param cf : curve to add.
      ///
      void add_curve(const curve_t& cf)
      {
JasonChmn's avatar
JasonChmn committed
105
        // Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
106
        if (size_!=0 && !(fabs(cf.min()-T_max_)<MARGIN))
JasonChmn's avatar
JasonChmn committed
107
        {
108
          throw std::invalid_argument("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
JasonChmn's avatar
JasonChmn committed
109
110
111
112
113
        }
        curves_.push_back(cf);
        size_ = curves_.size();
        T_max_ = cf.max();
        time_curves_.push_back(T_max_);
114
115
        if (size_ == 1)
        {
116
117
118
          // First curve added
          time_curves_.push_back(cf.min());
          T_min_ = cf.min();
119
        }
120
121
122
123
124
125
126
127
      }

      ///  \brief Check if the curve is continuous of order given.
      ///  \param order : order of continuity we want to check.
      ///  \return True if the curve is continuous of order given.
      ///
      bool is_continuous(const std::size_t order)
      {
128
        check_if_not_empty();
JasonChmn's avatar
JasonChmn committed
129
130
131
132
133
        bool isContinuous = true;
        std::size_t i=0;
        point_t value_end, value_start;
        while (isContinuous && i<(size_-1))
        {
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
          curve_t current = curves_.at(i);
          curve_t next = 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);
            value_start = next.derivate(next.min(),order);
          }
          if ((value_end-value_start).norm() > MARGIN)
          {
            isContinuous = false;
          }
          i++;
JasonChmn's avatar
JasonChmn committed
151
152
        }
        return isContinuous;
153
      }
JasonChmn's avatar
JasonChmn committed
154

155
156
157
      template<typename Bezier>
      piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> convert_piecewise_curve_to_bezier()
      {
158
        check_if_not_empty();
159
160
161
162
163
164
165
166
167
        typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> piecewise_curve_out_t;
        // Get first curve (segment)
        curve_t first_curve = curves_.at(0);
        Bezier first_curve_output = bezier_from_curve<Bezier, curve_t>(first_curve);
        // Create piecewise curve
        piecewise_curve_out_t pc_res(first_curve_output);
        // Convert and add all other curves (segments)
        for (std::size_t i=1; i<size_; i++)
        {
168
          pc_res.add_curve(bezier_from_curve<Bezier, curve_t>(curves_.at(i)));
169
170
        }
        return pc_res;
171
      }
172

173
174
175
      template<typename Hermite>
      piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite()
      {
176
        check_if_not_empty();
177
178
179
180
181
182
183
184
185
        typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> piecewise_curve_out_t;
        // Get first curve (segment)
        curve_t first_curve = curves_.at(0);
        Hermite first_curve_output = hermite_from_curve<Hermite, curve_t>(first_curve);
        // Create piecewise curve
        piecewise_curve_out_t pc_res(first_curve_output);
        // Convert and add all other curves (segments)
        for (std::size_t i=1; i<size_; i++)
        {
186
          pc_res.add_curve(hermite_from_curve<Hermite, curve_t>(curves_.at(i)));
187
188
        }
        return pc_res;
189
      }
190

191
192
193
      template<typename Polynomial>
      piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> convert_piecewise_curve_to_polynomial()
      {
194
        check_if_not_empty();
195
196
197
198
199
200
201
202
203
        typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
        // Get first curve (segment)
        curve_t first_curve = curves_.at(0);
        Polynomial first_curve_output = polynomial_from_curve<Polynomial, curve_t>(first_curve);
        // Create piecewise curve
        piecewise_curve_out_t pc_res(first_curve_output);
        // Convert and add all other curves (segments)
        for (std::size_t i=1; i<size_; i++)
        {
204
          pc_res.add_curve(polynomial_from_curve<Polynomial, curve_t>(curves_.at(i)));
205
206
        }
        return pc_res;
207
      }
208

209
210
211
212
      template<typename Polynomial>
      static piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> 
      convert_discrete_points_to_polynomial(T_Point points, Time T_min, Time T_max)
      {
213
214
        if(Safe &! (points.size()>1))
        {
215
216
          //std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
          throw std::invalid_argument("piecewise_curve -> convert_discrete_points_to_polynomial, Error, less than 2 discrete points");
217
218
        }
        typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
JasonChmn's avatar
JasonChmn committed
219
        Time discretization_step = (T_max-T_min)/Time(points.size()-1);
220
221
222
223
224
225
226
227
228
229
230
231
232
        Time time_actual = T_min;
        // Initialization at first points
        point_t actual_point = points[0];
        point_t next_point = points[1];
        point_t coeff_order_zero(actual_point);
        point_t coeff_order_one((next_point-actual_point)/discretization_step);
        t_point_t coeffs;
        coeffs.push_back(coeff_order_zero);
        coeffs.push_back(coeff_order_one);
        Polynomial pol(coeffs,time_actual,time_actual+discretization_step);
        piecewise_curve_out_t ppc(pol);
        time_actual += discretization_step;
        // Other points
JasonChmn's avatar
JasonChmn committed
233
        for (std::size_t i=1; i<points.size()-2; i++)
234
        {
235
236
237
238
239
240
241
242
243
          coeffs.clear();
          actual_point = points[i];
          next_point = points[i+1];
          coeff_order_zero = actual_point;
          coeff_order_one = (next_point-actual_point)/discretization_step;
          coeffs.push_back(coeff_order_zero);
          coeffs.push_back(coeff_order_one);
          ppc.add_curve(Polynomial(coeffs,time_actual,time_actual+discretization_step));
          time_actual += discretization_step;
244
245
246
247
248
249
250
251
252
253
254
        }
        // Last points
        coeffs.clear();
        actual_point = points[points.size()-2];
        next_point = points[points.size()-1];
        coeff_order_zero = actual_point;
        coeff_order_one = (next_point-actual_point)/discretization_step;
        coeffs.push_back(coeff_order_zero);
        coeffs.push_back(coeff_order_one);
        ppc.add_curve(Polynomial(coeffs,time_actual,T_max));
        return ppc;
255
      }
JasonChmn's avatar
JasonChmn committed
256
257
258

    private:

259
260
261
262
263
264
      /// \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.
      ///
      std::size_t find_interval(const Numeric t) const
      {   
JasonChmn's avatar
JasonChmn committed
265
266
267
        // time before first control point time.
        if(t < time_curves_[0])
        {
268
          return 0;
JasonChmn's avatar
JasonChmn committed
269
270
271
272
        }
        // time is after last control point time
        if(t > time_curves_[size_-1])
        {
273
          return size_-1;
JasonChmn's avatar
JasonChmn committed
274
275
276
277
278
279
        }

        std::size_t left_id = 0;
        std::size_t right_id = size_-1;
        while(left_id <= right_id)
        {
280
281
282
283
284
285
286
287
288
289
290
291
292
          const std::size_t middle_id = left_id + (right_id - left_id)/2;
          if(time_curves_.at(middle_id) < t)
          {
            left_id = middle_id+1;
          }
          else if(time_curves_.at(middle_id) > t)
          {
            right_id = middle_id-1;
          }
          else
          {
            return middle_id;
          }
JasonChmn's avatar
JasonChmn committed
293
294
        }
        return left_id-1;
295
      }
JasonChmn's avatar
JasonChmn committed
296

297
298
299
300
301
302
303
304
      void check_if_not_empty() const
      {
        if (curves_.size() == 0)
        {
          throw std::runtime_error("Error in piecewise curve : No curve added");
        }
      }

305

JasonChmn's avatar
JasonChmn committed
306
    /*Helpers*/
JasonChmn's avatar
JasonChmn committed
307
    public:
308
309
310
      /// \brief Get dimension of curve.
      /// \return dimension of curve.
      std::size_t virtual dim() const{return dim_;};
311
312
313
314
315
316
317
318
319
320
      /// \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_;}
      std::size_t getNumberCurves() { return curves_.size(); }
      /*Helpers*/

      /* Attributes */
321
      std::size_t dim_; // Dim of curve
322
323
324
325
326
327
328
329
330
331
332
333
      t_curve_t curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
      t_time_t time_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
      std::size_t size_; // Number of segments in piecewise curve = size of curves_
      Time T_min_, T_max_;
      static const double MARGIN;
      /* Attributes */

      // Serialization of the class
      friend class boost::serialization::access;

      template<class Archive>
      void serialize(Archive& ar, const unsigned int version){
334
        if (version) {
335
          // Do something depending on version ?
336
        }
337
        ar & boost::serialization::make_nvp("dim", dim_);
338
339
340
341
342
        ar & boost::serialization::make_nvp("curves", curves_);
        ar & boost::serialization::make_nvp("time_curves", time_curves_);
        ar & boost::serialization::make_nvp("size", size_);
        ar & boost::serialization::make_nvp("T_min", T_min_);
        ar & boost::serialization::make_nvp("T_max", T_max_);
343
344
      }
  }; // End struct piecewise curve
JasonChmn's avatar
JasonChmn committed
345

346
347
  template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point, typename Curve>
  const double piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Curve>::MARGIN(0.001);
348

JasonChmn's avatar
JasonChmn committed
349
350
351
352
} // end namespace


#endif // _CLASS_PIECEWISE_CURVE