bezier_curve.h
Go to the documentation of this file.
1 
9 #ifndef _CLASS_BEZIERCURVE
10 #define _CLASS_BEZIERCURVE
11 
12 #include "curve_abc.h"
13 #include "bernstein.h"
14 #include "curve_constraint.h"
15 
16 #include "MathDefs.h"
17 
18 #include <vector>
19 #include <stdexcept>
20 
21 #include <iostream>
22 
23 namespace spline {
29 template <typename Time = double, typename Numeric = Time, Eigen::Index Dim = 3, bool Safe = false,
30  typename Point = Eigen::Matrix<Numeric, Dim, 1> >
31 struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> {
32  typedef Point point_t;
33  typedef Time time_t;
34  typedef Numeric num_t;
35  typedef curve_constraints<point_t> curve_constraints_t;
36  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
37  typedef typename t_point_t::const_iterator cit_point_t;
39 
40  /* Constructors - destructors */
41  public:
45  template <typename In>
46  bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound = 0, const time_t maxBound = 1)
47  : minBound_(minBound),
48  maxBound_(maxBound),
49  size_(std::distance(PointsBegin, PointsEnd)),
50  degree_(size_ - 1),
52  assert(bernstein_.size() == size_);
53  In it(PointsBegin);
54  if (Safe && (size_ < 1 || minBound >= maxBound))
55  throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
56  for (; it != PointsEnd; ++it) pts_.push_back(*it);
57  }
58 
65  template <typename In>
66  bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t minBound = 0,
67  const time_t maxBound = 1)
68  : minBound_(minBound),
69  maxBound_(maxBound),
70  size_(std::distance(PointsBegin, PointsEnd) + 4),
71  degree_(size_ - 1),
73  if (Safe && (size_ < 1 || minBound >= maxBound))
74  throw std::out_of_range("can't create bezier min bound is higher than max bound");
75  t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
76  for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) pts_.push_back(*cit);
77  }
78 
81  // NOTHING
82  }
83 
84  private:
85  // bezier_curve(const bezier_curve&);
86  // bezier_curve& operator=(const bezier_curve&);
87  /* Constructors - destructors */
88 
89  /*Operations*/
90  public:
94  virtual point_t operator()(const time_t t) const {
95  num_t nT = (t - minBound_) / (maxBound_ - minBound_);
96  if (Safe & !(0 <= nT && nT <= 1)) {
97  throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
98  } else {
99  num_t dt = (1 - nT);
100  switch (size_) {
101  case 1:
102  return pts_[0];
103  case 2:
104  return pts_[0] * dt + nT * pts_[1];
105  break;
106  case 3:
107  return pts_[0] * dt * dt + 2 * pts_[1] * nT * dt + pts_[2] * nT * nT;
108  break;
109  case 4:
110  return pts_[0] * dt * dt * dt + 3 * pts_[1] * nT * dt * dt + 3 * pts_[2] * nT * nT * dt +
111  pts_[3] * nT * nT * nT;
112  default:
113  return evalHorner(nT);
114  break;
115  }
116  }
117  }
118 
122  bezier_curve_t compute_derivate(const std::size_t order) const {
123  if (order == 0) return *this;
124  t_point_t derived_wp;
125  for (typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end() - 1; ++pit)
126  derived_wp.push_back(degree_ * (*(pit + 1) - (*pit)));
127  if (derived_wp.empty()) derived_wp.push_back(point_t::Zero());
128  bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), minBound_, maxBound_);
129  return deriv.compute_derivate(order - 1);
130  }
131 
135  bezier_curve_t compute_primitive(const std::size_t order) const {
136  if (order == 0) return *this;
137  num_t new_degree = (num_t)(degree_ + 1);
138  t_point_t n_wp;
139  point_t current_sum = point_t::Zero();
140  // recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
141  // then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
142  n_wp.push_back(current_sum);
143  for (typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end(); ++pit) {
144  current_sum += *pit;
145  n_wp.push_back(current_sum / new_degree);
146  }
147  bezier_curve_t integ(n_wp.begin(), n_wp.end(), minBound_, maxBound_);
148  return integ.compute_primitive(order - 1);
149  }
150 
157  virtual point_t derivate(const time_t t, const std::size_t order) const {
158  bezier_curve_t deriv = compute_derivate(order);
159  return deriv(t);
160  }
161 
165  point_t evalBernstein(const Numeric u) const {
166  point_t res = point_t::Zero();
167  typename t_point_t::const_iterator pts_it = pts_.begin();
168  for (typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin(); cit != bernstein_.end();
169  ++cit, ++pts_it)
170  res += cit->operator()(u) * (*pts_it);
171  return res;
172  }
173 
177  point_t evalHorner(const Numeric t) const {
178  typename t_point_t::const_iterator pts_it = pts_.begin();
179  Numeric u, bc, tn;
180  u = 1.0 - t;
181  bc = 1;
182  tn = 1;
183  point_t tmp = (*pts_it) * u;
184  ++pts_it;
185  for (int i = 1; i < degree_; i++, ++pts_it) {
186  tn = tn * t;
187  bc = bc * (degree_ - i + 1) / i;
188  tmp = (tmp + tn * bc * (*pts_it)) * u;
189  }
190  return (tmp + tn * t * (*pts_it));
191  }
192 
193  const t_point_t& waypoints() const { return pts_; }
194 
195  private:
196  template <typename In>
197  t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints) {
198  t_point_t res;
199  point_t P0, P1, P2, P_n_2, P_n_1, PN;
200  P0 = *PointsBegin;
201  PN = *(PointsEnd - 1);
202  P1 = P0 + constraints.init_vel / degree_;
203  P_n_1 = PN - constraints.end_vel / degree_;
204  P2 = constraints.init_acc / (degree_ * (degree_ - 1)) + 2 * P1 - P0;
205  P_n_2 = constraints.end_acc / (degree_ * (degree_ - 1)) + 2 * P_n_1 - PN;
206 
207  res.push_back(P0);
208  res.push_back(P1);
209  res.push_back(P2);
210 
211  for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) res.push_back(*it);
212 
213  res.push_back(P_n_2);
214  res.push_back(P_n_1);
215  res.push_back(PN);
216  return res;
217  }
218 
219  /*Operations*/
220 
221  /*Helpers*/
222  public:
223  virtual time_t min() const { return minBound_; }
224  virtual time_t max() const { return maxBound_; }
225  /*Helpers*/
226 
227  public:
229  const std::size_t size_;
230  const std::size_t degree_;
231  const std::vector<Bern<Numeric> > bernstein_;
232 
233  private:
234  t_point_t pts_;
235 
236  // storing bernstein polynoms, even in low dimension
237 };
238 } // namespace spline
239 #endif //_CLASS_BEZIERCURVE
const t_point_t & waypoints() const
Definition: bezier_curve.h:193
bezier_curve< Time, Numeric, Dim, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:38
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:35
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluates the derivative at order N of the curve. If the derivative is to be evaluated several times...
Definition: bezier_curve.h:157
Definition: bernstein.h:20
point_t evalHorner(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree using horner&#39;s scheme.
Definition: bezier_curve.h:177
const time_t maxBound_
Definition: bezier_curve.h:228
double Time
Definition: effector_spline.h:27
double Numeric
Definition: effector_spline.h:26
const std::vector< Bern< Numeric > > bernstein_
Definition: bezier_curve.h:231
bezier_curve_t compute_derivate(const std::size_t order) const
Computes the derivative curve at order N.
Definition: bezier_curve.h:122
std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Computes all Bernstein polynomes for a certain degree.
Definition: bernstein.h:59
const std::size_t degree_
Definition: bezier_curve.h:230
Point point_t
Definition: bezier_curve.h:32
Numeric num_t
Definition: bezier_curve.h:34
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:37
virtual time_t min() const
Definition: bezier_curve.h:223
Time time_t
Definition: bezier_curve.h:33
Definition: bernstein.h:40
point_t evalBernstein(const Numeric u) const
Evaluates all Bernstein polynomes for a certain degree.
Definition: bezier_curve.h:165
const std::size_t size_
Definition: bezier_curve.h:229
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: bezier_curve.h:94
Definition: bezier_curve.h:31
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t &constraints, const time_t minBound=0, const time_t maxBound=1)
Constructor This constructor will add 4 points (2 after the first one, 2 before the last one) to ensu...
Definition: bezier_curve.h:66
~bezier_curve()
Destructor.
Definition: bezier_curve.h:80
bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound=0, const time_t maxBound=1)
Constructor.
Definition: bezier_curve.h:46
bezier_curve_t compute_primitive(const std::size_t order) const
Computes the primitive of the curve at order N.
Definition: bezier_curve.h:135
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:36
virtual time_t max() const
Definition: bezier_curve.h:224
const time_t minBound_
Definition: bezier_curve.h:228
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28