9 #ifndef _CLASS_BEZIERCURVE 10 #define _CLASS_BEZIERCURVE 29 template <
typename Time = double,
typename Numeric =
Time, std::size_t Dim = 3,
bool Safe =
false,
30 typename Point = Eigen::Matrix<Numeric, Dim, 1> >
36 typedef std::vector<point_t, Eigen::aligned_allocator<point_t> >
t_point_t;
45 template <
typename In>
49 size_(std::distance(PointsBegin, PointsEnd)),
54 if (Safe && (
size_ < 1 ||
T_ <= 0.))
55 throw std::out_of_range(
"can't create bezier min bound is higher than max bound");
56 for (; it != PointsEnd; ++it) pts_.push_back(*it);
62 template <
typename In>
66 size_(std::distance(PointsBegin, PointsEnd)),
71 if (Safe && (
size_ < 1 ||
T_ <= 0.))
72 throw std::out_of_range(
"can't create bezier min bound is higher than max bound");
73 for (; it != PointsEnd; ++it) pts_.push_back(*it);
79 template <
typename In>
80 bezier_curve(In PointsBegin, In PointsEnd,
const time_t T,
const time_t mult_T)
83 size_(std::distance(PointsBegin, PointsEnd)),
88 if (Safe && (
size_ < 1 ||
T_ <= 0.))
89 throw std::out_of_range(
"can't create bezier min bound is higher than max bound");
90 for (; it != PointsEnd; ++it) pts_.push_back(*it);
99 template <
typename In>
100 bezier_curve(In PointsBegin, In PointsEnd,
const curve_constraints_t& constraints,
const time_t T = 1.)
103 size_(std::distance(PointsBegin, PointsEnd) + 4),
106 if (Safe && (
size_ < 1 ||
T_ <= 0.))
107 throw std::out_of_range(
"can't create bezier min bound is higher than max bound");
108 t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
109 for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) pts_.push_back(*cit);
128 if (Safe & !(0 <= t && t <=
T_))
throw std::out_of_range(
"can't evaluate bezier curve, out of range");
140 if (order == 0)
return *
this;
141 t_point_t derived_wp;
142 for (
typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end() - 1; ++pit)
143 derived_wp.push_back((num_t)
degree_ * (*(pit + 1) - (*pit)));
144 if (derived_wp.empty()) derived_wp.push_back(point_t::Zero(Dim));
145 bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),
T_,
mult_T_ * (1. /
T_));
153 if (order == 0)
return *
this;
156 point_t current_sum = point_t::Zero(Dim);
159 n_wp.push_back(current_sum);
160 for (
typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end(); ++pit) {
162 n_wp.push_back(current_sum / new_degree);
164 bezier_curve_t integ(n_wp.begin(), n_wp.end(),
T_,
mult_T_ *
T_);
165 return integ.compute_primitive(order - 1);
174 virtual point_t
derivate(
const time_t t,
const std::size_t order)
const {
185 const Numeric u = t /
T_;
186 point_t res = point_t::Zero(Dim);
187 typename t_point_t::const_iterator pts_it = pts_.begin();
190 res += cit->operator()(u) * (*pts_it);
198 const Numeric u = t /
T_;
199 typename t_point_t::const_iterator pts_it = pts_.begin();
200 Numeric u_op, bc, tn;
204 point_t tmp = (*pts_it) * u_op;
206 for (
unsigned int i = 1; i <
degree_; i++, ++pts_it) {
208 bc = bc * ((
num_t)(degree_ - i + 1)) / i;
209 tmp = (tmp + tn * bc * (*pts_it)) * u_op;
211 return (tmp + tn * u * (*pts_it)) *
mult_T_;
223 const Numeric u = t /
T_;
225 while (pts.size() > 1) {
240 if (u < 0 || u > 1)
throw std::out_of_range(
"In deCasteljau reduction : u is not in [0;1]");
241 if (pts.size() == 1)
return pts;
244 for (cit_point_t cit = pts.begin(); cit != (pts.end() - 1); ++cit) {
245 new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1)));
255 std::pair<bezier_curve_t, bezier_curve_t>
split(
const Numeric t) {
256 if (t ==
T_)
throw std::runtime_error(
"can't split curve, interval range is equal to original curve");
257 t_point_t wps_first(
size_), wps_second(
size_);
258 const double u = t /
T_;
259 wps_first[0] = pts_.front();
260 wps_second[
degree_] = pts_.back();
263 while (casteljau_pts.size() > 1) {
265 wps_first[id] = casteljau_pts.front();
266 wps_second[
degree_ - id] = casteljau_pts.back();
270 bezier_curve_t c_first(wps_first.begin(), wps_first.end(), t,
mult_T_);
271 bezier_curve_t c_second(wps_second.begin(), wps_second.end(),
T_ - t,
mult_T_);
272 return std::make_pair(c_first, c_second);
275 bezier_curve_t
extract(
const Numeric t1,
const Numeric t2) {
276 if (t1 < 0. || t1 >
T_ || t2 < 0. || t2 >
T_)
throw std::out_of_range(
"In Extract curve : times out of bounds");
278 if (t1 == 0.)
return split(t2).first;
279 if (t2 == T_)
return split(t1).second;
281 std::pair<bezier_curve_t, bezier_curve_t> c_split = this->
split(t1);
282 return c_split.second.split(t2 - t1).first;
286 template <
typename In>
287 t_point_t add_constraints(In PointsBegin, In PointsEnd,
const curve_constraints_t& constraints) {
289 point_t P0, P1, P2, P_n_2, P_n_1, PN;
291 PN = *(PointsEnd - 1);
301 for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) res.push_back(*it);
303 res.push_back(P_n_2);
304 res.push_back(P_n_1);
313 virtual time_t
min()
const {
return 0.; }
314 virtual time_t
max()
const {
return T_; }
328 static bezier_curve_t
zero(
const time_t T = 1.) {
329 std::vector<point_t> ts;
330 ts.push_back(point_t::Zero(Dim));
335 #endif //_CLASS_BEZIERCURVE const t_point_t & waypoints() const
Definition: bezier_curve.h:214
bezier_curve< Time, Numeric, Dim, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:38
bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T)
Constructor.
Definition: bezier_curve.h:80
bezier_curve_t extract(const Numeric t1, const Numeric t2)
Definition: bezier_curve.h:275
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:35
Definition: curve_constraint.h:21
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:174
static bezier_curve_t zero(const time_t T=1.)
Definition: bezier_curve.h:328
Definition: bernstein.h:20
std::pair< bezier_curve_t, bezier_curve_t > split(const Numeric t)
split split the curve in 2 at time t
Definition: bezier_curve.h:255
point_t evalDeCasteljau(const Numeric t) const
evalDeCasteljau evaluate the curve value at time t using deCasteljau algorithm
Definition: bezier_curve.h:221
point_t evalHorner(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree using horner's scheme.
Definition: bezier_curve.h:197
bezier_curve(In PointsBegin, In PointsEnd)
Constructor.
Definition: bezier_curve.h:46
point_t evalBernstein(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree Warning: the horner scheme is about 100 times ...
Definition: bezier_curve.h:184
t_point_t deCasteljauReduction(const t_point_t &pts, const Numeric u) const
deCasteljauReduction compute the de Casteljau's reduction of the given list of points at time t ...
Definition: bezier_curve.h:239
interface for a Curve of arbitrary dimension.
t_point_t deCasteljauReduction(const Numeric t) const
Definition: bezier_curve.h:231
point_t init_vel
Definition: curve_constraint.h:26
double Time
Definition: effector_spline.h:27
double Numeric
Definition: effector_spline.h:26
bezier_curve_t compute_derivate(const std::size_t order) const
Computes the derivative curve at order N.
Definition: bezier_curve.h:139
std::vector< Bern< Numeric > > bernstein_
Definition: bezier_curve.h:322
bezier_curve(In PointsBegin, In PointsEnd, const time_t T)
Constructor.
Definition: bezier_curve.h:63
point_t end_acc
Definition: curve_constraint.h:29
time_t T_
Definition: bezier_curve.h:318
std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Computes all Bernstein polynomes for a certain degree.
Definition: bernstein.h:58
point_t init_acc
Definition: curve_constraint.h:27
std::size_t degree_
Definition: bezier_curve.h:321
point_t end_vel
Definition: curve_constraint.h:28
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
Returns the minimum time for wich curve is defined.
Definition: bezier_curve.h:313
Time time_t
Definition: bezier_curve.h:33
Definition: bernstein.h:39
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: bezier_curve.h:127
struct to define constraints on start / end velocities and acceleration on a curve ...
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: curve_abc.h:24
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t &constraints, const time_t T=1.)
Constructor This constructor will add 4 points (2 after the first one, 2 before the last one) to ensu...
Definition: bezier_curve.h:100
Definition: bezier_curve.h:31
time_t mult_T_
Definition: bezier_curve.h:319
~bezier_curve()
Destructor.
Definition: bezier_curve.h:113
bezier_curve_t compute_primitive(const std::size_t order) const
Computes the primitive of the curve at order N.
Definition: bezier_curve.h:152
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:36
std::size_t size_
Definition: bezier_curve.h:320
virtual time_t max() const
Returns the maximum time for wich curve is defined.
Definition: bezier_curve.h:314
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28