piecewise_curve.h 23.1 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>
15
16
#include <fstream>
#include <sstream>
JasonChmn's avatar
JasonChmn committed
17

Guilhem Saurel's avatar
Guilhem Saurel committed
18
19
20
21
22
23
24
25
26
27
28
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,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
29
30
31
          typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>, typename Point_derivate = Point,
          typename CurveType = curve_abc<Time, Numeric, Safe, Point, Point_derivate> >
struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point, Point_derivate> {
Guilhem Saurel's avatar
Guilhem Saurel committed
32
  typedef Point point_t;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
33
  typedef Point_derivate point_derivate_t;
34
35
  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
36
37
  typedef Time time_t;
  typedef Numeric num_t;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
38
39
  typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> base_curve_t;  // parent class
  typedef CurveType curve_t;                                                       // contained curves base class
40
  typedef boost::shared_ptr<curve_t> curve_ptr_t;
41
  typedef typename std::vector<curve_ptr_t> t_curve_ptr_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
42
  typedef typename std::vector<Time> t_time_t;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
43
  typedef piecewise_curve<Time, Numeric, Safe, Point, Point_derivate, CurveType> piecewise_curve_t;
44
45
46
  typedef piecewise_curve<Time, Numeric, Safe, Point_derivate, Point_derivate,
    typename CurveType::curve_derivate_t> piecewise_curve_derivate_t;
  typedef boost::shared_ptr<typename piecewise_curve_derivate_t::curve_t> curve_derivate_ptr_t;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
47

Guilhem Saurel's avatar
Guilhem Saurel committed
48
49
 public:
  /// \brief Empty constructor. Add at least one curve to call other class functions.
50
  ///
Guilhem Saurel's avatar
Guilhem Saurel committed
51
  piecewise_curve() : dim_(0), size_(0), T_min_(0), T_max_(0) {}
52

Guilhem Saurel's avatar
Guilhem Saurel committed
53
54
  /// \brief Constructor.
  /// Initialize a piecewise curve by giving the first curve.
55
  /// \param cf   : a curve.
Guilhem Saurel's avatar
Guilhem Saurel committed
56
  ///
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
57
  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
58

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
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) {
61
      add_curve_ptr(*it);
Guilhem Saurel's avatar
Guilhem Saurel committed
62
63
64
65
66
67
68
69
70
71
72
73
74
    }
  }

  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() {}

75
  virtual point_t operator()(const Time t) const {
Guilhem Saurel's avatar
Guilhem Saurel committed
76
77
78
79
80
    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");
    }
81
    return (*curves_.at(find_interval(t)))(t);
Guilhem Saurel's avatar
Guilhem Saurel committed
82
83
  }

Pierre Fernbach's avatar
Pierre Fernbach committed
84
  /**
85
   * @brief isApprox check if other and *this are approximately equals.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
86
87
   * Only two curves of the same class can be approximately equals, for comparison between different type of curves see
   * isEquivalent
Pierre Fernbach's avatar
Pierre Fernbach committed
88
89
90
91
   * @param other the other curve to check
   * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
   * @return true is the two curves are approximately equals
   */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
92
93
94
95
96
  bool isApprox(const piecewise_curve_t& other,
                const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
    if (num_curves() != other.num_curves()) return false;
    for (size_t i = 0; i < num_curves(); ++i) {
      if (!curve_at_index(i)->isApprox(other.curve_at_index(i).get(), prec)) return false;
Pierre Fernbach's avatar
Pierre Fernbach committed
97
98
99
100
    }
    return true;
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
101
102
  virtual bool isApprox(const base_curve_t* other,
                        const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
103
    const piecewise_curve_t* other_cast = dynamic_cast<const piecewise_curve_t*>(other);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
104
105
    if (other_cast)
      return isApprox(*other_cast, prec);
106
107
108
    else
      return false;
  }
Pierre Fernbach's avatar
Pierre Fernbach committed
109

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
110
  virtual bool operator==(const piecewise_curve_t& other) const { return isApprox(other); }
Pierre Fernbach's avatar
Pierre Fernbach committed
111

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
112
  virtual bool operator!=(const piecewise_curve_t& other) const { return !(*this == other); }
Pierre Fernbach's avatar
Pierre Fernbach committed
113

Guilhem Saurel's avatar
Guilhem Saurel committed
114
115
116
117
118
  ///  \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.
  ///
119
  virtual point_derivate_t derivate(const Time t, const std::size_t order) const {
Guilhem Saurel's avatar
Guilhem Saurel committed
120
121
122
123
    check_if_not_empty();
    if (Safe & !(T_min_ <= t && t <= T_max_)) {
      throw std::invalid_argument("can't evaluate piecewise curve, out of range");
    }
124
    return (*curves_.at(find_interval(t))).derivate(t, order);
Guilhem Saurel's avatar
Guilhem Saurel committed
125
126
127
128
129
130
131
  }

  /**
   * @brief compute_derivate return a piecewise_curve which is the derivative of this at given order
   * @param order order of derivative
   * @return
   */
132
133
  piecewise_curve_derivate_t* compute_derivate_ptr(const std::size_t order) const {
    piecewise_curve_derivate_t* res(new piecewise_curve_derivate_t());
134
    for (typename t_curve_ptr_t::const_iterator itc = curves_.begin(); itc < curves_.end(); ++itc) {
135
      curve_derivate_ptr_t ptr((*itc)->compute_derivate_ptr(order));
136
      res->add_curve_ptr(ptr);
137
138
139
    }
    return res;
  }
140
141
142
143
144

  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
145
146
147
148
149
150
151
152
  }

  ///  \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.
  ///
153
  void add_curve_ptr(const curve_ptr_t& cf) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
154
    if (size_ == 0) {  // first curve added
155
      dim_ = cf->dim();
Guilhem Saurel's avatar
Guilhem Saurel committed
156
    }
157
    // Check time continuity : Beginning time of cf must be equal to T_max_ of actual piecewise curve.
158
    if (size_ != 0 && !(fabs(cf->min() - T_max_) < MARGIN)) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
159
160
161
162
      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();
163
      throw std::invalid_argument(ss.str().c_str());
Guilhem Saurel's avatar
Guilhem Saurel committed
164
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
165
166
167
168
    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();
169
      throw std::invalid_argument(ss.str().c_str());
170
    }
171
    curves_.push_back(cf);
Guilhem Saurel's avatar
Guilhem Saurel committed
172
    size_ = curves_.size();
173
    T_max_ = cf->max();
Guilhem Saurel's avatar
Guilhem Saurel committed
174
175
    if (size_ == 1) {
      // First curve added
176
177
      time_curves_.push_back(cf->min());
      T_min_ = cf->min();
Guilhem Saurel's avatar
Guilhem Saurel committed
178
    }
179
    time_curves_.push_back(T_max_);
Guilhem Saurel's avatar
Guilhem Saurel committed
180
181
182
183
184
185
186
187
188
189
  }

  ///  \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;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
190
    if (order == 0) {
191
192
      point_t value_end, value_start;
      while (isContinuous && i < (size_ - 1)) {
193
194
195
196
        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());
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
197
        if (!value_end.isApprox(value_start, MARGIN)) {
198
199
200
201
          isContinuous = false;
        }
        i++;
      }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
202
    } else {
203
204
      point_derivate_t value_end, value_start;
      while (isContinuous && i < (size_ - 1)) {
205
206
207
208
        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);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
209
        if (!value_end.isApprox(value_start, MARGIN)) {
210
211
212
          isContinuous = false;
        }
        i++;
213
      }
Guilhem Saurel's avatar
Guilhem Saurel committed
214
215
216
217
    }
    return isContinuous;
  }

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

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

222
  curve_ptr_t curve_at_index(const std::size_t idx) const {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
223
224
225
226
227
    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
228
229
  }

Guilhem Saurel's avatar
Guilhem Saurel committed
230
  template <typename Bezier>
231
  piecewise_curve_t convert_piecewise_curve_to_bezier() {
Guilhem Saurel's avatar
Guilhem Saurel committed
232
    check_if_not_empty();
233
234
235
    // 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
236
    // Create piecewise curve
237
    piecewise_curve_t pc_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
238
    // Convert and add all other curves (segments)
239
    for (std::size_t i = 0; i < size_; i++) {
240
      pc_res.add_curve(bezier_from_curve<Bezier>(*curves_.at(i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
241
242
243
244
    }
    return pc_res;
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
245
246
  template <typename Hermite>
  piecewise_curve_t convert_piecewise_curve_to_cubic_hermite() {
Guilhem Saurel's avatar
Guilhem Saurel committed
247
    check_if_not_empty();
248
249
250
    // 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
251
    // Create piecewise curve
252
    piecewise_curve_t pc_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
253
    // Convert and add all other curves (segments)
254
    for (std::size_t i = 0; i < size_; i++) {
255
      pc_res.add_curve(hermite_from_curve<Hermite>(*curves_.at(i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
256
257
258
259
260
    }
    return pc_res;
  }

  template <typename Polynomial>
261
  piecewise_curve_t convert_piecewise_curve_to_polynomial() {
Guilhem Saurel's avatar
Guilhem Saurel committed
262
    check_if_not_empty();
263
264
265
    // 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
266
    // Create piecewise curve
267
    piecewise_curve_t pc_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
268
    // Convert and add all other curves (segments)
269
    for (std::size_t i = 0; i < size_; i++) {
270
      pc_res.add_curve(polynomial_from_curve<Polynomial>(*curves_.at(i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
271
272
273
274
275
    }
    return pc_res;
  }

  template <typename Polynomial>
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
276
  static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points, t_time_t time_points) {
Guilhem Saurel's avatar
Guilhem Saurel committed
277
278
279
280
281
282
283
284
285
286
    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.");
    }
287
288
289
    // 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);
290
    piecewise_curve_t piecewise_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
291
292
293
294
295
296
297
298

    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>
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
299
300
301
  static piecewise_curve_t convert_discrete_points_to_polynomial(t_point_t points,
                                                                 t_point_derivate_t points_derivative,
                                                                 t_time_t time_points) {
Guilhem Saurel's avatar
Guilhem Saurel committed
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
    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.");
    }
317
318
319
    // 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);
320
    piecewise_curve_t piecewise_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
321
322
323
324
325
326
327
328
329

    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>
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
330
331
332
333
  static piecewise_curve_t convert_discrete_points_to_polynomial(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
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
    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.");
    }
354
355
356
    // 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);
357
    piecewise_curve_t piecewise_res;
Guilhem Saurel's avatar
Guilhem Saurel committed
358
359
360
361
362
363
364
365
366

    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;
  }

367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
  /**
   * @brief load_piecewise_from_text_file build a piecewise polynomial from a list of discrete points read from a file.
   * The file should contains one points per line, optionally with it's derivative and second derivatives.
   * Each lines should then contains dim, 2*dim or 3*dim values
   * @param filename the (absolute) name of the file to load
   * @param dt the time step between each points in the file
   * @param dim the dimension of the curve
   * @return a piecewise curves containing polynomial connectiong all the points in the file
   */
  template <typename Polynomial>
  static piecewise_curve_t load_piecewise_from_text_file(const std::string& filename, const time_t dt, const size_t dim){
    if(dim <= 0)
      throw std::invalid_argument("The dimension should be strictly positive.");
    if(dt <= 0.)
      throw std::invalid_argument("The time step should be strictly positive.");

    piecewise_curve_t piecewise_res;
    std::ifstream file;
    file.open(filename.c_str());
    point_t last_pos = point_t::Zero(dim),
        last_vel = point_t::Zero(dim),
        last_acc = point_t::Zero(dim),
        new_pos = point_t::Zero(dim),
        new_vel = point_t::Zero(dim),
        new_acc = point_t::Zero(dim);
    bool use_vel, use_acc;
    std::string line;
    // read first line to found out if we use velocity / acceleration :
    std::getline(file, line);
    std::istringstream iss_length(line);
    const size_t length = std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
    if(length == dim){
      use_vel = false;
      use_acc = false;
    }else if(length == dim * 2){
      use_vel = true;
      use_acc = false;
    }else if(length == dim * 3){
      use_vel = true;
      use_acc = true;
    }else{
      std::stringstream error;
      error<<"The first line of the file shold contains either " << dim << ", " << dim * 2 << " or " << dim * 3 <<
             "values, got : " << length;
      throw std::invalid_argument(error.str());
    }
    // initialize the first points of the trajectory:
    num_t val;
    std::istringstream iss(line);
    for(size_t i = 0 ; i < dim ; ++i){
      iss >> val;
      last_pos[i] = val;
    }
    if(use_vel){
      for(size_t i = 0 ; i < dim ; ++i){
        iss >> val;
        last_vel[i] = val;
      }
    }
    if(use_acc){
      for(size_t i = 0 ; i < dim ; ++i){
        iss >> val;
        last_acc[i] = val;
      }
    }

    size_t current_length;
    size_t line_id = 0;
    // parse all lines of the file:
    while (std::getline(file, line))
    {
        ++line_id;
        std::istringstream iss_length(line);
        current_length = std::distance(std::istream_iterator<std::string>(iss_length), std::istream_iterator<std::string>());
        if(current_length != length){
          std::stringstream error;
          error<<"Cannot parse line " << line_id << " got " << current_length << " values instead of " << length;
          throw std::invalid_argument(error.str());
        }
        std::istringstream iss(line);
        // parse the points values from the file:
        for(size_t i = 0 ; i < dim ; ++i){
          iss >> val;
          new_pos[i] = val;
        }
        if(use_vel){
          for(size_t i = 0 ; i < dim ; ++i){
            iss >> val;
            new_vel[i] = val;
          }
        }
        if(use_acc){
          for(size_t i = 0 ; i < dim ; ++i){
            iss >> val;
            new_acc[i] = val;
          }
        }
        // append a new curves connectiong this points
        if(use_acc){
          piecewise_res.add_curve(Polynomial(last_pos, last_vel, last_acc,
                                             new_pos, new_vel, new_acc,
                                             dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
        }else if(use_vel){
          piecewise_res.add_curve(Polynomial(last_pos, last_vel,
                                             new_pos, new_vel,
                                             dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
        }else{
          piecewise_res.add_curve(Polynomial(last_pos, new_pos,
                                             dt * static_cast<time_t>(line_id - 1), dt * static_cast<time_t>(line_id)));
        }
        last_pos = new_pos;
        last_vel = new_vel;
        last_acc = new_acc;
    }

    file.close();
    return piecewise_res;
  }

Guilhem Saurel's avatar
Guilhem Saurel committed
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
 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;
511
      }
Guilhem Saurel's avatar
Guilhem Saurel committed
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
    }
    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_; }
533
534
  /// \brief Get the degree of the curve.
  /// \return \f$degree\f$, the degree of the curve.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
535
  virtual std::size_t degree() const {
536
537
    throw std::runtime_error("degree() method is not implemented for this type of curve.");
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
538
539
540
541
542
  std::size_t getNumberCurves() { return curves_.size(); }
  /*Helpers*/

  /* Attributes */
  std::size_t dim_;       // Dim of curve
543
  t_curve_ptr_t curves_;  // for curves 0/1/2 : [ curve0, curve1, curve2 ]
Guilhem Saurel's avatar
Guilhem Saurel committed
544
545
546
547
548
549
550
551
552
553
554
555
556
557
  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 ?
    }
stevet's avatar
stevet committed
558
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(base_curve_t);
Guilhem Saurel's avatar
Guilhem Saurel committed
559
    ar& boost::serialization::make_nvp("dim", dim_);
560
    ar& boost::serialization::make_nvp("curves", curves_);
Guilhem Saurel's avatar
Guilhem Saurel committed
561
562
563
564
565
566
567
    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

stevet's avatar
stevet committed
568
template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate, typename CurveType>
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
569
const double piecewise_curve<Time, Numeric, Safe, Point, Point_derivate, CurveType>::MARGIN(0.001);
Guilhem Saurel's avatar
Guilhem Saurel committed
570
571
572
573

}  // namespace curves

#endif  // _CLASS_PIECEWISE_CURVE