GCC Code Coverage Report

Directory: ./
File: include/ndcurves/bezier_curve.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 388 401 96.8%
Branches: 380 681 55.8%
Line Branch Exec Source
1 /**
2 * \file bezier_curve.h
3 * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
4 * \author Steve T.
5 * \version 0.1
6 * \date 06/17/2013
7 */
8
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 "cross_implementation.h"
19 #include "curve_abc.h"
20 #include "curve_constraint.h"
21 #include "piecewise_curve.h"
22
23 namespace ndcurves {
24 /// \class BezierCurve.
25 /// \brief Represents a Bezier curve of arbitrary dimension and order.
26 /// For degree lesser than 4, the evaluation is analitycal. Otherwise
27 /// the bernstein polynoms are used to evaluate the spline at a given location.
28 ///
29 template <typename Time = double, typename Numeric = Time, bool Safe = false,
30 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
31 struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
32 typedef Point point_t;
33 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
34 typedef Eigen::Ref<const vector_x_t> vector_x_ref_t;
35 typedef Time time_t;
36 typedef Numeric num_t;
37 typedef curve_constraints<point_t> curve_constraints_t;
38 typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
39 typedef typename t_point_t::const_iterator cit_point_t;
40 typedef bezier_curve<Time, Numeric, Safe, Point> bezier_curve_t;
41 typedef std::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
42 typedef piecewise_curve<Time, Numeric, Safe, point_t, point_t, bezier_curve_t>
43 piecewise_curve_t;
44 typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
45 typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
46
47 /* Constructors - destructors */
48 public:
49 /// \brief Empty constructor. Curve obtained this way can not perform other
50 /// class functions.
51 ///
52
1/2
✓ Branch 3 taken 121 times.
✗ Branch 4 not taken.
 242 bezier_curve() : dim_(0), T_min_(0), T_max_(0) {}
53
54 /// \brief Constructor.
55 /// Given the first and last point of a control points set, create the bezier
56 /// curve. \param PointsBegin : an iterator pointing to the first element of
57 /// a control point container. \param PointsEnd : an iterator pointing to
58 /// the last element of a control point container. \param T_min :
59 /// lower bound of time, curve will be defined for time in [T_min, T_max].
60 /// \param T_max : upper bound of time, curve will be defined for time
61 /// in [T_min, T_max]. \param mult_T : ... (default value is 1.0).
62 ///
63 template <typename In>
64 12084 bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min = 0.,
65 const time_t T_max = 1., const time_t mult_T = 1.)
66 12084 : dim_(PointsBegin->size()),
67 12084 T_min_(T_min),
68 12084 T_max_(T_max),
69 12084 mult_T_(mult_T),
70
1/2
✓ Branch 1 taken 6042 times.
✗ Branch 2 not taken.
 12084 size_(std::distance(PointsBegin, PointsEnd)),
71 12084 degree_(size_ - 1),
72
2/4
✓ Branch 3 taken 6042 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6042 times.
✗ Branch 7 not taken.
 24168 bernstein_(ndcurves::makeBernstein<num_t>((unsigned int)degree_)) {
73
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6042 times.
 12084 if (bernstein_.size() != size_) {
74 ✗ throw std::invalid_argument("Invalid size of polynomial");
75 }
76 12084 In it(PointsBegin);
77
2/4
✓ Branch 0 taken 6042 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 6042 times.
 12084 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");
80 }
81
3/3
✓ Branch 1 taken 113 times.
✓ Branch 2 taken 22083 times.
✓ Branch 3 taken 6019 times.
 56430 for (; it != PointsEnd; ++it) {
82
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 22173 times.
 44346 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
2/4
✓ Branch 2 taken 22173 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 113 times.
✗ Branch 6 not taken.
 44346 control_points_.push_back(*it);
86 }
87 12084 }
88
89 /// \brief Constructor with constraints.
90 /// This constructor will add 4 points (2 after the first one, 2 before the
91 /// last one) to ensure that velocity and acceleration constraints are
92 /// respected. \param PointsBegin : an iterator pointing to the first
93 /// element of a control point container. \param PointsEnd : an iterator
94 /// pointing to the last element of a control point container. \param
95 /// constraints : constraints applying on start / end velocities and
96 /// acceleration. \param T_min : lower bound of time, curve will be
97 /// defined for time in [T_min, T_max]. \param T_max : upper bound of
98 /// time, curve will be defined for time in [T_min, T_max]. \param mult_T :
99 /// ... (default value is 1.0).
100 ///
101 template <typename In>
102 5 bezier_curve(In PointsBegin, In PointsEnd,
103 const curve_constraints_t& constraints, const time_t T_min = 0.,
104 const time_t T_max = 1., const time_t mult_T = 1.)
105 5 : dim_(PointsBegin->size()),
106 5 T_min_(T_min),
107 5 T_max_(T_max),
108 5 mult_T_(mult_T),
109
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 size_(std::distance(PointsBegin, PointsEnd) + 4),
110 5 degree_(size_ - 1),
111
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 10 bernstein_(ndcurves::makeBernstein<num_t>((unsigned int)degree_)) {
112
2/4
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 5 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");
115 }
116
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 t_point_t updatedList =
117 add_constraints<In>(PointsBegin, PointsEnd, constraints);
118
2/2
✓ Branch 4 taken 19 times.
✓ Branch 5 taken 3 times.
 36 for (cit_point_t cit = updatedList.begin(); cit != updatedList.end();
119 31 ++cit) {
120
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 19 times.
 31 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
1/2
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
 31 control_points_.push_back(*cit);
124 }
125 5 }
126
127 4198 bezier_curve(const bezier_curve& other)
128 4198 : dim_(other.dim_),
129 4198 T_min_(other.T_min_),
130 4198 T_max_(other.T_max_),
131 4198 mult_T_(other.mult_T_),
132 4198 size_(other.size_),
133 4198 degree_(other.degree_),
134 4198 bernstein_(other.bernstein_),
135
2/4
✓ Branch 2 taken 2099 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2099 times.
✗ Branch 6 not taken.
 4198 control_points_(other.control_points_) {}
136
137 ///\brief Destructor
138 16824 virtual ~bezier_curve() {}
139
140 /*Operations*/
141 /// \brief Evaluation of the bezier curve at time t.
142 /// \param t : time when to evaluate the curve.
143 /// \return \f$x(t)\f$ point corresponding on curve at time t.
144 840212 virtual point_t operator()(const time_t t) const {
145 840212 check_conditions();
146
6/6
✓ Branch 0 taken 420104 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 420102 times.
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 420102 times.
 840212 if (Safe & !(T_min_ <= t && t <= T_max_)) {
147
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 throw std::invalid_argument(
148 "can't evaluate bezier curve, time t is out of range"); // TODO
149 }
150
2/2
✓ Branch 0 taken 400 times.
✓ Branch 1 taken 419702 times.
 840204 if (size_ == 1) {
151
1/2
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
 800 return mult_T_ * control_points_[0];
152 } else {
153 839404 return evalHorner(t);
154 }
155 }
156
157 /**
158 * @brief isApprox check if other and *this are approximately equals.
159 * Only two curves of the same class can be approximately equals, for
160 * comparison between different type of curves see isEquivalent
161 * @param other the other curve to check
162 * @param prec the precision threshold, default
163 * Eigen::NumTraits<Numeric>::dummy_precision()
164 * @return true if the two curves are approximately equals
165 */
166 50 bool isApprox(
167 const bezier_curve_t& other,
168 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
169
2/2
✓ Branch 2 taken 24 times.
✓ Branch 3 taken 1 times.
 100 bool equal = ndcurves::isApprox<num_t>(T_min_, other.min()) &&
170 50 ndcurves::isApprox<num_t>(T_max_, other.max()) &&
171
3/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✓ Branch 5 taken 1 times.
 48 dim_ == other.dim() && degree_ == other.degree() &&
172
2/4
✓ Branch 0 taken 23 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 92 size_ == other.size_ &&
173
1/2
✓ Branch 0 taken 25 times.
✗ Branch 1 not taken.
 146 ndcurves::isApprox<Numeric>(mult_T_, other.mult_T_) &&
174
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 46 bernstein_ == other.bernstein_;
175
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 23 times.
 50 if (!equal) return false;
176
2/2
✓ Branch 0 taken 99 times.
✓ Branch 1 taken 20 times.
 238 for (size_t i = 0; i < size_; ++i) {
177
2/2
✓ Branch 3 taken 3 times.
✓ Branch 4 taken 96 times.
 198 if (!control_points_.at(i).isApprox(other.control_points_.at(i), prec))
178 6 return false;
179 }
180 40 return true;
181 }
182
183 40 virtual bool isApprox(
184 const curve_abc_t* other,
185 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
186 40 const bezier_curve_t* other_cast =
187
1/2
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
 40 dynamic_cast<const bezier_curve_t*>(other);
188
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 5 times.
 40 if (other_cast)
189 30 return isApprox(*other_cast, prec);
190 else
191 10 return false;
192 }
193
194 20 virtual bool operator==(const bezier_curve_t& other) const {
195 20 return isApprox(other);
196 }
197
198 6 virtual bool operator!=(const bezier_curve_t& other) const {
199 6 return !(*this == other);
200 }
201
202 /// \brief Compute the derived curve at order N.
203 /// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier
204 /// curve of parametric equation x(t). \param order : order of derivative.
205 /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
206 10654 bezier_curve_t compute_derivate(const std::size_t order) const {
207
1/2
✓ Branch 1 taken 5327 times.
✗ Branch 2 not taken.
 10654 check_conditions();
208
2/2
✓ Branch 0 taken 1897 times.
✓ Branch 1 taken 3430 times.
 10654 if (order == 0) {
209
1/2
✓ Branch 1 taken 1897 times.
✗ Branch 2 not taken.
 3794 return *this;
210 }
211
1/2
✓ Branch 1 taken 3430 times.
✗ Branch 2 not taken.
 6860 t_point_t derived_wp;
212 6860 for (typename t_point_t::const_iterator pit = control_points_.begin();
213
2/2
✓ Branch 3 taken 10848 times.
✓ Branch 4 taken 3430 times.
 28556 pit != control_points_.end() - 1; ++pit) {
214
4/8
✓ Branch 4 taken 10848 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10848 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10848 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10832 times.
✗ Branch 14 not taken.
 21696 derived_wp.push_back((num_t)degree_ * (*(pit + 1) - (*pit)));
215 }
216
2/2
✓ Branch 1 taken 501 times.
✓ Branch 2 taken 2929 times.
 6860 if (derived_wp.empty()) {
217
3/6
✓ Branch 1 taken 501 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 501 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 501 times.
✗ Branch 8 not taken.
 1002 derived_wp.push_back(point_t::Zero(dim_));
218 }
219
1/2
✓ Branch 2 taken 3430 times.
✗ Branch 3 not taken.
 6860 bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), T_min_, T_max_,
220 6860 mult_T_ * (1. / (T_max_ - T_min_)));
221
1/2
✓ Branch 1 taken 3430 times.
✗ Branch 2 not taken.
 6860 return deriv.compute_derivate(order - 1);
222 6860 }
223
224 /// \brief Compute the derived curve at order N.
225 /// \param order : order of derivative.
226 /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the
227 /// curve.
228 4 bezier_curve_t* compute_derivate_ptr(const std::size_t order) const {
229
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 return new bezier_curve_t(compute_derivate(order));
230 }
231
232 /// \brief Compute the primitive of the curve at order N.
233 /// Computes the primitive at order N of bezier curve of parametric equation
234 /// \f$x(t)\f$. <br> At order \f$N=1\f$, the primitve \f$X(t)\f$ of
235 /// \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$. \param order : order
236 /// of the primitive. \param init : constant valuefor the first point of the
237 /// primitive (can tipycally be zero) \return primitive at order N of x(t).
238 38 bezier_curve_t compute_primitive(const std::size_t order,
239 const point_t& init) const {
240
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 38 check_conditions();
241
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 11 times.
 38 if (order == 0) {
242
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 17 return *this;
243 }
244 21 num_t new_degree_inv = 1. / ((num_t)(degree_ + 1));
245
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 21 t_point_t n_wp;
246
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 21 point_t current_sum(init);
247
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 21 n_wp.push_back(current_sum);
248 21 for (typename t_point_t::const_iterator pit = control_points_.begin();
249
2/2
✓ Branch 2 taken 35 times.
✓ Branch 3 taken 11 times.
 84 pit != control_points_.end(); ++pit) {
250
1/2
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
 63 current_sum += *pit;
251
3/6
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28 times.
✗ Branch 8 not taken.
 63 n_wp.push_back(current_sum * new_degree_inv);
252 }
253
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 21 bezier_curve_t integ(n_wp.begin(), n_wp.end(), T_min_, T_max_,
254 21 mult_T_ * (T_max_ - T_min_));
255
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 21 return integ.compute_primitive(order - 1);
256 21 }
257
258 37 bezier_curve_t compute_primitive(const std::size_t order) const {
259
2/4
✓ Branch 2 taken 19 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
 37 return compute_primitive(order, point_t::Zero(dim_));
260 }
261
262 bezier_curve_t* compute_primitive_ptr(const std::size_t order,
263 const point_t& init) const {
264 return new bezier_curve_t(compute_primitive(order, init));
265 }
266
267 /// \brief Computes a Bezier curve of order degrees higher than the current
268 /// curve, but strictly equivalent. Order elevation is required for addition
269 /// / substraction and other comparison operations. \param order : number of
270 /// order the curve must be updated \return An equivalent Bezier, with one
271 /// more degree.
272 30 bezier_curve_t elevate(const std::size_t order) const {
273
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
 30 t_point_t new_waypoints = control_points_, temp_waypoints;
274
2/2
✓ Branch 0 taken 44 times.
✓ Branch 1 taken 21 times.
 84 for (std::size_t i = 1; i <= order; ++i) {
275 54 num_t new_degree_inv = 1. / ((num_t)(degree_ + i));
276
1/2
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
 54 temp_waypoints.push_back(*new_waypoints.begin());
277 54 num_t idx_deg_inv = 0.;
278 54 for (typename t_point_t::const_iterator pit = new_waypoints.begin() + 1;
279
2/2
✓ Branch 2 taken 120 times.
✓ Branch 3 taken 44 times.
 185 pit != new_waypoints.end(); ++pit) {
280 131 idx_deg_inv += new_degree_inv;
281
4/10
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 120 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 120 times.
✗ Branch 13 not taken.
 131 temp_waypoints.push_back(idx_deg_inv * (*(pit - 1)) +
282
1/2
✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
 262 (1 - idx_deg_inv) * (*pit));
283 }
284
1/2
✓ Branch 4 taken 44 times.
✗ Branch 5 not taken.
 54 temp_waypoints.push_back(*(new_waypoints.end() - 1));
285
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 54 new_waypoints = temp_waypoints;
286 54 temp_waypoints.clear();
287 }
288 30 return bezier_curve_t(new_waypoints.begin(), new_waypoints.end(), T_min_,
289
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 60 T_max_, mult_T_);
290 30 }
291
292 /// \brief Elevate the Bezier curve of order degrees higher than the current
293 /// curve, but strictly equivalent. Order elevation is required for addition
294 /// / substraction and other comparison operations. \param order : number of
295 /// order the curve must be updated
296 24 void elevate_self(const std::size_t order) {
297
2/4
✓ Branch 0 taken 17 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 24 if (order > 0) (*this) = elevate(order);
298 24 }
299
300 /// \brief Evaluate the derivative order N of curve at time t.
301 /// If derivative is to be evaluated several times, it is
302 /// rather recommended to compute derived curve using compute_derivate.
303 /// \param order : order of derivative.
304 /// \param t : time when to evaluate the curve.
305 /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve
306 /// of order N at time t.
307 ///
308 3764 virtual point_t derivate(const time_t t, const std::size_t order) const {
309
1/2
✓ Branch 2 taken 1882 times.
✗ Branch 3 not taken.
 3764 return compute_derivate(order)(t);
310 }
311
312 /// \brief Evaluate all Bernstein polynomes for a certain degree.
313 /// A bezier curve with N control points is represented by : \f$x(t) =
314 /// \sum_{i=0}^{N} B_i^N(t) P_i\f$ with \f$ B_i^N(t) = \binom{N}{i}t^i
315 /// (1-t)^{N-i} \f$.<br/> Warning: the horner scheme is about 100 times faster
316 /// than this method.<br> This method will probably be removed in the future
317 /// as the computation of bernstein polynomial is very costly. \param t : time
318 /// when to evaluate the curve. \return \f$x(t)\f$ point corresponding on
319 /// curve at time t.
320 ///
321 200020 point_t evalBernstein(const Numeric t) const {
322 200020 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
323
2/4
✓ Branch 1 taken 200020 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200020 times.
✗ Branch 5 not taken.
 200020 point_t res = point_t::Zero(dim_);
324 typename t_point_t::const_iterator control_points_it =
325 200020 control_points_.begin();
326 200020 for (typename std::vector<Bern<Numeric> >::const_iterator cit =
327 200020 bernstein_.begin();
328
2/2
✓ Branch 3 taken 1200070 times.
✓ Branch 4 taken 200020 times.
 1400090 cit != bernstein_.end(); ++cit, ++control_points_it) {
329
3/6
✓ Branch 3 taken 1200070 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1200070 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1200070 times.
✗ Branch 10 not taken.
 1200070 res += cit->operator()(u) * (*control_points_it);
330 }
331
2/4
✓ Branch 1 taken 200020 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200020 times.
✗ Branch 5 not taken.
 400040 return res * mult_T_;
332 200020 }
333
334 /// \brief Evaluate all Bernstein polynomes for a certain degree using
335 /// Horner's scheme. A bezier curve with N control points is expressed as :
336 /// \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.<br> To evaluate the position on
337 /// curve at time t,we can apply the Horner's scheme : <br> \f$ x(t) =
338 /// (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.<br> Horner's
339 /// scheme : for a polynom of degree N expressed by : <br> \f$x(t) = a_0 +
340 /// a_1t + a_2t^2 + ... + a_nt^n\f$ where \f$number of additions = N\f$ /
341 /// f$number of multiplication = N!\f$<br> Using Horner's method, the polynom
342 /// is transformed into : <br> \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N
343 /// additions and multiplications. \param t : time when to evaluate the curve.
344 /// \return \f$x(t)\f$ point corresponding on curve at time t.
345 ///
346 1239444 point_t evalHorner(const Numeric t) const {
347 1239444 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
348 typename t_point_t::const_iterator control_points_it =
349 1239444 control_points_.begin();
350 Numeric u_op, bc, tn;
351 1239444 u_op = 1.0 - u;
352 1239444 bc = 1;
353 1239444 tn = 1;
354
2/4
✓ Branch 2 taken 619722 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 619654 times.
✗ Branch 6 not taken.
 1239444 point_t tmp = (*control_points_it) * u_op;
355 1239444 ++control_points_it;
356
2/2
✓ Branch 0 taken 2452898 times.
✓ Branch 1 taken 619722 times.
 6145240 for (unsigned int i = 1; i < degree_; i++, ++control_points_it) {
357 4905796 tn = tn * u;
358 4905796 bc = bc * ((num_t)(degree_ - i + 1)) / i;
359
4/8
✓ Branch 2 taken 2452898 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2452898 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2452898 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2452898 times.
✗ Branch 12 not taken.
 4905796 tmp = (tmp + tn * bc * (*control_points_it)) * u_op;
360 }
361
4/8
✓ Branch 2 taken 619722 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 619722 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 619722 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 619654 times.
✗ Branch 12 not taken.
 2465656 return (tmp + tn * u * (*control_points_it)) * mult_T_;
362 }
363
364 800500 const t_point_t& waypoints() const { return control_points_; }
365
366 296 const point_t waypointAtIndex(const std::size_t index) const {
367 296 point_t waypoint;
368
1/2
✓ Branch 1 taken 148 times.
✗ Branch 2 not taken.
 296 if (index < control_points_.size()) {
369
1/2
✓ Branch 2 taken 134 times.
✗ Branch 3 not taken.
 296 waypoint = control_points_[index];
370 }
371 296 return waypoint;
372 }
373
374 /// \brief Evaluate the curve value at time t using deCasteljau algorithm.
375 /// The algorithm will compute the \f$N-1\f$ centroids of parameters
376 /// \f${t,1-t}\f$ of consecutive \f$N\f$ control points of bezier curve, and
377 /// perform it iteratively until getting one point in the list which will be
378 /// the evaluation of bezier curve at time \f$t\f$. \param t : time when to
379 /// evaluate the curve. \return \f$x(t)\f$ point corresponding on curve at
380 /// time t.
381 ///
382 400000 point_t evalDeCasteljau(const Numeric t) const {
383 // normalize time :
384 400000 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
385
1/2
✓ Branch 2 taken 400000 times.
✗ Branch 3 not taken.
 400000 t_point_t pts = deCasteljauReduction(waypoints(), u);
386
2/2
✓ Branch 1 taken 1600000 times.
✓ Branch 2 taken 400000 times.
 2000000 while (pts.size() > 1) {
387
1/2
✓ Branch 1 taken 1600000 times.
✗ Branch 2 not taken.
 1600000 pts = deCasteljauReduction(pts, u);
388 }
389
2/4
✓ Branch 2 taken 400000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 400000 times.
✗ Branch 6 not taken.
 800000 return pts[0] * mult_T_;
390 400000 }
391
392 t_point_t deCasteljauReduction(const Numeric t) const {
393 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
394 return deCasteljauReduction(waypoints(), u);
395 }
396
397 /// \brief Compute de Casteljau's reduction of the given list of points at
398 /// time t. For the list \f$pts\f$ of N points, compute a new list of points
399 /// of size N-1 :<br> \f$<br>( pts[0]*(1-t)+pts[1], pts[1]*(1-t)+pts[2], ...,
400 /// pts[0]*(N-2)+pts[N-1] )\f$<br> with t the time when to evaluate bezier
401 /// curve.<br>\ The new list contains centroid of parameters \f${t,1-t}\f$ of
402 /// consecutive points in the list. \param pts : list of points. \param u :
403 /// NORMALIZED time when to evaluate the curve. \return reduced list of point
404 /// (size of pts - 1).
405 ///
406 4000072 t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const {
407
2/4
✓ Branch 0 taken 2000036 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2000036 times.
 4000072 if (u < 0 || u > 1) {
408 ✗ throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
409 }
410
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2000036 times.
 4000072 if (pts.size() == 1) {
411 ✗ return pts;
412 }
413
414
1/2
✓ Branch 1 taken 2000036 times.
✗ Branch 2 not taken.
 4000072 t_point_t new_pts;
415
2/2
✓ Branch 4 taken 7800095 times.
✓ Branch 5 taken 2000036 times.
 19600262 for (cit_point_t cit = pts.begin(); cit != (pts.end() - 1); ++cit) {
416
5/10
✓ Branch 3 taken 7800095 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 7800095 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7800095 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7800095 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7800083 times.
✗ Branch 17 not taken.
 15600190 new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1)));
417 }
418 4000072 return new_pts;
419 4000072 }
420
421 /// \brief Split the bezier curve in 2 at time t.
422 /// \param t : list of points.
423 /// \return pair containing the first element of both bezier curve obtained.
424 ///
425 20 std::pair<bezier_curve_t, bezier_curve_t> split(const Numeric t) const {
426
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 check_conditions();
427
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
 20 if (fabs(t - T_max_) < MARGIN) {
428 ✗ throw std::runtime_error(
429 "can't split curve, interval range is equal to original curve");
430 }
431
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
 40 t_point_t wps_first(size_), wps_second(size_);
432 20 const Numeric u = (t - T_min_) / (T_max_ - T_min_);
433
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 20 t_point_t casteljau_pts = waypoints();
434
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 20 wps_first[0] = casteljau_pts.front();
435
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 20 wps_second[degree_] = casteljau_pts.back();
436 20 size_t id = 1;
437
2/2
✓ Branch 1 taken 36 times.
✓ Branch 2 taken 10 times.
 92 while (casteljau_pts.size() > 1) {
438
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 casteljau_pts = deCasteljauReduction(casteljau_pts, u);
439
1/2
✓ Branch 3 taken 36 times.
✗ Branch 4 not taken.
 72 wps_first[id] = casteljau_pts.front();
440
1/2
✓ Branch 3 taken 36 times.
✗ Branch 4 not taken.
 72 wps_second[degree_ - id] = casteljau_pts.back();
441 72 ++id;
442 }
443
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 20 bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T_min_, t,
444 20 mult_T_);
445
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 20 bezier_curve_t c_second(wps_second.begin(), wps_second.end(), t, T_max_,
446 20 mult_T_);
447
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 40 return std::make_pair(c_first, c_second);
448 20 }
449
450 /// \brief Split the bezier curve in several curves, all accessible
451 /// within a piecewise_curve_t.
452 /// \param times : list of times of size n.
453 /// \return a piecewise_curve_t comprising n+1 curves
454 ///
455 6 piecewise_curve_t split(const vector_x_t& times) const {
456 6 std::vector<bezier_curve_t> curves;
457
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 bezier_curve_t current = *this;
458
2/2
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 3 times.
 16 for (int i = 0; i < times.rows(); ++i) {
459 10 std::pair<bezier_curve_t, bezier_curve_t> pairsplit =
460
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 10 current.split(times[i]);
461
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 curves.push_back(pairsplit.first);
462
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 current = pairsplit.second;
463 }
464
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 curves.push_back(current);
465
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 6 piecewise_curve_t res;
466 22 for (typename std::vector<bezier_curve_t>::const_iterator cit =
467 12 curves.begin();
468
2/2
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 3 times.
 22 cit != curves.end(); ++cit) {
469
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
 16 typename piecewise_curve_t::curve_ptr_t ptr(new bezier_curve_t(*cit));
470
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 16 res.add_curve_ptr(ptr);
471 }
472 12 return res;
473 6 }
474
475 /// \brief Extract a bezier curve defined between \f$[t_1,t_2]\f$ from the
476 /// actual bezier curve
477 /// defined between \f$[T_{min},T_{max}]\f$ with \f$T_{min} \leq t_1
478 /// \leq t_2 \leq T_{max}\f$.
479 /// \param t1 : start time of bezier curve extracted.
480 /// \param t2 : end time of bezier curve extracted.
481 /// \return bezier curve extract defined between \f$[t_1,t_2]\f$.
482 ///
483 4 bezier_curve_t extract(const Numeric t1, const Numeric t2) {
484
4/8
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 4 times.
 4 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");
486 }
487
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
 4 if (fabs(t1 - T_min_) < MARGIN &&
488
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 fabs(t2 - T_max_) < MARGIN) // t1=T_min and t2=T_max
489 {
490 1 return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_,
491
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 T_max_, mult_T_);
492 }
493
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 2 times.
 3 if (fabs(t1 - T_min_) < MARGIN) // t1=T_min
494 {
495
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 return split(t2).first;
496 }
497
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 if (fabs(t2 - T_max_) < MARGIN) // t2=T_max
498 {
499
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 return split(t1).second;
500 }
501
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::pair<bezier_curve_t, bezier_curve_t> c_split = this->split(t1);
502
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 return c_split.second.split(t2).first;
503 1 }
504
505 /// \brief Compute the cross product of the current bezier curve by another
506 /// bezier curve.
507 /// The cross product p1Xp2 of 2 bezier curves p1 and p2 is defined such that
508 /// forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product.
509 /// This method of course only makes sense for dimension 3 curves.
510 /// It assumes that a method point_t cross(const point_t&, const point_t&) has
511 /// been defined
512 /// \param pOther other polynomial to compute the cross product with.
513 /// \return a new polynomial defining the cross product between this and
514 /// pother
515 10 bezier_curve_t cross(const bezier_curve_t& g) const {
516 // See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form
517 // and http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
518
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 assert_operator_compatible(g);
519
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
 10 if (dim() != 3)
520 ✗ throw std::invalid_argument(
521 "Can't perform cross product on Bezier curves with dimensions != 3 ");
522
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 int m = (int)(degree());
523
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 int n = (int)(g.degree());
524 unsigned int mj, n_ij, mn_i;
525
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 t_point_t new_waypoints;
526
3/3
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 29 times.
✓ Branch 2 taken 4 times.
 74 for (int i = 0; i <= m + n; ++i) {
527
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 bezier_curve_t::point_t current_point =
528
2/4
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 64 bezier_curve_t::point_t::Zero(dim());
529
2/2
✓ Branch 2 taken 70 times.
✓ Branch 3 taken 32 times.
 204 for (int j = std::max(0, i - n); j <= std::min(m, i); ++j) {
530
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 140 mj = bin(m, j);
531
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 140 n_ij = bin(n, i - j);
532
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 140 mn_i = bin(m + n, i);
533 140 num_t mul = num_t(mj * n_ij) / num_t(mn_i);
534
4/8
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 70 times.
✗ Branch 11 not taken.
 140 current_point +=
535
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 140 mul * ndcurves::cross(waypointAtIndex(j), g.waypointAtIndex(i - j));
536 }
537
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 64 new_waypoints.push_back(current_point);
538 }
539 10 return bezier_curve_t(new_waypoints.begin(), new_waypoints.end(), min(),
540
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
 20 max(), mult_T_ * g.mult_T_);
541 10 }
542
543 /// \brief Compute the cross product of the current bezier b by a point
544 /// point.
545 /// The cross product pXpoint of is defined such that
546 /// forall t, bXpoint(t) = b(t) X point, with X designing the cross product.
547 /// This method of course only makes sense for dimension 3 polynomials.
548 /// \param point point to compute the cross product with.
549 /// \return a new polynomial defining the cross product between this and
550 /// point
551 1 bezier_curve_t cross(const bezier_curve_t::point_t& point) const {
552 // See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form
553 // and http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
554
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (dim() != 3)
555 ✗ throw std::invalid_argument(
556 "Can't perform cross product on Bezier curves with dimensions != 3 ");
557
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_point_t new_waypoints;
558 1 for (typename t_point_t::const_iterator cit = waypoints().begin();
559
2/2
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 1 times.
 7 cit != waypoints().end(); ++cit) {
560
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 6 new_waypoints.push_back(ndcurves::cross(*cit, point));
561 }
562 1 return bezier_curve_t(new_waypoints.begin(), new_waypoints.end(), min(),
563
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 max(), mult_T_);
564 1 }
565
566 16 bezier_curve_t& operator+=(const bezier_curve_t& other) {
567
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 3 times.
 16 assert_operator_compatible(other);
568 13 bezier_curve_t other_elevated =
569 other *
570
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 13 (other.mult_T_ / this->mult_T_); // TODO remove mult_T_ from Bezier
571
4/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4 times.
✓ Branch 7 taken 5 times.
 13 if (other.degree() > degree()) {
572
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 6 elevate_self(other.degree() - degree());
573
3/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✓ Branch 5 taken 2 times.
 7 } else if (other_elevated.degree() < degree()) {
574
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 other_elevated.elevate_self(degree() - other_elevated.degree());
575 }
576 13 typename t_point_t::const_iterator otherit =
577 13 other_elevated.control_points_.begin();
578 13 for (typename t_point_t::iterator it = control_points_.begin();
579
2/2
✓ Branch 4 taken 43 times.
✓ Branch 5 taken 9 times.
 68 it != control_points_.end(); ++it, ++otherit) {
580
1/2
✓ Branch 3 taken 43 times.
✗ Branch 4 not taken.
 55 (*it) += (*otherit);
581 }
582 13 return *this;
583 13 }
584
585 22 bezier_curve_t& operator-=(const bezier_curve_t& other) {
586
2/2
✓ Branch 1 taken 13 times.
✓ Branch 2 taken 3 times.
 22 assert_operator_compatible(other);
587
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 19 bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_);
588
4/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4 times.
✓ Branch 7 taken 9 times.
 19 if (other.degree() > degree()) {
589
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 6 elevate_self(other.degree() - degree());
590
3/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✓ Branch 5 taken 4 times.
 13 } else if (other_elevated.degree() < degree()) {
591
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 7 other_elevated.elevate_self(degree() - other_elevated.degree());
592 }
593 19 typename t_point_t::const_iterator otherit =
594 19 other_elevated.control_points_.begin();
595 19 for (typename t_point_t::iterator it = control_points_.begin();
596
2/2
✓ Branch 4 taken 62 times.
✓ Branch 5 taken 13 times.
 99 it != control_points_.end(); ++it, ++otherit) {
597
1/2
✓ Branch 3 taken 62 times.
✗ Branch 4 not taken.
 80 (*it) -= (*otherit);
598 }
599 19 return *this;
600 19 }
601
602 11 bezier_curve_t& operator+=(const bezier_curve_t::point_t& point) {
603 11 for (typename t_point_t::iterator it = control_points_.begin();
604
2/2
✓ Branch 3 taken 28 times.
✓ Branch 4 taken 7 times.
 49 it != control_points_.end(); ++it) {
605
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 38 (*it) += point;
606 }
607 11 return *this;
608 }
609
610 9 bezier_curve_t& operator-=(const bezier_curve_t::point_t& point) {
611 9 for (typename t_point_t::iterator it = control_points_.begin();
612
2/2
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 5 times.
 35 it != control_points_.end(); ++it) {
613
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 26 (*it) -= point;
614 }
615 9 return *this;
616 }
617
618 6 bezier_curve_t& operator/=(const double d) {
619 6 for (typename t_point_t::iterator it = control_points_.begin();
620
2/2
✓ Branch 3 taken 20 times.
✓ Branch 4 taken 4 times.
 32 it != control_points_.end(); ++it) {
621
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 26 (*it) /= d;
622 }
623 6 return *this;
624 }
625
626 39 bezier_curve_t& operator*=(const double d) {
627 39 for (typename t_point_t::iterator it = control_points_.begin();
628
2/2
✓ Branch 3 taken 110 times.
✓ Branch 4 taken 27 times.
 183 it != control_points_.end(); ++it) {
629
1/2
✓ Branch 2 taken 110 times.
✗ Branch 3 not taken.
 144 (*it) *= d;
630 }
631 39 return *this;
632 }
633
634 private:
635 /// \brief Ensure constraints of bezier curve.
636 /// Add 4 points (2 after the first one, 2 before the last one) to Bezier
637 /// curve to ensure that velocity and acceleration constraints are respected.
638 ///
639 template <typename In>
640 5 t_point_t add_constraints(In PointsBegin, In PointsEnd,
641 const curve_constraints_t& constraints) {
642
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 t_point_t res;
643 5 num_t T = T_max_ - T_min_;
644 5 num_t T_square = T * T;
645
6/12
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
 5 point_t P0, P1, P2, P_n_2, P_n_1, PN;
646
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 5 P0 = *PointsBegin;
647
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 5 PN = *(PointsEnd - 1);
648
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 5 P1 = P0 + constraints.init_vel * T / (num_t)degree_;
649
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 5 P_n_1 = PN - constraints.end_vel * T / (num_t)degree_;
650
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
 5 P2 = constraints.init_acc * T_square / (num_t)(degree_ * (degree_ - 1)) +
651
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 2 * P1 - P0;
652
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
 5 P_n_2 = constraints.end_acc * T_square / (num_t)(degree_ * (degree_ - 1)) +
653
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 2 * P_n_1 - PN;
654
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 res.push_back(P0);
655
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 res.push_back(P1);
656
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 res.push_back(P2);
657
3/3
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 2 times.
 6 for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) {
658
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 res.push_back(*it);
659 }
660
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 res.push_back(P_n_2);
661
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 res.push_back(P_n_1);
662
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 res.push_back(PN);
663 10 return res;
664 3 }
665
666 850926 void check_conditions() const {
667
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 425463 times.
 850926 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
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 425463 times.
 850926 } 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 ?");
675 }
676 850926 }
677
678 66 void assert_operator_compatible(const bezier_curve_t& other) const {
679
4/4
✓ Branch 2 taken 29 times.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 27 times.
 124 if ((fabs(min() - other.min()) > MARGIN) ||
680
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 27 times.
 58 (fabs(max() - other.max()) > MARGIN)) {
681
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 throw std::invalid_argument(
682 "Can't perform base operation (+ - ) on two Bezier curves with "
683 "different time ranges");
684 }
685 54 }
686
687 /*Operations*/
688
689 public:
690 /*Helpers*/
691 /// \brief Get dimension of curve.
692 /// \return dimension of curve.
693 10854 std::size_t virtual dim() const { return dim_; };
694 /// \brief Get the minimum time for which the curve is defined
695 /// \return \f$t_{min}\f$, lower bound of time range.
696 678 virtual time_t min() const { return T_min_; }
697 /// \brief Get the maximum time for which the curve is defined.
698 /// \return \f$t_{max}\f$, upper bound of time range.
699 1848 virtual time_t max() const { return T_max_; }
700 /// \brief Get the degree of the curve.
701 /// \return \f$degree\f$, the degree of the curve.
702 470 virtual std::size_t degree() const { return degree_; }
703 /*Helpers*/
704
705 /* Attributes */
706 /// Dim of curve
707 std::size_t dim_;
708 /// Starting time of cubic hermite spline : T_min_ is equal to first time of
709 /// control points.
710 /*const*/ time_t T_min_;
711 /// Ending time of cubic hermite spline : T_max_ is equal to last time of
712 /// control points.
713 /*const*/ time_t T_max_;
714 /*const*/ time_t mult_T_;
715 /*const*/ std::size_t size_;
716 /*const*/ std::size_t degree_;
717 /*const*/ std::vector<Bern<Numeric> > bernstein_;
718 /*const*/ t_point_t control_points_;
719 /* Attributes */
720
721 static bezier_curve_t zero(const std::size_t dim, const time_t T = 1.) {
722 std::vector<point_t> ts;
723 ts.push_back(point_t::Zero(dim));
724 return bezier_curve_t(ts.begin(), ts.end(), 0., T);
725 }
726
727 // Serialization of the class
728 friend class boost::serialization::access;
729
730 template <class Archive>
731 100 void serialize(Archive& ar, const unsigned int version) {
732 if (version) {
733 // Do something depending on version ?
734 }
735
1/2
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
 100 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
736
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 ar& boost::serialization::make_nvp("dim", dim_);
737
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 ar& boost::serialization::make_nvp("T_min", T_min_);
738
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 ar& boost::serialization::make_nvp("T_max", T_max_);
739
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 ar& boost::serialization::make_nvp("mult_T", mult_T_);
740
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 ar& boost::serialization::make_nvp("size", size_);
741
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 ar& boost::serialization::make_nvp("degree", degree_);
742
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 ar& boost::serialization::make_nvp("bernstein", bernstein_);
743
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 ar& boost::serialization::make_nvp("control_points", control_points_);
744 }
745 }; // End struct bezier_curve
746
747 template <typename T, typename N, bool S, typename P>
748 11 bezier_curve<T, N, S, P> operator+(const bezier_curve<T, N, S, P>& p1,
749 const bezier_curve<T, N, S, P>& p2) {
750
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 11 bezier_curve<T, N, S, P> res(p1);
751
3/4
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3 times.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 19 return res += p2;
752 11 }
753
754 template <typename T, typename N, bool S, typename P>
755 6 bezier_curve<T, N, S, P> operator-(const bezier_curve<T, N, S, P>& p1) {
756 6 std::vector<typename bezier_curve<T, N, S, P>::point_t> ts;
757
3/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 19 times.
✓ Branch 4 taken 4 times.
 31 for (std::size_t i = 0; i <= p1.degree(); ++i) {
758
4/8
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19 times.
✗ Branch 11 not taken.
 25 ts.push_back(bezier_curve<T, N, S, P>::point_t::Zero(p1.dim()));
759 }
760
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 6 bezier_curve<T, N, S, P> res(ts.begin(), ts.end(), p1.min(), p1.max());
761
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 6 res -= p1;
762 12 return res;
763 6 }
764
765 template <typename T, typename N, bool S, typename P>
766 11 bezier_curve<T, N, S, P> operator-(const bezier_curve<T, N, S, P>& p1,
767 const bezier_curve<T, N, S, P>& p2) {
768
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 11 bezier_curve<T, N, S, P> res(p1);
769
3/4
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3 times.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 19 return res -= p2;
770 11 }
771
772 template <typename T, typename N, bool S, typename P>
773 5 bezier_curve<T, N, S, P> operator-(
774 const bezier_curve<T, N, S, P>& p1,
775 const typename bezier_curve<T, N, S, P>::point_t& point) {
776
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 bezier_curve<T, N, S, P> res(p1);
777
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 10 return res -= point;
778 5 }
779
780 template <typename T, typename N, bool S, typename P>
781 1 bezier_curve<T, N, S, P> operator-(
782 const typename bezier_curve<T, N, S, P>::point_t& point,
783 const bezier_curve<T, N, S, P>& p1) {
784
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_curve<T, N, S, P> res(-p1);
785
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 return res += point;
786 1 }
787
788 template <typename T, typename N, bool S, typename P>
789 5 bezier_curve<T, N, S, P> operator+(
790 const bezier_curve<T, N, S, P>& p1,
791 const typename bezier_curve<T, N, S, P>::point_t& point) {
792
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 bezier_curve<T, N, S, P> res(p1);
793
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 10 return res += point;
794 5 }
795
796 template <typename T, typename N, bool S, typename P>
797 1 bezier_curve<T, N, S, P> operator+(
798 const typename bezier_curve<T, N, S, P>::point_t& point,
799 const bezier_curve<T, N, S, P>& p1) {
800
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_curve<T, N, S, P> res(p1);
801
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 return res += point;
802 1 }
803
804 template <typename T, typename N, bool S, typename P>
805 5 bezier_curve<T, N, S, P> operator/(const bezier_curve<T, N, S, P>& p1,
806 const double k) {
807
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 5 bezier_curve<T, N, S, P> res(p1);
808
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 10 return res /= k;
809 5 }
810
811 template <typename T, typename N, bool S, typename P>
812 33 bezier_curve<T, N, S, P> operator*(const bezier_curve<T, N, S, P>& p1,
813 const double k) {
814
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 33 bezier_curve<T, N, S, P> res(p1);
815
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 66 return res *= k;
816 33 }
817
818 template <typename T, typename N, bool S, typename P>
819 1 bezier_curve<T, N, S, P> operator*(const double k,
820 const bezier_curve<T, N, S, P>& p1) {
821
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_curve<T, N, S, P> res(p1);
822
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 return res *= k;
823 1 }
824
825 } // namespace ndcurves
826
827 DEFINE_CLASS_TEMPLATE_VERSION(
828 SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
829 SINGLE_ARG(ndcurves::bezier_curve<Time, Numeric, Safe, Point>))
830
831 #endif //_CLASS_BEZIERCURVE
832