bezier_curve.h 31.3 KB
Newer Older
1
/**
Guilhem Saurel's avatar
Guilhem Saurel committed
2
3
4
5
6
7
 * \file bezier_curve.h
 * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
 * \author Steve T.
 * \version 0.1
 * \date 06/17/2013
 */
8
9
10
11

#ifndef _CLASS_BEZIERCURVE
#define _CLASS_BEZIERCURVE

stonneau's avatar
stonneau committed
12
#include "curve_abc.h"
Steve T's avatar
Steve T committed
13
#include "cross_implementation.h"
14
#include "bernstein.h"
15
#include "curve_constraint.h"
stevet's avatar
stevet committed
16
#include "piecewise_curve.h"
17
18
19
20

#include "MathDefs.h"

#include <vector>
21
#include <stdexcept>
22

23
24
#include <iostream>

Steve T's avatar
Steve T committed
25
namespace ndcurves {
Guilhem Saurel's avatar
Guilhem Saurel committed
26
27
28
29
30
31
32
33
34
/// \class BezierCurve.
/// \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.
///
template <typename Time = double, typename Numeric = Time, bool Safe = false,
          typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
  typedef Point point_t;
stevet's avatar
stevet committed
35
36
  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
  typedef Eigen::Ref<const vector_x_t> vector_x_ref_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
37
38
39
40
41
42
  typedef Time time_t;
  typedef Numeric num_t;
  typedef curve_constraints<point_t> curve_constraints_t;
  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
  typedef typename t_point_t::const_iterator cit_point_t;
  typedef bezier_curve<Time, Numeric, Safe, Point> bezier_curve_t;
43
  typedef boost::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
stevet's avatar
stevet committed
44
  typedef piecewise_curve<Time, Numeric, Safe, point_t, point_t, bezier_curve_t> piecewise_curve_t;
45
  typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t;  // parent class
46
  typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
47
48
49
50

  /* Constructors - destructors */
 public:
  /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
51
  ///
Guilhem Saurel's avatar
Guilhem Saurel committed
52
53
54
55
56
57
  bezier_curve() : dim_(0), T_min_(0), T_max_(0) {}

  /// \brief Constructor.
  /// Given the first and last point of a control points set, create the bezier curve.
  /// \param PointsBegin   : an iterator pointing to the first element of a control point container.
  /// \param PointsEnd     : an iterator pointing to the last element of a control point container.
58
59
  /// \param T_min         : lower bound of time, curve will be defined for time in [T_min, T_max].
  /// \param T_max         : upper bound of time, curve will be defined for time in [T_min, T_max].
Guilhem Saurel's avatar
Guilhem Saurel committed
60
61
62
63
64
  /// \param mult_T        : ... (default value is 1.0).
  ///
  template <typename In>
  bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min = 0., const time_t T_max = 1.,
               const time_t mult_T = 1.)
65
66
      : dim_(PointsBegin->size()),
        T_min_(T_min),
Guilhem Saurel's avatar
Guilhem Saurel committed
67
68
69
70
        T_max_(T_max),
        mult_T_(mult_T),
        size_(std::distance(PointsBegin, PointsEnd)),
        degree_(size_ - 1),
Steve T's avatar
Steve T committed
71
        bernstein_(ndcurves::makeBernstein<num_t>((unsigned int)degree_)) 
72
  {
73
74
75
    if (bernstein_.size() != size_) {
      throw std::invalid_argument("Invalid size of polynomial");
    }
Guilhem Saurel's avatar
Guilhem Saurel committed
76
77
    In it(PointsBegin);
    if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
78
      throw std::invalid_argument("can't create bezier min bound is higher than max bound");
Guilhem Saurel's avatar
Guilhem Saurel committed
79
80
    }
    for (; it != PointsEnd; ++it) {
81
82
      if(Safe && static_cast<size_t>(it->size()) != dim_)
        throw std::invalid_argument("All the control points must have the same dimension.");
Guilhem Saurel's avatar
Guilhem Saurel committed
83
84
85
86
      control_points_.push_back(*it);
    }
  }

87
  /// \brief Constructor with constraints.
Guilhem Saurel's avatar
Guilhem Saurel committed
88
89
90
91
92
  /// This constructor will add 4 points (2 after the first one, 2 before the last one)
  /// to ensure that velocity and acceleration constraints are respected.
  /// \param PointsBegin   : an iterator pointing to the first element of a control point container.
  /// \param PointsEnd     : an iterator pointing to the last element of a control point container.
  /// \param constraints : constraints applying on start / end velocities and acceleration.
93
94
95
  /// \param T_min         : lower bound of time, curve will be defined for time in [T_min, T_max].
  /// \param T_max         : upper bound of time, curve will be defined for time in [T_min, T_max].
  /// \param mult_T        : ... (default value is 1.0).
Guilhem Saurel's avatar
Guilhem Saurel committed
96
97
98
99
  ///
  template <typename In>
  bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T_min = 0.,
               const time_t T_max = 1., const time_t mult_T = 1.)
100
101
      : dim_(PointsBegin->size()),
        T_min_(T_min),
Guilhem Saurel's avatar
Guilhem Saurel committed
102
103
104
105
        T_max_(T_max),
        mult_T_(mult_T),
        size_(std::distance(PointsBegin, PointsEnd) + 4),
        degree_(size_ - 1),
Steve T's avatar
Steve T committed
106
        bernstein_(ndcurves::makeBernstein<num_t>((unsigned int)degree_)) 
107
  {
Guilhem Saurel's avatar
Guilhem Saurel committed
108
109
110
111
112
    if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
      throw std::invalid_argument("can't create bezier min bound is higher than max bound");
    }
    t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
    for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) {
113
114
      if(Safe && static_cast<size_t>(cit->size()) != dim_)
        throw std::invalid_argument("All the control points must have the same dimension.");
Guilhem Saurel's avatar
Guilhem Saurel committed
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
      control_points_.push_back(*cit);
    }
  }

  bezier_curve(const bezier_curve& other)
      : dim_(other.dim_),
        T_min_(other.T_min_),
        T_max_(other.T_max_),
        mult_T_(other.mult_T_),
        size_(other.size_),
        degree_(other.degree_),
        bernstein_(other.bernstein_),
        control_points_(other.control_points_) {}

  ///\brief Destructor
  ~bezier_curve() {
    // NOTHING
  }

  /*Operations*/
  ///  \brief Evaluation of the bezier curve at time t.
  ///  \param t : time when to evaluate the curve.
  ///  \return \f$x(t)\f$ point corresponding on curve at time t.
  virtual point_t operator()(const time_t t) const {
    check_conditions();
    if (Safe & !(T_min_ <= t && t <= T_max_)) {
      throw std::invalid_argument("can't evaluate bezier curve, time t is out of range");  // TODO
    }
    if (size_ == 1) {
      return mult_T_ * control_points_[0];
    } else {
      return evalHorner(t);
    }
  }

150
151
  /**
   * @brief isApprox check if other and *this are approximately equals.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
152
153
   * Only two curves of the same class can be approximately equals, for comparison between different type of curves see
   * isEquivalent
154
155
   * @param other the other curve to check
   * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
156
   * @return true if the two curves are approximately equals
157
   */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
158
  bool isApprox(const bezier_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
Steve T's avatar
Steve T committed
159
    bool equal = ndcurves::isApprox<num_t>(T_min_, other.min()) && ndcurves::isApprox<num_t>(T_max_, other.max()) &&
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
160
                 dim_ == other.dim() && degree_ == other.degree() && size_ == other.size_ &&
Steve T's avatar
Steve T committed
161
                 ndcurves::isApprox<Numeric>(mult_T_, other.mult_T_) && bernstein_ == other.bernstein_;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
162
163
164
    if (!equal) return false;
    for (size_t i = 0; i < size_; ++i) {
      if (!control_points_.at(i).isApprox(other.control_points_.at(i), prec)) return false;
Pierre Fernbach's avatar
Pierre Fernbach committed
165
166
167
168
    }
    return true;
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
169
170
  virtual bool isApprox(const curve_abc_t* other,
                        const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
171
    const bezier_curve_t* other_cast = dynamic_cast<const bezier_curve_t*>(other);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
172
173
    if (other_cast)
      return isApprox(*other_cast, prec);
174
175
176
177
    else
      return false;
  }

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
178
  virtual bool operator==(const bezier_curve_t& other) const { return isApprox(other); }
Pierre Fernbach's avatar
Pierre Fernbach committed
179

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

Guilhem Saurel's avatar
Guilhem Saurel committed
182
183
184
185
  ///  \brief Compute the derived curve at order N.
  ///  Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
  ///  \param order : order of derivative.
  ///  \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
186
  bezier_curve_t compute_derivate(const std::size_t order) const {
Guilhem Saurel's avatar
Guilhem Saurel committed
187
    check_conditions();
188
    if (order == 0) {
189
      return *this;
190
    }
Guilhem Saurel's avatar
Guilhem Saurel committed
191
192
193
194
195
196
197
    t_point_t derived_wp;
    for (typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end() - 1; ++pit) {
      derived_wp.push_back((num_t)degree_ * (*(pit + 1) - (*pit)));
    }
    if (derived_wp.empty()) {
      derived_wp.push_back(point_t::Zero(dim_));
    }
198
199
    bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), T_min_, T_max_, mult_T_ * (1. / (T_max_ - T_min_)));
    return deriv.compute_derivate(order - 1);
200
  }
201

202
203
204
205
206
  ///  \brief Compute the derived curve at order N.
  ///  \param order : order of derivative.
  ///  \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
  bezier_curve_t* compute_derivate_ptr(const std::size_t order) const {
    return new bezier_curve_t(compute_derivate(order));
Guilhem Saurel's avatar
Guilhem Saurel committed
207
208
209
210
211
212
213
214
215
216
217
218
  }

  ///  \brief Compute the primitive of the curve at order N.
  ///  Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
  ///  At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
  ///  \param order : order of the primitive.
  ///  \return primitive at order N of x(t).
  bezier_curve_t compute_primitive(const std::size_t order) const {
    check_conditions();
    if (order == 0) {
      return *this;
    }
219
    num_t new_degree_inv = 1. / ((num_t)(degree_ + 1));
Guilhem Saurel's avatar
Guilhem Saurel committed
220
221
222
223
224
225
226
    t_point_t n_wp;
    point_t current_sum = point_t::Zero(dim_);
    // 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 = control_points_.begin(); pit != control_points_.end(); ++pit) {
      current_sum += *pit;
227
      n_wp.push_back(current_sum * new_degree_inv);
Guilhem Saurel's avatar
Guilhem Saurel committed
228
229
230
231
232
    }
    bezier_curve_t integ(n_wp.begin(), n_wp.end(), T_min_, T_max_, mult_T_ * (T_max_ - T_min_));
    return integ.compute_primitive(order - 1);
  }

233
  ///  \brief Computes a Bezier curve of order degrees higher than the current curve, but strictly equivalent.
234
  ///  Order elevation is required for addition / substraction and other comparison operations.
235
  ///  \param order : number of order the curve must be updated
236
  ///  \return An equivalent Bezier, with one more degree.
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
  bezier_curve_t elevate(const std::size_t order) const {
    t_point_t new_waypoints = control_points_, temp_waypoints;
    for (std::size_t i = 1; i<= order; ++i)
    {
        num_t new_degree_inv = 1. / ((num_t)(degree_ + i));
        temp_waypoints.push_back(*new_waypoints.begin());
        num_t idx_deg_inv = 0.;
        for (typename t_point_t::const_iterator pit = new_waypoints.begin()+1; pit != new_waypoints.end(); ++pit) {
          idx_deg_inv += new_degree_inv;
          temp_waypoints.push_back(idx_deg_inv * (*(pit-1)) + (1 - idx_deg_inv) * (*pit));
        }
        temp_waypoints.push_back(*(new_waypoints.end()-1));
        new_waypoints = temp_waypoints;
        temp_waypoints.clear();
    }
    return bezier_curve_t (new_waypoints.begin(), new_waypoints.end(), T_min_, T_max_, mult_T_);
  }

  ///  \brief Elevate the Bezier curve of order degrees higher than the current curve, but strictly equivalent.
  ///  Order elevation is required for addition / substraction and other comparison operations.
  ///  \param order : number of order the curve must be updated
  void elevate_self(const std::size_t order) {
    if (order > 0)
        (*this) = elevate(order);
261
262
  }

Guilhem Saurel's avatar
Guilhem Saurel committed
263
264
265
266
267
268
269
  ///  \brief Evaluate the derivative order N of curve at time t.
  ///  If derivative is to be evaluated several times, it is
  ///  rather recommended to compute derived curve using compute_derivate.
  ///  \param order : order of derivative.
  ///  \param t : time when to evaluate the curve.
  ///  \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
  ///
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
270
  virtual point_t derivate(const time_t t, const std::size_t order) const { return compute_derivate(order)(t); }
Guilhem Saurel's avatar
Guilhem Saurel committed
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323

  /// \brief Evaluate all Bernstein polynomes for a certain degree.
  /// A bezier curve with N control points is represented by : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
  /// with \f$ B_i^N(t) = \binom{N}{i}t^i (1-t)^{N-i} \f$.<br/>
  /// Warning: the horner scheme is about 100 times faster than this method.<br>
  /// This method will probably be removed in the future as the computation of bernstein polynomial is very costly.
  /// \param t : time when to evaluate the curve.
  /// \return \f$x(t)\f$ point corresponding on curve at time t.
  ///
  point_t evalBernstein(const Numeric t) const {
    const Numeric u = (t - T_min_) / (T_max_ - T_min_);
    point_t res = point_t::Zero(dim_);
    typename t_point_t::const_iterator control_points_it = control_points_.begin();
    for (typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin(); cit != bernstein_.end();
         ++cit, ++control_points_it) {
      res += cit->operator()(u) * (*control_points_it);
    }
    return res * mult_T_;
  }

  /// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
  /// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.<br>
  /// To evaluate the position on curve at time t,we can apply the Horner's scheme : <br>
  /// \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.<br>
  /// Horner's scheme : for a polynom of degree N expressed by : <br>
  /// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
  /// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$<br>
  /// Using Horner's method, the polynom is transformed into : <br>
  /// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
  /// \param t : time when to evaluate the curve.
  /// \return \f$x(t)\f$ point corresponding on curve at time t.
  ///
  point_t evalHorner(const Numeric t) const {
    const Numeric u = (t - T_min_) / (T_max_ - T_min_);
    typename t_point_t::const_iterator control_points_it = control_points_.begin();
    Numeric u_op, bc, tn;
    u_op = 1.0 - u;
    bc = 1;
    tn = 1;
    point_t tmp = (*control_points_it) * u_op;
    ++control_points_it;
    for (unsigned int i = 1; i < degree_; i++, ++control_points_it) {
      tn = tn * u;
      bc = bc * ((num_t)(degree_ - i + 1)) / i;
      tmp = (tmp + tn * bc * (*control_points_it)) * u_op;
    }
    return (tmp + tn * u * (*control_points_it)) * mult_T_;
  }

  const t_point_t& waypoints() const { return control_points_; }

  const point_t waypointAtIndex(const std::size_t index) const {
    point_t waypoint;
324
    if (index < control_points_.size()) {
Guilhem Saurel's avatar
Guilhem Saurel committed
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
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
      waypoint = control_points_[index];
    }
    return waypoint;
  }

  /// \brief Evaluate the curve value at time t using deCasteljau algorithm.
  /// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control
  /// points of bezier curve, and perform it iteratively until getting one point in the list which will be the
  /// evaluation of bezier curve at time \f$t\f$. \param t : time when to evaluate the curve. \return \f$x(t)\f$ point
  /// corresponding on curve at time t.
  ///
  point_t evalDeCasteljau(const Numeric t) const {
    // normalize time :
    const Numeric u = (t - T_min_) / (T_max_ - T_min_);
    t_point_t pts = deCasteljauReduction(waypoints(), u);
    while (pts.size() > 1) {
      pts = deCasteljauReduction(pts, u);
    }
    return pts[0] * mult_T_;
  }

  t_point_t deCasteljauReduction(const Numeric t) const {
    const Numeric u = (t - T_min_) / (T_max_ - T_min_);
    return deCasteljauReduction(waypoints(), u);
  }

  /// \brief Compute de Casteljau's reduction of the given list of points at time t.
  /// For the list \f$pts\f$ of N points, compute a new list of points of size N-1 :<br>
  /// \f$<br>( pts[0]*(1-t)+pts[1], pts[1]*(1-t)+pts[2], ..., pts[0]*(N-2)+pts[N-1] )\f$<br>
  /// with t the time when to evaluate bezier curve.<br>\ The new list contains centroid of
  /// parameters \f${t,1-t}\f$ of consecutive points in the list.
  /// \param pts : list of points.
  /// \param u   : NORMALIZED time when to evaluate the curve.
  /// \return reduced list of point (size of pts - 1).
  ///
  t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const {
    if (u < 0 || u > 1) {
      throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
    }
    if (pts.size() == 1) {
      return pts;
    }

    t_point_t new_pts;
    for (cit_point_t cit = pts.begin(); cit != (pts.end() - 1); ++cit) {
      new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1)));
    }
    return new_pts;
  }

  /// \brief Split the bezier curve in 2 at time t.
  /// \param t : list of points.
  /// \return pair containing the first element of both bezier curve obtained.
  ///
379
  std::pair<bezier_curve_t, bezier_curve_t> split(const Numeric t) const {
Guilhem Saurel's avatar
Guilhem Saurel committed
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
    check_conditions();
    if (fabs(t - T_max_) < MARGIN) {
      throw std::runtime_error("can't split curve, interval range is equal to original curve");
    }
    t_point_t wps_first(size_), wps_second(size_);
    const Numeric u = (t - T_min_) / (T_max_ - T_min_);
    t_point_t casteljau_pts = waypoints();
    wps_first[0] = casteljau_pts.front();
    wps_second[degree_] = casteljau_pts.back();
    size_t id = 1;
    while (casteljau_pts.size() > 1) {
      casteljau_pts = deCasteljauReduction(casteljau_pts, u);
      wps_first[id] = casteljau_pts.front();
      wps_second[degree_ - id] = casteljau_pts.back();
      ++id;
    }
396
397
    bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T_min_, t, mult_T_);
    bezier_curve_t c_second(wps_second.begin(), wps_second.end(), t, T_max_, mult_T_);
Guilhem Saurel's avatar
Guilhem Saurel committed
398
399
400
    return std::make_pair(c_first, c_second);
  }

stevet's avatar
stevet committed
401
  /// \brief Split the bezier curve in several curves, all accessible
402
  /// within a piecewise_curve_t.
stevet's avatar
stevet committed
403
  /// \param times : list of times of size n.
404
  /// \return a piecewise_curve_t comprising n+1 curves
stevet's avatar
stevet committed
405
  ///
406
  piecewise_curve_t split(const vector_x_t& times) const {
407
408
409
410
    std::vector<bezier_curve_t> curves;
    bezier_curve_t current = *this;
    for (int i = 0; i < times.rows(); ++i) {
      std::pair<bezier_curve_t, bezier_curve_t> pairsplit = current.split(times[i]);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
411
412
413
414
      curves.push_back(pairsplit.first);
      current = pairsplit.second;
    }
    curves.push_back(current);
415
    piecewise_curve_t res;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
416
    for (typename std::vector<bezier_curve_t>::const_iterator cit = curves.begin(); cit != curves.end(); ++cit) {
417
418
419
420
      typename piecewise_curve_t::curve_ptr_t ptr(new bezier_curve_t(*cit));
      res.add_curve_ptr(ptr);
    }
    return res;
stevet's avatar
stevet committed
421
422
  }

Guilhem Saurel's avatar
Guilhem Saurel committed
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
  /// \brief Extract a bezier curve defined between \f$[t_1,t_2]\f$ from the actual bezier curve
  ///        defined between \f$[T_{min},T_{max}]\f$ with \f$T_{min} \leq t_1 \leq t_2 \leq T_{max}\f$.
  /// \param t1 : start time of bezier curve extracted.
  /// \param t2 : end time of bezier curve extracted.
  /// \return bezier curve extract defined between \f$[t_1,t_2]\f$.
  ///
  bezier_curve_t extract(const Numeric t1, const Numeric t2) {
    if (t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_) {
      throw std::out_of_range("In Extract curve : times out of bounds");
    }
    if (fabs(t1 - T_min_) < MARGIN && fabs(t2 - T_max_) < MARGIN)  // t1=T_min and t2=T_max
    {
      return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
    }
    if (fabs(t1 - T_min_) < MARGIN)  // t1=T_min
    {
      return split(t2).first;
    }
    if (fabs(t2 - T_max_) < MARGIN)  // t2=T_max
    {
      return split(t1).second;
    }
    std::pair<bezier_curve_t, bezier_curve_t> c_split = this->split(t1);
    return c_split.second.split(t2).first;
  }

Steve T's avatar
Steve T committed
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
  ///  \brief Compute the cross product of the current bezier curve by another bezier curve.
  /// The cross product p1Xp2 of 2 bezier curves p1 and p2 is defined such that
  /// forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product.
  /// This method of course only makes sense for dimension 3 curves.
  /// It assumes that a method point_t cross(const point_t&, const point_t&) has been defined
  ///  \param pOther other polynomial to compute the cross product with.
  ///  \return a new polynomial defining the cross product between this and pother
  bezier_curve_t cross(const bezier_curve_t& g) const {
    //See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and
    //http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
    assert_operator_compatible(g);
    if (dim()!= 3)
        throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 ");
    int m =(int)(degree());
    int n =(int)(g.degree());
    unsigned int mj, n_ij, mn_i;
    t_point_t new_waypoints;
    for(int i = 0; i<= m+n; ++i)
    {
        bezier_curve_t::point_t current_point = bezier_curve_t::point_t::Zero(dim());
        for (int j = std::max(0,i-n); j <=std::min(m,i); ++j){
            mj = bin(m,j);
            n_ij = bin(n,i-j);
            mn_i = bin(m+n,i);
            num_t mul = num_t(mj*n_ij) / num_t(mn_i);
Steve T's avatar
Steve T committed
474
            current_point += mul*ndcurves::cross(waypointAtIndex(j), g.waypointAtIndex(i-j));
Steve T's avatar
Steve T committed
475
476
477
478
479
480
        }
        new_waypoints.push_back(current_point);
    }
    return bezier_curve_t(new_waypoints.begin(),new_waypoints.end(),min(),max(),mult_T_ * g.mult_T_);
  }

481
482
483
484
485
486
487
488
489
490
491
492
493
494

  ///  \brief Compute the cross product of the current bezier b by a point point.
  /// The cross product pXpoint of is defined such that
  /// forall t, bXpoint(t) = b(t) X point, with X designing the cross product.
  /// This method of course only makes sense for dimension 3 polynomials.
  ///  \param point point to compute the cross product with.
  ///  \return a new polynomial defining the cross product between this and point
  bezier_curve_t cross(const bezier_curve_t::point_t& point) const {
    //See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and
    //http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
    if (dim()!= 3)
        throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 ");
    t_point_t new_waypoints;
    for(typename t_point_t::const_iterator cit = waypoints().begin(); cit != waypoints().end(); ++cit){
Steve T's avatar
Steve T committed
495
      new_waypoints.push_back(ndcurves::cross(*cit, point));
496
497
498
499
    }
    return bezier_curve_t(new_waypoints.begin(),new_waypoints.end(),min(),max(),mult_T_);
  }

500
501
  bezier_curve_t& operator+=(const bezier_curve_t& other) {
      assert_operator_compatible(other);
502
      bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_); // TODO remove mult_T_ from Bezier
503
504
      if(other.degree() > degree()){
          elevate_self(other.degree() - degree());
505
      }
506
507
      else if(other_elevated.degree() < degree()){
          other_elevated.elevate_self(degree() - other_elevated.degree());
508
      }
509
510
511
      typename t_point_t::const_iterator otherit = other_elevated.control_points_.begin();
      for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it, ++otherit){
        (*it)+=(*otherit);
512
513
514
515
      }
      return *this;
    }

516
517
  bezier_curve_t& operator-=(const bezier_curve_t& other)  {
      assert_operator_compatible(other);
518
      bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_);
519
520
521
522
523
524
525
      if(other.degree() > degree()){
          elevate_self(other.degree() - degree());
      }
      else if(other_elevated.degree() < degree()){
          other_elevated.elevate_self(degree() - other_elevated.degree());
      }
      typename t_point_t::const_iterator otherit = other_elevated.control_points_.begin();
526
527
528
      for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it, ++otherit){
        (*it)-=(*otherit);
      }
529
      return *this;
530
531
    }

Steve T's avatar
Steve T committed
532
533
534
535
536
537
538
539
540
541
542
543
544
545
  bezier_curve_t& operator+=(const bezier_curve_t::point_t& point) {
    for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){
      (*it)+=point;
    }
    return *this;
  }

  bezier_curve_t& operator-=(const bezier_curve_t::point_t& point) {
    for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){
      (*it)-=point;
    }
    return *this;
  }

546
547
548
549
550
551
552
553
554
555
556
557
558
559
  bezier_curve_t& operator/=(const double d) {
    for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){
      (*it)/=d;
    }
    return *this;
  }

  bezier_curve_t& operator*=(const double d) {
    for (typename t_point_t::iterator it = control_points_.begin(); it!=control_points_.end(); ++it){
      (*it)*=d;
    }
    return *this;
  }

Steve T's avatar
Steve T committed
560

Guilhem Saurel's avatar
Guilhem Saurel committed
561
 private:
562
563
564
565
  /// \brief Ensure constraints of bezier curve.
  /// Add 4 points (2 after the first one, 2 before the last one) to biezer curve
  /// to ensure that velocity and acceleration constraints are respected.
  ///
Guilhem Saurel's avatar
Guilhem Saurel committed
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
  template <typename In>
  t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints) {
    t_point_t res;
    num_t T = T_max_ - T_min_;
    num_t T_square = T * T;
    point_t P0, P1, P2, P_n_2, P_n_1, PN;
    P0 = *PointsBegin;
    PN = *(PointsEnd - 1);
    P1 = P0 + constraints.init_vel * T / (num_t)degree_;
    P_n_1 = PN - constraints.end_vel * T / (num_t)degree_;
    P2 = constraints.init_acc * T_square / (num_t)(degree_ * (degree_ - 1)) + 2 * P1 - P0;
    P_n_2 = constraints.end_acc * T_square / (num_t)(degree_ * (degree_ - 1)) + 2 * P_n_1 - PN;
    res.push_back(P0);
    res.push_back(P1);
    res.push_back(P2);
    for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) {
      res.push_back(*it);
    }
    res.push_back(P_n_2);
    res.push_back(P_n_1);
    res.push_back(PN);
    return res;
  }

  void check_conditions() const {
    if (control_points_.size() == 0) {
      throw std::runtime_error(
          "Error in bezier curve : there is no control points set / did you use empty constructor ?");
    } else if (dim_ == 0) {
      throw std::runtime_error(
          "Error in bezier curve : Dimension of points is zero / did you use empty constructor ?");
    }
  }
599
600


Steve T's avatar
Steve T committed
601
  void assert_operator_compatible(const bezier_curve_t& other) const{
Steve T's avatar
Steve T committed
602
    if ((fabs(min() - other.min()) > MARGIN) || (fabs(max() - other.max()) > MARGIN)){
Steve T's avatar
Steve T committed
603
        throw std::invalid_argument("Can't perform base operation (+ - ) on two Bezier curves with different time ranges");
604
605
606
    }
  }

Guilhem Saurel's avatar
Guilhem Saurel committed
607
608
609
610
611
612
613
614
615
616
617
618
619
  /*Operations*/

 public:
  /*Helpers*/
  /// \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.
  virtual time_t 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.
  virtual time_t max() const { return T_max_; }
620
621
  /// \brief Get the degree of the curve.
  /// \return \f$degree\f$, the degree of the curve.
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
622
  virtual std::size_t degree() const { return degree_; }
Guilhem Saurel's avatar
Guilhem Saurel committed
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
  /*Helpers*/

  /* Attributes */
  /// Dim of curve
  std::size_t dim_;
  /// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
  /*const*/ time_t T_min_;
  /// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
  /*const*/ time_t T_max_;
  /*const*/ time_t mult_T_;
  /*const*/ std::size_t size_;
  /*const*/ std::size_t degree_;
  /*const*/ std::vector<Bern<Numeric> > bernstein_;
  /*const*/ t_point_t control_points_;
  /* Attributes */

  static bezier_curve_t zero(const std::size_t dim, const time_t T = 1.) {
    std::vector<point_t> ts;
    ts.push_back(point_t::Zero(dim));
    return bezier_curve_t(ts.begin(), ts.end(), 0., T);
  }

  // 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 ?
    }
653
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
Guilhem Saurel's avatar
Guilhem Saurel committed
654
655
656
657
658
659
660
661
662
663
664
    ar& boost::serialization::make_nvp("dim", dim_);
    ar& boost::serialization::make_nvp("T_min", T_min_);
    ar& boost::serialization::make_nvp("T_max", T_max_);
    ar& boost::serialization::make_nvp("mult_T", mult_T_);
    ar& boost::serialization::make_nvp("size", size_);
    ar& boost::serialization::make_nvp("degree", degree_);
    ar& boost::serialization::make_nvp("bernstein", bernstein_);
    ar& boost::serialization::make_nvp("control_points", control_points_);
  }
};  // End struct bezier_curve

665
666
667
668
669
670
671
672
673
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator+(const bezier_curve<T,N,S,P>& p1, const bezier_curve<T,N,S,P>& p2) {
  bezier_curve<T,N,S,P> res(p1);
  return res+=p2;
}

template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1) {
    std::vector<typename bezier_curve<T,N,S,P>::point_t> ts;
674
    for (std::size_t i = 0; i <= p1.degree(); ++i){
675
676
677
678
679
680
681
682
683
684
685
686
687
      ts.push_back(bezier_curve<T,N,S,P>::point_t::Zero(p1.dim()));
    }
    bezier_curve<T,N,S,P> res (ts.begin(),ts.end(),p1.min(),p1.max());
    res-=p1;
    return res;
}

template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1, const bezier_curve<T,N,S,P>& p2) {
    bezier_curve<T,N,S,P> res(p1);
    return res-=p2;
}

Steve T's avatar
Steve T committed
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712

template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator-(const bezier_curve<T,N,S,P>& p1, const typename bezier_curve<T,N,S,P>::point_t& point) {
  bezier_curve<T,N,S,P> res(p1);
  return res-=point;
}

template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator-(const typename bezier_curve<T,N,S,P>::point_t& point, const bezier_curve<T,N,S,P>& p1) {
  bezier_curve<T,N,S,P> res(-p1);
  return res+=point;
}

template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator+(const bezier_curve<T,N,S,P>& p1, const typename bezier_curve<T,N,S,P>::point_t& point) {
  bezier_curve<T,N,S,P> res(p1);
  return res+=point;
}

template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator+(const typename bezier_curve<T,N,S,P>::point_t& point, const bezier_curve<T,N,S,P>& p1) {
  bezier_curve<T,N,S,P> res(p1);
  return res+=point;
}

713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator/(const bezier_curve<T,N,S,P>& p1, const double k) {
    bezier_curve<T,N,S,P> res(p1);
    return res/=k;
}

template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator*(const bezier_curve<T,N,S,P>& p1,const double k)  {
    bezier_curve<T,N,S,P> res(p1);
    return res*=k;
}

template <typename T, typename N, bool S, typename P >
bezier_curve<T,N,S,P> operator*(const double k, const bezier_curve<T,N,S,P>& p1)  {
    bezier_curve<T,N,S,P> res(p1);
    return res*=k;
}

Steve T's avatar
Steve T committed
731
}  // namespace ndcurves
732
733

DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
Steve T's avatar
Steve T committed
734
                              SINGLE_ARG(ndcurves::bezier_curve<Time, Numeric, Safe, Point>))
735

Guilhem Saurel's avatar
Guilhem Saurel committed
736
#endif  //_CLASS_BEZIERCURVE