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

#ifndef _CLASS_PIECEWISE_CURVE
#define _CLASS_PIECEWISE_CURVE

#include "curve_abc.h"
#include "curve_conversion.h"
13
#include <boost/smart_ptr/shared_ptr.hpp>
14
#include <boost/serialization/vector.hpp>
JasonChmn's avatar
JasonChmn committed
15

Guilhem Saurel's avatar
Guilhem Saurel committed
16
17
18
19
20
21
22
23
24
25
26
27
namespace curves {
/// \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, bool Safe = false,
          typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
28
29
          typename Point_derivate = Point >
struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_derivate> {
Guilhem Saurel's avatar
Guilhem Saurel committed
30
  typedef Point point_t;
31
32
33
  typedef Point_derivate   point_derivate_t;
  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
  typedef std::vector<point_derivate_t, Eigen::aligned_allocator<point_derivate_t> > t_point_derivate_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
34
35
  typedef Time time_t;
  typedef Numeric num_t;
36
37
  typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_t; // parent class
  typedef boost::shared_ptr<curve_t> curve_ptr_t;
38
  typedef typename std::vector<curve_ptr_t> t_curve_ptr_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
39
  typedef typename std::vector<Time> t_time_t;
40
  typedef piecewise_curve<Time, Numeric, Safe, Point,Point_derivate> piecewise_curve_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
41
42
 public:
  /// \brief Empty constructor. Add at least one curve to call other class functions.
43
  ///
Guilhem Saurel's avatar
Guilhem Saurel committed
44
  piecewise_curve() : dim_(0), size_(0), T_min_(0), T_max_(0) {}
45

Guilhem Saurel's avatar
Guilhem Saurel committed
46
47
  /// \brief Constructor.
  /// Initialize a piecewise curve by giving the first curve.
48
  /// \param cf   : a curve.
Guilhem Saurel's avatar
Guilhem Saurel committed
49
  ///
50
51
52
53
  piecewise_curve(const curve_ptr_t& cf):
    dim_(0), size_(0), T_min_(0), T_max_(0)
  {
    add_curve_ptr(cf);
Guilhem Saurel's avatar
Guilhem Saurel committed
54
55
  }

56
57
58
59
60
  piecewise_curve(const t_curve_ptr_t& curves_list):
    dim_(0), size_(0), T_min_(0), T_max_(0)
  {
    for(typename t_curve_ptr_t::const_iterator it = curves_list.begin() ; it != curves_list.end() ; ++it){
      add_curve_ptr(*it);
Guilhem Saurel's avatar
Guilhem Saurel committed
61
62
63
64
65
66
67
68
69
70
71
72
73
    }
  }

  piecewise_curve(const piecewise_curve& other)
      : dim_(other.dim_),
        curves_(other.curves_),
        time_curves_(other.time_curves_),
        size_(other.size_),
        T_min_(other.T_min_),
        T_max_(other.T_max_) {}

  virtual ~piecewise_curve() {}

74
  virtual point_t operator()(const Time t) const {
Guilhem Saurel's avatar
Guilhem Saurel committed
75
76
77
78
79
    check_if_not_empty();
    if (Safe & !(T_min_ <= t && t <= T_max_)) {
      // 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");
    }
80
    return (*curves_.at(find_interval(t)))(t);
Guilhem Saurel's avatar
Guilhem Saurel committed
81
82
83
84
85
86
87
  }

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

  /**
   * @brief compute_derivate return a piecewise_curve which is the derivative of this at given order
   * @param order order of derivative
   * @return
   */
101
  piecewise_curve_t* compute_derivate_ptr(const std::size_t order) const {
102
    piecewise_curve_t* res(new piecewise_curve_t());
103
    for (typename t_curve_ptr_t::const_iterator itc = curves_.begin(); itc < curves_.end(); ++itc) {
104
      curve_ptr_t ptr((*itc)->compute_derivate_ptr(order));
105
      res->add_curve_ptr(ptr);
106
107
108
    }
    return res;
  }
109
110
111
112
113
114


  template <typename Curve>
  void add_curve(const Curve& curve) {
    curve_ptr_t curve_ptr = boost::make_shared<Curve>(curve);
    add_curve_ptr(curve_ptr);
Guilhem Saurel's avatar
Guilhem Saurel committed
115
116
117
118
119
120
121
122
  }

  ///  \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.
  ///
123
124
125
  void add_curve_ptr(const curve_ptr_t& cf) {
    if (size_ == 0) { // first curve added
      dim_ = cf->dim();
Guilhem Saurel's avatar
Guilhem Saurel committed
126
    }
127
    // Check time continuity : Beginning time of cf must be equal to T_max_ of actual piecewise curve.
128
129
130
    if (size_ != 0 && !(fabs(cf->min() - T_max_) < MARGIN)) {
      std::stringstream ss; ss <<  "Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min(). Current T_max is "<<T_max_<<" new curve min is "<<cf->min();
      throw std::invalid_argument(ss.str().c_str());
Guilhem Saurel's avatar
Guilhem Saurel committed
131
    }
132
133
134
    if(cf->dim() != dim_){
      std::stringstream ss; ss << "All the curves in a piecewiseCurve should have the same dimension. Current dim is "<<dim_<<" dim of the new curve is "<<cf->dim();
      throw std::invalid_argument(ss.str().c_str());
135
    }
136
    curves_.push_back(cf);
Guilhem Saurel's avatar
Guilhem Saurel committed
137
    size_ = curves_.size();
138
    T_max_ = cf->max();
Guilhem Saurel's avatar
Guilhem Saurel committed
139
140
    if (size_ == 1) {
      // First curve added
141
142
      time_curves_.push_back(cf->min());
      T_min_ = cf->min();
Guilhem Saurel's avatar
Guilhem Saurel committed
143
    }
144
    time_curves_.push_back(T_max_);
Guilhem Saurel's avatar
Guilhem Saurel committed
145
146
  }

147
148


Guilhem Saurel's avatar
Guilhem Saurel committed
149
150
151
152
153
154
155
156
  ///  \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) {
    check_if_not_empty();
    bool isContinuous = true;
    std::size_t i = 0;
157
158
159
    if(order ==0){
      point_t value_end, value_start;
      while (isContinuous && i < (size_ - 1)) {
160
161
162
163
        curve_ptr_t current = curves_.at(i);
        curve_ptr_t next = curves_.at(i + 1);
        value_end = (*current)(current->max());
        value_start = (*next)(next->min());
164
165
166
167
168
169
170
171
        if (!value_end.isApprox(value_start,MARGIN)) {
          isContinuous = false;
        }
        i++;
      }
    }else{
      point_derivate_t value_end, value_start;
      while (isContinuous && i < (size_ - 1)) {
172
173
174
175
        curve_ptr_t current = curves_.at(i);
        curve_ptr_t next = curves_.at(i + 1);
        value_end = current->derivate(current->max(), order);
        value_start = next->derivate(next->min(), order);
176
177
178
179
        if (!value_end.isApprox(value_start,MARGIN)) {
          isContinuous = false;
        }
        i++;
180
      }
Guilhem Saurel's avatar
Guilhem Saurel committed
181
182
183
184
    }
    return isContinuous;
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
185
  std::size_t num_curves() const { return curves_.size(); }
stevet's avatar
stevet committed
186

187
  const curve_ptr_t& curve_at_time(const time_t t) const { return curves_[find_interval(t)]; }
stevet's avatar
stevet committed
188

189
  const curve_ptr_t& curve_at_index(const std::size_t idx) const {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
190
191
192
193
194
    if (Safe && idx >= num_curves()) {
      throw std::length_error(
          "curve_at_index: requested index greater than number of curves in piecewise_curve instance");
    }
    return curves_[idx];
stevet's avatar
stevet committed
195
196
  }

Guilhem Saurel's avatar
Guilhem Saurel committed
197
  template <typename Bezier>
198
  piecewise_curve_t convert_piecewise_curve_to_bezier() {
Guilhem Saurel's avatar
Guilhem Saurel committed
199
    check_if_not_empty();
200
201
202
    // check if given Bezier curve have the correct dimension :
    BOOST_STATIC_ASSERT(boost::is_same<typename Bezier::point_t, point_t>::value);
    BOOST_STATIC_ASSERT(boost::is_same<typename Bezier::point_derivate_t, point_derivate_t>::value);
Guilhem Saurel's avatar
Guilhem Saurel committed
203
    // Create piecewise curve
204
    piecewise_curve_t pc_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
205
    // Convert and add all other curves (segments)
206
    for (std::size_t i = 0; i < size_; i++) {
207
      pc_res.add_curve(bezier_from_curve<Bezier>(*curves_.at(i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
208
209
210
211
    }
    return pc_res;
  }

212
213
 template <typename Hermite>
 piecewise_curve_t convert_piecewise_curve_to_cubic_hermite() {
Guilhem Saurel's avatar
Guilhem Saurel committed
214
    check_if_not_empty();
215
216
217
    // check if given Hermite curve have the correct dimension :
    BOOST_STATIC_ASSERT(boost::is_same<typename Hermite::point_t, point_t>::value);
    BOOST_STATIC_ASSERT(boost::is_same<typename Hermite::point_derivate_t, point_derivate_t>::value);
Guilhem Saurel's avatar
Guilhem Saurel committed
218
    // Create piecewise curve
219
    piecewise_curve_t pc_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
220
    // Convert and add all other curves (segments)
221
    for (std::size_t i = 0; i < size_; i++) {
222
      pc_res.add_curve(hermite_from_curve<Hermite>(*curves_.at(i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
223
224
225
226
227
    }
    return pc_res;
  }

  template <typename Polynomial>
228
  piecewise_curve_t convert_piecewise_curve_to_polynomial() {
Guilhem Saurel's avatar
Guilhem Saurel committed
229
    check_if_not_empty();
230
231
232
    // check if given Polynomial curve have the correct dimension :
    BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
    BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
Guilhem Saurel's avatar
Guilhem Saurel committed
233
    // Create piecewise curve
234
    piecewise_curve_t pc_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
235
    // Convert and add all other curves (segments)
236
    for (std::size_t i = 0; i < size_; i++) {
237
      pc_res.add_curve(polynomial_from_curve<Polynomial>(*curves_.at(i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
238
239
240
241
    }
    return pc_res;
  }

242

Guilhem Saurel's avatar
Guilhem Saurel committed
243
  template <typename Polynomial>
244
  static piecewise_curve_t convert_discrete_points_to_polynomial(
245
      t_point_t points, t_time_t time_points) {
Guilhem Saurel's avatar
Guilhem Saurel committed
246
247
248
249
250
251
252
253
254
255
    if (Safe & !(points.size() > 1)) {
      // 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");
    }
    if (points.size() != time_points.size()) {
      throw std::invalid_argument(
          "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same "
          "size.");
    }
256
257
258
    // check if given Polynomial curve have the correct dimension :
    BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
    BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
259
    piecewise_curve_t piecewise_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
260
261
262
263
264
265
266
267

    for (size_t i = 1; i < points.size(); ++i) {
      piecewise_res.add_curve(Polynomial(points[i - 1], points[i], time_points[i - 1], time_points[i]));
    }
    return piecewise_res;
  }

  template <typename Polynomial>
268
  static piecewise_curve_t convert_discrete_points_to_polynomial(
269
      t_point_t points, t_point_derivate_t points_derivative, t_time_t time_points) {
Guilhem Saurel's avatar
Guilhem Saurel committed
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
    if (Safe & !(points.size() > 1)) {
      // 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");
    }
    if (points.size() != time_points.size()) {
      throw std::invalid_argument(
          "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same "
          "size.");
    }
    if (points.size() != points_derivative.size()) {
      throw std::invalid_argument(
          "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_derivative must have the "
          "same size.");
    }
285
286
287
    // check if given Polynomial curve have the correct dimension :
    BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
    BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
288
    piecewise_curve_t piecewise_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
289
290
291
292
293
294
295
296
297

    for (size_t i = 1; i < points.size(); ++i) {
      piecewise_res.add_curve(Polynomial(points[i - 1], points_derivative[i - 1], points[i], points_derivative[i],
                                         time_points[i - 1], time_points[i]));
    }
    return piecewise_res;
  }

  template <typename Polynomial>
298
  static piecewise_curve_t convert_discrete_points_to_polynomial(
299
      t_point_t points, t_point_derivate_t points_derivative, t_point_derivate_t points_second_derivative, t_time_t time_points) {
Guilhem Saurel's avatar
Guilhem Saurel committed
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
    if (Safe & !(points.size() > 1)) {
      // 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");
    }
    if (points.size() != time_points.size()) {
      throw std::invalid_argument(
          "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same "
          "size.");
    }
    if (points.size() != points_derivative.size()) {
      throw std::invalid_argument(
          "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_derivative must have the "
          "same size.");
    }
    if (points.size() != points_second_derivative.size()) {
      throw std::invalid_argument(
          "piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_second_derivative must "
          "have the same size.");
    }
320
321
322
    // check if given Polynomial curve have the correct dimension :
    BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
    BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
323
    piecewise_curve_t piecewise_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357

    for (size_t i = 1; i < points.size(); ++i) {
      piecewise_res.add_curve(Polynomial(points[i - 1], points_derivative[i - 1], points_second_derivative[i - 1],
                                         points[i], points_derivative[i], points_second_derivative[i],
                                         time_points[i - 1], time_points[i]));
    }
    return piecewise_res;
  }

 private:
  /// \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 {
    // time before first control point time.
    if (t < time_curves_[0]) {
      return 0;
    }
    // time is after last control point time
    if (t > time_curves_[size_ - 1]) {
      return size_ - 1;
    }

    std::size_t left_id = 0;
    std::size_t right_id = size_ - 1;
    while (left_id <= right_id) {
      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;
358
      }
Guilhem Saurel's avatar
Guilhem Saurel committed
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
    }
    return left_id - 1;
  }

  void check_if_not_empty() const {
    if (curves_.size() == 0) {
      throw std::runtime_error("Error in piecewise curve : No curve added");
    }
  }

  /*Helpers*/
 public:
  /// \brief Get dimension of curve.
  /// \return dimension of curve.
  std::size_t virtual dim() const { return dim_; };
  /// \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_; }
380
381
382
383
384
  /// \brief Get the degree of the curve.
  /// \return \f$degree\f$, the degree of the curve.
  virtual std::size_t  degree() const {
    throw std::runtime_error("degree() method is not implemented for this type of curve.");
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
385
386
387
388
389
  std::size_t getNumberCurves() { return curves_.size(); }
  /*Helpers*/

  /* Attributes */
  std::size_t dim_;       // Dim of curve
390
  t_curve_ptr_t curves_;  // for curves 0/1/2 : [ curve0, curve1, curve2 ]
Guilhem Saurel's avatar
Guilhem Saurel committed
391
392
393
394
395
396
397
398
399
400
401
402
403
404
  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) {
    if (version) {
      // Do something depending on version ?
    }
405
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_t);
Guilhem Saurel's avatar
Guilhem Saurel committed
406
    ar& boost::serialization::make_nvp("dim", dim_);
407
    ar& boost::serialization::make_nvp("curves", curves_);
Guilhem Saurel's avatar
Guilhem Saurel committed
408
409
410
411
412
413
414
    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_);
  }
};  // End struct piecewise curve

415
416
417

template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate>
const double piecewise_curve<Time, Numeric, Safe, Point,Point_derivate >::MARGIN(0.001);
Guilhem Saurel's avatar
Guilhem Saurel committed
418
419
420
421

}  // namespace curves

#endif  // _CLASS_PIECEWISE_CURVE