9 #ifndef _CLASS_BEZIERCURVE 10 #define _CLASS_BEZIERCURVE 30 template <
typename Time = double,
typename Numeric =
Time,
bool Safe =
false,
31 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
34 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1>
vector_x_t;
39 typedef std::vector<point_t, Eigen::aligned_allocator<point_t> >
t_point_t;
61 template <
typename In>
62 bezier_curve(In PointsBegin, In PointsEnd,
const time_t T_min = 0.,
const time_t T_max = 1.,
63 const time_t mult_T = 1.)
64 : dim_(PointsBegin->size()),
68 size_(std::distance(PointsBegin, PointsEnd)),
72 if (bernstein_.size() != size_) {
73 throw std::invalid_argument(
"Invalid size of polynomial");
76 if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
77 throw std::invalid_argument(
"can't create bezier min bound is higher than max bound");
79 for (; it != PointsEnd; ++it) {
80 if(Safe && static_cast<size_t>(it->size()) != dim_)
81 throw std::invalid_argument(
"All the control points must have the same dimension.");
82 control_points_.push_back(*it);
96 template <
typename In>
97 bezier_curve(In PointsBegin, In PointsEnd,
const curve_constraints_t& constraints,
const time_t T_min = 0.,
98 const time_t T_max = 1.,
const time_t mult_T = 1.)
99 : dim_(PointsBegin->size()),
103 size_(std::distance(PointsBegin, PointsEnd) + 4),
107 if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
108 throw std::invalid_argument(
"can't create bezier min bound is higher than max bound");
110 t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
111 for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) {
112 if(Safe && static_cast<size_t>(cit->size()) != dim_)
113 throw std::invalid_argument(
"All the control points must have the same dimension.");
114 control_points_.push_back(*cit);
120 T_min_(other.T_min_),
121 T_max_(other.T_max_),
122 mult_T_(other.mult_T_),
124 degree_(other.degree_),
125 bernstein_(other.bernstein_),
126 control_points_(other.control_points_) {}
139 if (Safe & !(T_min_ <= t && t <= T_max_)) {
140 throw std::invalid_argument(
"can't evaluate bezier curve, time t is out of range");
143 return mult_T_ * control_points_[0];
145 return evalHorner(t);
157 bool isApprox(
const bezier_curve_t& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
158 bool equal = curves::isApprox<num_t>(T_min_, other.
min()) && curves::isApprox<num_t>(T_max_, other.
max()) &&
159 dim_ == other.
dim() && degree_ == other.
degree() && size_ == other.size_ &&
160 curves::isApprox<Numeric>(mult_T_, other.mult_T_) && bernstein_ == other.bernstein_;
161 if (!equal)
return false;
162 for (
size_t i = 0; i < size_; ++i) {
163 if (!control_points_.at(i).isApprox(other.control_points_.at(i), prec))
return false;
169 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
170 const bezier_curve_t* other_cast =
dynamic_cast<const bezier_curve_t*
>(other);
179 virtual bool operator!=(
const bezier_curve_t& other)
const {
return !(*
this == other); }
190 t_point_t derived_wp;
191 for (
typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end() - 1; ++pit) {
192 derived_wp.push_back((num_t)degree_ * (*(pit + 1) - (*pit)));
194 if (derived_wp.empty()) {
195 derived_wp.push_back(point_t::Zero(dim_));
197 bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), T_min_, T_max_, mult_T_ * (1. / (T_max_ - T_min_)));
198 return deriv.compute_derivate(order - 1);
218 num_t new_degree = (
num_t)(degree_ + 1);
220 point_t current_sum = point_t::Zero(dim_);
223 n_wp.push_back(current_sum);
224 for (
typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end(); ++pit) {
226 n_wp.push_back(current_sum / new_degree);
228 bezier_curve_t integ(n_wp.begin(), n_wp.end(), T_min_, T_max_, mult_T_ * (T_max_ - T_min_));
250 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
251 point_t res = point_t::Zero(dim_);
252 typename t_point_t::const_iterator control_points_it = control_points_.begin();
253 for (
typename std::vector<
Bern<Numeric> >::const_iterator cit = bernstein_.begin(); cit != bernstein_.end();
254 ++cit, ++control_points_it) {
255 res += cit->operator()(u) * (*control_points_it);
257 return res * mult_T_;
272 point_t evalHorner(
const Numeric t)
const {
273 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
274 typename t_point_t::const_iterator control_points_it = control_points_.begin();
275 Numeric u_op, bc, tn;
279 point_t tmp = (*control_points_it) * u_op;
281 for (
unsigned int i = 1; i < degree_; i++, ++control_points_it) {
283 bc = bc * ((
num_t)(degree_ - i + 1)) / i;
284 tmp = (tmp + tn * bc * (*control_points_it)) * u_op;
286 return (tmp + tn * u * (*control_points_it)) * mult_T_;
289 const t_point_t& waypoints()
const {
return control_points_; }
291 const point_t waypointAtIndex(
const std::size_t index)
const {
293 if (index < control_points_.size()) {
294 waypoint = control_points_[index];
305 point_t evalDeCasteljau(
const Numeric t)
const {
307 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
308 t_point_t pts = deCasteljauReduction(waypoints(), u);
309 while (pts.size() > 1) {
310 pts = deCasteljauReduction(pts, u);
312 return pts[0] * mult_T_;
315 t_point_t deCasteljauReduction(
const Numeric t)
const {
316 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
317 return deCasteljauReduction(waypoints(), u);
329 t_point_t deCasteljauReduction(
const t_point_t& pts,
const Numeric u)
const {
330 if (u < 0 || u > 1) {
331 throw std::out_of_range(
"In deCasteljau reduction : u is not in [0;1]");
333 if (pts.size() == 1) {
338 for (cit_point_t cit = pts.begin(); cit != (pts.end() - 1); ++cit) {
339 new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1)));
348 std::pair<bezier_curve_t, bezier_curve_t>
split(
const Numeric t)
const {
350 if (fabs(t - T_max_) < MARGIN) {
351 throw std::runtime_error(
"can't split curve, interval range is equal to original curve");
353 t_point_t wps_first(size_), wps_second(size_);
354 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
355 t_point_t casteljau_pts = waypoints();
356 wps_first[0] = casteljau_pts.front();
357 wps_second[degree_] = casteljau_pts.back();
359 while (casteljau_pts.size() > 1) {
360 casteljau_pts = deCasteljauReduction(casteljau_pts, u);
361 wps_first[id] = casteljau_pts.front();
362 wps_second[degree_ - id] = casteljau_pts.back();
365 bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T_min_, t, mult_T_);
366 bezier_curve_t c_second(wps_second.begin(), wps_second.end(), t, T_max_, mult_T_);
367 return std::make_pair(c_first, c_second);
375 piecewise_curve_t
split(
const vector_x_t& times)
const {
376 std::vector<bezier_curve_t>
curves;
377 bezier_curve_t current = *
this;
378 for (
int i = 0; i < times.rows(); ++i) {
379 std::pair<bezier_curve_t, bezier_curve_t> pairsplit = current.split(times[i]);
380 curves.push_back(pairsplit.first);
381 current = pairsplit.second;
383 curves.push_back(current);
384 piecewise_curve_t res;
385 for (
typename std::vector<bezier_curve_t>::const_iterator cit = curves.begin(); cit != curves.end(); ++cit) {
398 bezier_curve_t extract(
const Numeric t1,
const Numeric t2) {
399 if (t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_) {
400 throw std::out_of_range(
"In Extract curve : times out of bounds");
402 if (fabs(t1 - T_min_) < MARGIN && fabs(t2 - T_max_) < MARGIN)
404 return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
406 if (fabs(t1 - T_min_) < MARGIN)
408 return split(t2).first;
410 if (fabs(t2 - T_max_) < MARGIN)
412 return split(t1).second;
414 std::pair<bezier_curve_t, bezier_curve_t> c_split = this->
split(t1);
415 return c_split.second.split(t2).first;
423 template <
typename In>
424 t_point_t add_constraints(In PointsBegin, In PointsEnd,
const curve_constraints_t& constraints) {
426 num_t T = T_max_ - T_min_;
427 num_t T_square = T * T;
428 point_t P0, P1, P2, P_n_2, P_n_1, PN;
430 PN = *(PointsEnd - 1);
433 P2 = constraints.
init_acc * T_square / (
num_t)(degree_ * (degree_ - 1)) + 2 * P1 - P0;
434 P_n_2 = constraints.
end_acc * T_square / (
num_t)(degree_ * (degree_ - 1)) + 2 * P_n_1 - PN;
438 for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) {
441 res.push_back(P_n_2);
442 res.push_back(P_n_1);
447 void check_conditions()
const {
448 if (control_points_.size() == 0) {
449 throw std::runtime_error(
450 "Error in bezier curve : there is no control points set / did you use empty constructor ?");
451 }
else if (dim_ == 0) {
452 throw std::runtime_error(
453 "Error in bezier curve : Dimension of points is zero / did you use empty constructor ?");
462 std::size_t
virtual dim()
const {
return dim_; };
465 virtual time_t
min()
const {
return T_min_; }
468 virtual time_t
max()
const {
return T_max_; }
471 virtual std::size_t
degree()
const {
return degree_; }
484 std::vector<Bern<Numeric> > bernstein_;
485 t_point_t control_points_;
486 static const double MARGIN;
489 static bezier_curve_t zero(
const std::size_t
dim,
const time_t T = 1.) {
490 std::vector<point_t> ts;
491 ts.push_back(point_t::Zero(dim));
496 friend class boost::serialization::access;
498 template <
class Archive>
499 void serialize(Archive& ar,
const unsigned int version) {
503 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
504 ar& boost::serialization::make_nvp(
"dim", dim_);
505 ar& boost::serialization::make_nvp(
"T_min", T_min_);
506 ar& boost::serialization::make_nvp(
"T_max", T_max_);
507 ar& boost::serialization::make_nvp(
"mult_T", mult_T_);
508 ar& boost::serialization::make_nvp(
"size", size_);
509 ar& boost::serialization::make_nvp(
"degree", degree_);
510 ar& boost::serialization::make_nvp(
"bernstein", bernstein_);
511 ar& boost::serialization::make_nvp(
"control_points", control_points_);
515 template <
typename Time,
typename Numeric,
bool Safe,
typename Po
int>
520 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe,
typename Point),
523 #endif //_CLASS_BEZIERCURVE Point point_t
Definition: bezier_curve.h:33
virtual time_t max() const =0
Get the maximum time for which the curve is defined.
Eigen::Ref< const vector_x_t > vector_x_ref_t
Definition: bezier_curve.h:35
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: bezier_curve.h:168
bezier_curve()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: bezier_curve.h:51
void add_curve_ptr(const curve_ptr_t &cf)
Add a new curve to piecewise curve, which should be defined in where is equal to of the actual pie...
Definition: piecewise_curve.h:153
virtual bool operator!=(const bezier_curve_t &other) const
Definition: bezier_curve.h:179
double Numeric
Definition: effector_spline.h:26
point_t end_vel
Definition: curve_constraint.h:45
bezier_curve_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: bezier_curve.h:204
bool isApprox(const bezier_curve_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: bezier_curve.h:157
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: bezier_curve.h:44
bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
Constructor. Given the first and last point of a control points set, create the bezier curve...
Definition: bezier_curve.h:62
point_t init_acc
Definition: curve_constraint.h:43
virtual time_t min() const =0
Get the minimum time for which the curve is defined.
Definition: curve_constraint.h:21
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluate the derivative order N of curve at time t. If derivative is to be evaluated several times...
Definition: bezier_curve.h:239
interface for a Curve of arbitrary dimension.
Definition: bernstein.h:20
brief Computes all Bernstein polynomes for a certain degree std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Definition: bernstein.h:88
point_t init_vel
Definition: curve_constraint.h:42
boost::shared_ptr< curve_t > curve_ptr_t
Definition: piecewise_curve.h:40
piecewise_curve< Time, Numeric, Safe, point_t, point_t, bezier_curve_t > piecewise_curve_t
Definition: bezier_curve.h:43
point_t end_acc
Definition: curve_constraint.h:46
~bezier_curve()
Destructor.
Definition: bezier_curve.h:129
std::vector< bezier_curve< Numeric, Numeric, true, linear_variable< Numeric > > > split(const problem_definition< Point, Numeric > &pDef, problem_data< Point, Numeric > &pData)
Definition: details.h:208
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:39
bezier_curve< Time, Numeric, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:41
virtual std::size_t degree() const =0
Get the degree of the curve.
virtual std::size_t dim() const =0
Get dimension of curve.
curve_abc_t::curve_ptr_t curve_ptr_t
Definition: bezier_curve.h:45
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:38
boost::shared_ptr< bezier_curve_t > bezier_curve_ptr_t
Definition: bezier_curve.h:42
void serialize(Archive &ar, const unsigned int version)
Definition: curve_abc.h:139
bezier_curve_t compute_primitive(const std::size_t order) const
Compute the primitive of the curve at order N. Computes the primitive at order N of bezier curve of p...
Definition: bezier_curve.h:213
virtual point_t operator()(const time_t t) const
Evaluation of the bezier curve at time t.
Definition: bezier_curve.h:137
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:40
struct to define constraints on start / end velocities and acceleration on a curve ...
boost::shared_ptr< curve_t > curve_ptr_t
Definition: curve_abc.h:41
Numeric num_t
Definition: bezier_curve.h:37
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.)
Constructor with constraints. This constructor will add 4 points (2 after the first one...
Definition: bezier_curve.h:97
class allowing to create a piecewise curve.
bezier_curve_t compute_derivate(const std::size_t order) const
Compute the derived curve at order N. Computes the derivative order N, of bezier curve of parametric...
Definition: bezier_curve.h:185
bezier_curve(const bezier_curve &other)
Definition: bezier_curve.h:118
Time time_t
Definition: bezier_curve.h:36
double Time
Definition: effector_spline.h:27
point_t evalBernstein(const Numeric t) const
Evaluate all Bernstein polynomes for a certain degree. A bezier curve with N control points is repres...
Definition: bezier_curve.h:249
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
Definition: bezier_curve.h:32
virtual bool operator==(const bezier_curve_t &other) const
Definition: bezier_curve.h:177
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: bezier_curve.h:34