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