9 #ifndef _CLASS_BEZIERCURVE 10 #define _CLASS_BEZIERCURVE 29 template <
typename Time = double,
typename Numeric =
Time,
bool Safe =
false,
30 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
33 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1>
vector_x_t;
38 typedef std::vector<point_t, Eigen::aligned_allocator<point_t> >
t_point_t;
63 template <
typename In>
66 : dim_(PointsBegin->size()),
70 size_(std::distance(PointsBegin, PointsEnd)),
73 if (bernstein_.size() != size_) {
74 throw std::invalid_argument(
"Invalid size of polynomial");
77 if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
78 throw std::invalid_argument(
79 "can't create bezier min bound is higher than max bound");
81 for (; it != PointsEnd; ++it) {
82 if (Safe && static_cast<size_t>(it->size()) != dim_)
83 throw std::invalid_argument(
84 "All the control points must have the same dimension.");
85 control_points_.push_back(*it);
101 template <
typename In>
105 : dim_(PointsBegin->size()),
109 size_(std::distance(PointsBegin, PointsEnd) + 4),
112 if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
113 throw std::invalid_argument(
114 "can't create bezier min bound is higher than max bound");
117 add_constraints<In>(PointsBegin, PointsEnd, constraints);
118 for (
cit_point_t cit = updatedList.begin(); cit != updatedList.end();
120 if (Safe && static_cast<size_t>(cit->size()) != dim_)
121 throw std::invalid_argument(
122 "All the control points must have the same dimension.");
123 control_points_.push_back(*cit);
129 T_min_(other.T_min_),
130 T_max_(other.T_max_),
131 mult_T_(other.mult_T_),
133 degree_(other.degree_),
134 bernstein_(other.bernstein_),
135 control_points_(other.control_points_) {}
146 if (Safe & !(T_min_ <= t && t <= T_max_)) {
147 throw std::invalid_argument(
148 "can't evaluate bezier curve, time t is out of range");
151 return mult_T_ * control_points_[0];
153 return evalHorner(t);
168 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
169 bool equal = ndcurves::isApprox<num_t>(T_min_, other.
min()) &&
170 ndcurves::isApprox<num_t>(T_max_, other.
max()) &&
171 dim_ == other.
dim() && degree_ == other.
degree() &&
172 size_ == other.size_ &&
173 ndcurves::isApprox<Numeric>(mult_T_, other.mult_T_) &&
174 bernstein_ == other.bernstein_;
175 if (!equal)
return false;
176 for (
size_t i = 0; i < size_; ++i) {
177 if (!control_points_.at(i).isApprox(other.control_points_.at(i), prec))
185 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision())
const {
199 return !(*
this == other);
212 for (
typename t_point_t::const_iterator pit = control_points_.begin();
213 pit != control_points_.end() - 1; ++pit) {
214 derived_wp.push_back((
num_t)degree_ * (*(pit + 1) - (*pit)));
216 if (derived_wp.empty()) {
217 derived_wp.push_back(point_t::Zero(dim_));
219 bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), T_min_, T_max_,
220 mult_T_ * (1. / (T_max_ - T_min_)));
244 num_t new_degree_inv = 1. / ((
num_t)(degree_ + 1));
247 n_wp.push_back(current_sum);
248 for (
typename t_point_t::const_iterator pit = control_points_.begin();
249 pit != control_points_.end(); ++pit) {
251 n_wp.push_back(current_sum * new_degree_inv);
254 mult_T_ * (T_max_ - T_min_));
273 t_point_t new_waypoints = control_points_, temp_waypoints;
274 for (std::size_t i = 1; i <= order; ++i) {
275 num_t new_degree_inv = 1. / ((
num_t)(degree_ + i));
276 temp_waypoints.push_back(*new_waypoints.begin());
277 num_t idx_deg_inv = 0.;
278 for (
typename t_point_t::const_iterator pit = new_waypoints.begin() + 1;
279 pit != new_waypoints.end(); ++pit) {
280 idx_deg_inv += new_degree_inv;
281 temp_waypoints.push_back(idx_deg_inv * (*(pit - 1)) +
282 (1 - idx_deg_inv) * (*pit));
284 temp_waypoints.push_back(*(new_waypoints.end() - 1));
285 new_waypoints = temp_waypoints;
286 temp_waypoints.clear();
288 return bezier_curve_t(new_waypoints.begin(), new_waypoints.end(), T_min_,
297 if (order > 0) (*this) =
elevate(order);
322 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
323 point_t res = point_t::Zero(dim_);
324 typename t_point_t::const_iterator control_points_it =
325 control_points_.begin();
326 for (
typename std::vector<
Bern<Numeric> >::const_iterator cit =
328 cit != bernstein_.end(); ++cit, ++control_points_it) {
329 res += cit->operator()(u) * (*control_points_it);
331 return res * mult_T_;
347 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
348 typename t_point_t::const_iterator control_points_it =
349 control_points_.begin();
354 point_t tmp = (*control_points_it) * u_op;
356 for (
unsigned int i = 1; i < degree_; i++, ++control_points_it) {
358 bc = bc * ((
num_t)(degree_ - i + 1)) / i;
359 tmp = (tmp + tn * bc * (*control_points_it)) * u_op;
361 return (tmp + tn * u * (*control_points_it)) * mult_T_;
364 const t_point_t& waypoints()
const {
return control_points_; }
366 const point_t waypointAtIndex(
const std::size_t index)
const {
368 if (index < control_points_.size()) {
369 waypoint = control_points_[index];
384 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
385 t_point_t pts = deCasteljauReduction(waypoints(), u);
386 while (pts.size() > 1) {
387 pts = deCasteljauReduction(pts, u);
389 return pts[0] * mult_T_;
393 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
394 return deCasteljauReduction(waypoints(), u);
407 if (u < 0 || u > 1) {
408 throw std::out_of_range(
"In deCasteljau reduction : u is not in [0;1]");
410 if (pts.size() == 1) {
415 for (
cit_point_t cit = pts.begin(); cit != (pts.end() - 1); ++cit) {
416 new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1)));
425 std::pair<bezier_curve_t, bezier_curve_t>
split(
const Numeric t)
const {
427 if (fabs(t - T_max_) < MARGIN) {
428 throw std::runtime_error(
429 "can't split curve, interval range is equal to original curve");
431 t_point_t wps_first(size_), wps_second(size_);
432 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
434 wps_first[0] = casteljau_pts.front();
435 wps_second[degree_] = casteljau_pts.back();
437 while (casteljau_pts.size() > 1) {
438 casteljau_pts = deCasteljauReduction(casteljau_pts, u);
439 wps_first[id] = casteljau_pts.front();
440 wps_second[degree_ - id] = casteljau_pts.back();
443 bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T_min_, t,
445 bezier_curve_t c_second(wps_second.begin(), wps_second.end(), t, T_max_,
447 return std::make_pair(c_first, c_second);
456 std::vector<bezier_curve_t> curves;
458 for (
int i = 0; i < times.rows(); ++i) {
459 std::pair<bezier_curve_t, bezier_curve_t> pairsplit =
460 current.split(times[i]);
461 curves.push_back(pairsplit.first);
462 current = pairsplit.second;
464 curves.push_back(current);
466 for (
typename std::vector<bezier_curve_t>::const_iterator cit =
468 cit != curves.end(); ++cit) {
470 res.add_curve_ptr(ptr);
484 if (t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_) {
485 throw std::out_of_range(
"In Extract curve : times out of bounds");
487 if (fabs(t1 - T_min_) < MARGIN &&
488 fabs(t2 - T_max_) < MARGIN)
490 return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_,
493 if (fabs(t1 - T_min_) < MARGIN)
495 return split(t2).first;
497 if (fabs(t2 - T_max_) < MARGIN)
499 return split(t1).second;
501 std::pair<bezier_curve_t, bezier_curve_t> c_split = this->
split(t1);
502 return c_split.second.split(t2).first;
518 assert_operator_compatible(g);
520 throw std::invalid_argument(
521 "Can't perform cross product on Bezier curves with dimensions != 3 ");
523 int n = (int)(g.degree());
524 unsigned int mj, n_ij, mn_i;
526 for (
int i = 0; i <= m + n; ++i) {
528 bezier_curve_t::point_t::Zero(
dim());
529 for (
int j = std::max(0, i - n); j <= std::min(m, i); ++j) {
531 n_ij = bin(n, i - j);
532 mn_i = bin(m + n, i);
537 new_waypoints.push_back(current_point);
540 max(), mult_T_ * g.mult_T_);
555 throw std::invalid_argument(
556 "Can't perform cross product on Bezier curves with dimensions != 3 ");
558 for (
typename t_point_t::const_iterator cit = waypoints().begin();
559 cit != waypoints().end(); ++cit) {
567 assert_operator_compatible(other);
570 (other.mult_T_ / this->mult_T_);
571 if (other.degree() >
degree()) {
573 }
else if (other_elevated.degree() <
degree()) {
574 other_elevated.elevate_self(
degree() - other_elevated.degree());
576 typename t_point_t::const_iterator otherit =
577 other_elevated.control_points_.begin();
578 for (
typename t_point_t::iterator it = control_points_.begin();
579 it != control_points_.end(); ++it, ++otherit) {
586 assert_operator_compatible(other);
587 bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_);
588 if (other.degree() >
degree()) {
590 }
else if (other_elevated.degree() <
degree()) {
591 other_elevated.elevate_self(
degree() - other_elevated.degree());
593 typename t_point_t::const_iterator otherit =
594 other_elevated.control_points_.begin();
595 for (
typename t_point_t::iterator it = control_points_.begin();
596 it != control_points_.end(); ++it, ++otherit) {
603 for (
typename t_point_t::iterator it = control_points_.begin();
604 it != control_points_.end(); ++it) {
611 for (
typename t_point_t::iterator it = control_points_.begin();
612 it != control_points_.end(); ++it) {
619 for (
typename t_point_t::iterator it = control_points_.begin();
620 it != control_points_.end(); ++it) {
627 for (
typename t_point_t::iterator it = control_points_.begin();
628 it != control_points_.end(); ++it) {
639 template <
typename In>
640 t_point_t add_constraints(In PointsBegin, In PointsEnd,
643 num_t T = T_max_ - T_min_;
644 num_t T_square = T * T;
645 point_t P0, P1, P2, P_n_2, P_n_1, PN;
647 PN = *(PointsEnd - 1);
648 P1 = P0 + constraints.init_vel * T / (
num_t)degree_;
649 P_n_1 = PN - constraints.end_vel * T / (
num_t)degree_;
650 P2 = constraints.init_acc * T_square / (
num_t)(degree_ * (degree_ - 1)) +
652 P_n_2 = constraints.end_acc * T_square / (
num_t)(degree_ * (degree_ - 1)) +
657 for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) {
660 res.push_back(P_n_2);
661 res.push_back(P_n_1);
666 void check_conditions()
const {
667 if (control_points_.size() == 0) {
668 throw std::runtime_error(
669 "Error in bezier curve : there is no control points set / did you " 670 "use empty constructor ?");
671 }
else if (dim_ == 0) {
672 throw std::runtime_error(
673 "Error in bezier curve : Dimension of points is zero / did you use " 674 "empty constructor ?");
678 void assert_operator_compatible(
const bezier_curve_t& other)
const {
679 if ((fabs(
min() - other.min()) > MARGIN) ||
680 (fabs(
max() - other.max()) > MARGIN)) {
681 throw std::invalid_argument(
682 "Can't perform base operation (+ - ) on two Bezier curves with " 683 "different time ranges");
693 std::size_t
virtual dim()
const {
return dim_; };
696 virtual time_t min()
const {
return T_min_; }
699 virtual time_t max()
const {
return T_max_; }
702 virtual std::size_t
degree()
const {
return degree_; }
717 std::vector<Bern<Numeric> > bernstein_;
722 std::vector<point_t> ts;
723 ts.push_back(point_t::Zero(
dim));
728 friend class boost::serialization::access;
730 template <
class Archive>
731 void serialize(Archive& ar,
const unsigned int version) {
735 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(
curve_abc_t);
736 ar& boost::serialization::make_nvp(
"dim", dim_);
737 ar& boost::serialization::make_nvp(
"T_min", T_min_);
738 ar& boost::serialization::make_nvp(
"T_max", T_max_);
739 ar& boost::serialization::make_nvp(
"mult_T", mult_T_);
740 ar& boost::serialization::make_nvp(
"size", size_);
741 ar& boost::serialization::make_nvp(
"degree", degree_);
742 ar& boost::serialization::make_nvp(
"bernstein", bernstein_);
743 ar& boost::serialization::make_nvp(
"control_points", control_points_);
747 template <
typename T,
typename N,
bool S,
typename P>
754 template <
typename T,
typename N,
bool S,
typename P>
756 std::vector<typename bezier_curve<T, N, S, P>::point_t> ts;
757 for (std::size_t i = 0; i <= p1.
degree(); ++i) {
765 template <
typename T,
typename N,
bool S,
typename P>
772 template <
typename T,
typename N,
bool S,
typename P>
780 template <
typename T,
typename N,
bool S,
typename P>
788 template <
typename T,
typename N,
bool S,
typename P>
796 template <
typename T,
typename N,
bool S,
typename P>
804 template <
typename T,
typename N,
bool S,
typename P>
811 template <
typename T,
typename N,
bool S,
typename P>
818 template <
typename T,
typename N,
bool S,
typename P>
827 DEFINE_CLASS_TEMPLATE_VERSION(
831 #endif //_CLASS_BEZIERCURVE Time time_t
Definition: bezier_curve.h:35
Definition: bernstein.h:20
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:308
bezier_curve_t elevate(const std::size_t order) const
Computes a Bezier curve of order degrees higher than the current curve, but strictly equivalent...
Definition: bezier_curve.h:272
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:206
curve_abc_t::curve_ptr_t curve_ptr_t
Definition: bezier_curve.h:45
boost::shared_ptr< curve_t > curve_ptr_t
Definition: piecewise_curve.h:49
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:166
bezier_curve_t compute_primitive(const std::size_t order, const point_t &init) 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:238
Eigen::Vector3d cross(const Eigen::VectorXd &a, const Eigen::VectorXd &b)
Definition: cross_implementation.h:14
virtual bool operator!=(const bezier_curve_t &other) const
Definition: bezier_curve.h:198
bezier_curve()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: bezier_curve.h:52
bezier_curve_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: bezier_curve.h:228
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:37
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:39
virtual time_t max() const =0
Get the maximum time for which the curve is defined.
class allowing to create a cubic hermite spline of any dimension.
interface for a Curve of arbitrary dimension.
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:102
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: bezier_curve.h:33
virtual point_t operator()(const time_t t) const
Evaluation of the bezier curve at time t.
Definition: bezier_curve.h:144
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:64
bezier_curve< T, N, S, P > operator/(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:805
brief Computes all Bernstein polynomes for a certain degree std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Definition: bernstein.h:91
Numeric num_t
Definition: bezier_curve.h:36
bezier_curve< T, N, S, P > operator+(const bezier_curve< T, N, S, P > &p1, const bezier_curve< T, N, S, P > &p2)
Definition: bezier_curve.h:748
boost::shared_ptr< curve_t > curve_ptr_t
Definition: curve_abc.h:47
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:321
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:38
boost::shared_ptr< bezier_curve_t > bezier_curve_ptr_t
Definition: bezier_curve.h:41
bezier_curve< T, N, S, P > operator*(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:812
void serialize(Archive &ar, const unsigned int version)
Definition: curve_abc.h:159
bezier_curve_t compute_primitive(const std::size_t order) const
Definition: bezier_curve.h:258
virtual std::size_t degree() const =0
Get the degree of the curve.
virtual bool operator==(const bezier_curve_t &other) const
Definition: bezier_curve.h:194
double Time
Definition: effector_spline.h:27
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:230
Eigen::Ref< const vector_x_t > vector_x_ref_t
Definition: bezier_curve.h:34
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: bezier_curve.h:183
bezier_curve< Time, Numeric, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:40
void elevate_self(const std::size_t order)
Elevate the Bezier curve of order degrees higher than the current curve, but strictly equivalent...
Definition: bezier_curve.h:296
virtual time_t min() const =0
Get the minimum time for which the curve is defined.
virtual std::size_t dim() const =0
Get dimension of curve.
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Definition: bezier_curve.h:31
Point point_t
Definition: bezier_curve.h:32
struct to define constraints on start / end velocities and acceleration on a curve ...
piecewise_curve< Time, Numeric, Safe, point_t, point_t, bezier_curve_t > piecewise_curve_t
Definition: bezier_curve.h:43
double Numeric
Definition: effector_spline.h:26
class allowing to create a piecewise curve.
bezier_curve(const bezier_curve &other)
Definition: bezier_curve.h:127
Definition: curve_constraint.h:20
bezier_curve< T, N, S, P > operator-(const bezier_curve< T, N, S, P > &p1)
Definition: bezier_curve.h:755
virtual ~bezier_curve()
Destructor.
Definition: bezier_curve.h:138
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:37
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: bezier_curve.h:44
bezier_curve_t * compute_primitive_ptr(const std::size_t order, const point_t &init) const
Definition: bezier_curve.h:262