Go to the documentation of this file.
9 #ifndef _CLASS_BEZIERCURVE
10 #define _CLASS_BEZIERCURVE
12 #include "curve_abc.h"
14 #include "curve_constraint.h"
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> {
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 || minBound >= maxBound))
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);
65 template <
typename In>
70 size_(std::distance(PointsBegin, PointsEnd) + 4),
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);
96 if (Safe & !(0 <= nT && nT <= 1)) {
97 throw std::out_of_range(
"can't evaluate bezier curve, out of range");
104 return pts_[0] * dt + nT * pts_[1];
107 return pts_[0] * dt * dt + 2 * pts_[1] * nT * dt + pts_[2] * nT * nT;
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;
123 if (order == 0)
return *
this;
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());
136 if (order == 0)
return *
this;
139 point_t current_sum = point_t::Zero();
142 n_wp.push_back(current_sum);
143 for (
typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end(); ++pit) {
145 n_wp.push_back(current_sum / new_degree);
167 typename t_point_t::const_iterator pts_it = pts_.begin();
170 res += cit->operator()(u) * (*pts_it);
178 typename t_point_t::const_iterator pts_it = pts_.begin();
185 for (
int i = 1; i <
degree_; i++, ++pts_it) {
187 bc = bc * (
degree_ - i + 1) / i;
188 tmp = (tmp + tn * bc * (*pts_it)) * u;
190 return (tmp + tn * t * (*pts_it));
196 template <
typename In>
199 point_t P0, P1, P2, P_n_2, P_n_1, PN;
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;
211 for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) res.push_back(*it);
213 res.push_back(P_n_2);
214 res.push_back(P_n_1);
239 #endif //_CLASS_BEZIERCURVE
Numeric num_t
Definition: bezier_curve.h:34
virtual time_t max() const
Definition: bezier_curve.h:224
Time time_t
Definition: bezier_curve.h:33
bezier_curve< Time, Numeric, Dim, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:38
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
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:36
const time_t minBound_
Definition: bezier_curve.h:228
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
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
Point point_t
Definition: bezier_curve.h:32
bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound=0, const time_t maxBound=1)
Constructor.
Definition: bezier_curve.h:46
point_t evalHorner(const Numeric t) const
Evaluates all Bernstein polynomes for a certain degree using horner's scheme.
Definition: bezier_curve.h:177
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:37
std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Computes all Bernstein polynomes for a certain degree.
Definition: bernstein.h:59
~bezier_curve()
Destructor.
Definition: bezier_curve.h:80
const std::size_t degree_
Definition: bezier_curve.h:230
const std::vector< Bern< Numeric > > bernstein_
Definition: bezier_curve.h:231
const time_t maxBound_
Definition: bezier_curve.h:228
double Numeric
Definition: effector_spline.h:26
double Time
Definition: effector_spline.h:27
point_t evalBernstein(const Numeric u) const
Evaluates all Bernstein polynomes for a certain degree.
Definition: bezier_curve.h:165
Definition: bernstein.h:40
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
Definition: bezier_curve.h:31
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:35
virtual time_t min() const
Definition: bezier_curve.h:223
const std::size_t size_
Definition: bezier_curve.h:229
Definition: bernstein.h:20
const t_point_t & waypoints() const
Definition: bezier_curve.h:193
bezier_curve_t compute_derivate(const std::size_t order) const
Computes the derivative curve at order N.
Definition: bezier_curve.h:122
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t.
Definition: bezier_curve.h:94