GCC Code Coverage Report

Directory: ./
File: include/ndcurves/piecewise_curve.h
Date: 2025-06-05 17:24:53
Exec Total Coverage
Lines: 263 291 90.4%
Functions: 111 275 40.4%
Branches: 251 448 56.0%
Line Branch Exec Source
1 /**
2 * \file piecewise_curve.h
3 * \brief class allowing to create a piecewise curve.
4 * \author Jason C.
5 * \date 05/2019
6 */
7
8 #ifndef _CLASS_PIECEWISE_CURVE
9 #define _CLASS_PIECEWISE_CURVE
10
11 #include <boost/serialization/vector.hpp>
12 #include <fstream>
13 #include <memory>
14 #include <sstream>
15
16 #include "curve_abc.h"
17 #include "curve_conversion.h"
18
19 namespace ndcurves {
20 /// \class PiecewiseCurve.
21 /// \brief Represent a piecewise curve. We can add some new curve,
22 /// but the starting time of the curve to add should be equal to the
23 /// ending time of the actual piecewise_curve.<br>\ Example : A piecewise
24 /// curve composed of three curves cf0, cf1 and cf2 where cf0 is defined
25 /// between \f$[T0_{min},T0_{max}]\f$, cf1 between
26 /// \f$[T0_{max},T1_{max}]\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
27 /// On the piecewise polynomial curve, cf0 is located between
28 /// \f$[T0_{min},T0_{max}[\f$, cf1 between \f$[T0_{max},T1_{max}[\f$ and
29 /// cf2 between \f$[T1_{max},T2_{max}]\f$.
30 ///
31 template <typename Time = double, typename Numeric = Time, bool Safe = false,
32 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
33 typename Point_derivate = Point,
34 typename CurveType =
35 curve_abc<Time, Numeric, Safe, Point, Point_derivate> >
36 struct piecewise_curve
37 : public curve_abc<Time, Numeric, Safe, Point, Point_derivate> {
38 typedef Point point_t;
39 typedef Point_derivate point_derivate_t;
40 typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
41 typedef std::vector<point_derivate_t,
42 Eigen::aligned_allocator<point_derivate_t> >
43 t_point_derivate_t;
44 typedef Time time_t;
45 typedef Numeric num_t;
46 typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t>
47 base_curve_t; // parent class
48 typedef CurveType curve_t; // contained curves base class
49 typedef std::shared_ptr<curve_t> curve_ptr_t;
50 typedef typename std::vector<curve_ptr_t> t_curve_ptr_t;
51 typedef typename std::vector<Time> t_time_t;
52 typedef piecewise_curve<Time, Numeric, Safe, Point, Point_derivate, CurveType>
53 piecewise_curve_t;
54 typedef piecewise_curve<Time, Numeric, Safe, Point_derivate, Point_derivate,
55 typename CurveType::curve_derivate_t>
56 piecewise_curve_derivate_t;
57 typedef std::shared_ptr<typename piecewise_curve_derivate_t::curve_t>
58 curve_derivate_ptr_t;
59
60 public:
61 /// \brief Empty constructor. Add at least one curve to call other class
62 /// functions.
63 ///
64 170 piecewise_curve() : dim_(0), size_(0), T_min_(0), T_max_(0) {}
65
66 /// \brief Constructor.
67 /// Initialize a piecewise curve by giving the first curve.
68 /// \param cf : a curve.
69 ///
70 32 piecewise_curve(const curve_ptr_t& cf)
71 32 : dim_(0), size_(0), T_min_(0), T_max_(0) {
72
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 add_curve_ptr(cf);
73 32 }
74
75 piecewise_curve(const t_curve_ptr_t& curves_list)
76 : dim_(0), size_(0), T_min_(0), T_max_(0) {
77 for (typename t_curve_ptr_t::const_iterator it = curves_list.begin();
78 it != curves_list.end(); ++it) {
79 add_curve_ptr(*it);
80 }
81 }
82
83 62 piecewise_curve(const piecewise_curve& other)
84 62 : dim_(other.dim_),
85 62 curves_(other.curves_),
86
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 62 time_curves_(other.time_curves_),
87 62 size_(other.size_),
88 62 T_min_(other.T_min_),
89
1/2
✓ Branch 2 taken 31 times.
✗ Branch 3 not taken.
 124 T_max_(other.T_max_) {}
90
91 328 virtual ~piecewise_curve() {}
92
93 26930 virtual point_t operator()(const Time t) const {
94 26930 check_if_not_empty();
95
5/6
✓ Branch 0 taken 12458 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 12457 times.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 12457 times.
 24916 if (Safe & !(T_min_ <= t && t <= T_max_)) {
96 // std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<"
97 // t="<<t<<std::endl;
98
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 throw std::out_of_range("can't evaluate piecewise curve, out of range");
99 }
100 26928 return (*curves_.at(find_interval(t)))(t);
101 }
102
103 /**
104 * @brief isApprox check if other and *this are approximately equals.
105 * Only two curves of the same class can be approximately equals, for
106 * comparison between different type of curves see isEquivalent
107 * @param other the other curve to check
108 * @param prec the precision threshold, default
109 * Eigen::NumTraits<Numeric>::dummy_precision()
110 * @return true is the two curves are approximately equals
111 */
112 26 bool isApprox(
113 const piecewise_curve_t& other,
114 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
115
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 12 times.
 26 if (num_curves() != other.num_curves()) return false;
116
2/2
✓ Branch 1 taken 21 times.
✓ Branch 2 taken 9 times.
 60 for (size_t i = 0; i < num_curves(); ++i) {
117
5/8
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 21 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 3 times.
✓ Branch 14 taken 18 times.
 42 if (!curve_at_index(i)->isApprox(other.curve_at_index(i).get(), prec))
118 6 return false;
119 }
120 18 return true;
121 }
122
123 ✗ virtual bool isApprox(
124 const base_curve_t* other,
125 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
126 ✗ const piecewise_curve_t* other_cast =
127 ✗ dynamic_cast<const piecewise_curve_t*>(other);
128 ✗ if (other_cast)
129 ✗ return isApprox(*other_cast, prec);
130 else
131 ✗ return false;
132 }
133
134 26 virtual bool operator==(const piecewise_curve_t& other) const {
135 26 return isApprox(other);
136 }
137
138 8 virtual bool operator!=(const piecewise_curve_t& other) const {
139 8 return !(*this == other);
140 }
141
142 /// \brief Evaluate the derivative of order N of curve at time t.
143 /// \param t : time when to evaluate the spline.
144 /// \param order : order of derivative.
145 /// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative
146 /// spline of order N at time t.
147 ///
148 1626 virtual point_derivate_t derivate(const Time t,
149 const std::size_t order) const {
150 1626 check_if_not_empty();
151
3/6
✓ Branch 0 taken 813 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 813 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 813 times.
 1626 if (Safe & !(T_min_ <= t && t <= T_max_)) {
152 ✗ throw std::invalid_argument(
153 "can't evaluate piecewise curve, out of range");
154 }
155 1626 return (*curves_.at(find_interval(t))).derivate(t, order);
156 }
157
158 /**
159 * @brief compute_derivate return a piecewise_curve which is the derivative of
160 * this at given order
161 * @param order order of derivative
162 * @return
163 */
164 4 piecewise_curve_derivate_t* compute_derivate_ptr(
165 const std::size_t order) const {
166
1/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 4 piecewise_curve_derivate_t* res(new piecewise_curve_derivate_t());
167 12 for (typename t_curve_ptr_t::const_iterator itc = curves_.begin();
168
2/2
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 2 times.
 12 itc < curves_.end(); ++itc) {
169
2/4
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 8 curve_derivate_ptr_t ptr((*itc)->compute_derivate_ptr(order));
170
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 res->add_curve_ptr(ptr);
171 }
172 4 return res;
173 }
174
175 template <typename Curve>
176 16022 void add_curve(const Curve& curve) {
177
1/2
✓ Branch 1 taken 8011 times.
✗ Branch 2 not taken.
 16022 curve_ptr_t curve_ptr = std::make_shared<Curve>(curve);
178
1/2
✓ Branch 1 taken 8011 times.
✗ Branch 2 not taken.
 16022 add_curve_ptr(curve_ptr);
179 16022 }
180
181 /// \brief Add a new curve to piecewise curve, which should be defined in
182 /// \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$
183 /// is equal to \f$T_{max}\f$ of the actual piecewise curve. The curve
184 /// added should be of type Curve as defined in the template.
185 /// \param cf : curve to add.
186 ///
187 16128 void add_curve_ptr(const curve_ptr_t& cf) {
188
2/2
✓ Branch 0 taken 78 times.
✓ Branch 1 taken 7986 times.
 16128 if (size_ == 0) { // first curve added
189 156 dim_ = cf->dim();
190 }
191 // Check time continuity : Beginning time of cf must be equal to T_max_ of
192 // actual piecewise curve.
193
6/6
✓ Branch 0 taken 7986 times.
✓ Branch 1 taken 78 times.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 7984 times.
✓ Branch 6 taken 2 times.
✓ Branch 7 taken 8062 times.
 16128 if (size_ != 0 && !(fabs(cf->min() - T_max_) < MARGIN)) {
194
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 std::stringstream ss;
195
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 ss << "Can not add new Polynom to PiecewiseCurve : time discontinuity "
196 "between T_max_ and pol.min(). Current "
197 "T_max is "
198
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
 4 << T_max_ << " new curve min is " << cf->min();
199
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 4 throw std::invalid_argument(ss.str().c_str());
200 4 }
201
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 8061 times.
 16124 if (cf->dim() != dim_) {
202
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::stringstream ss;
203
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ss << "All the curves in a piecewiseCurve should have the same "
204 "dimension. Current dim is "
205
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 << dim_ << " dim of the new curve is " << cf->dim();
206
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 throw std::invalid_argument(ss.str().c_str());
207 2 }
208 16122 curves_.push_back(cf);
209 16122 size_ = curves_.size();
210 16122 T_max_ = cf->max();
211
2/2
✓ Branch 0 taken 78 times.
✓ Branch 1 taken 7983 times.
 16122 if (size_ == 1) {
212 // First curve added
213
2/4
✓ Branch 2 taken 78 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 78 times.
✗ Branch 6 not taken.
 156 time_curves_.push_back(cf->min());
214 156 T_min_ = cf->min();
215 }
216 16122 time_curves_.push_back(T_max_);
217 16122 }
218
219 /// \brief Check if the curve is continuous of order given.
220 /// \param order : order of continuity we want to check.
221 /// \return True if the curve is continuous of order given.
222 ///
223 59 bool is_continuous(const std::size_t order) {
224 59 check_if_not_empty();
225 59 bool isContinuous = true;
226 59 std::size_t i = 0;
227
2/2
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 20 times.
 59 if (order == 0) {
228
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
 25 point_t value_end, value_start;
229
4/4
✓ Branch 1 taken 39 times.
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 29 times.
✓ Branch 4 taken 10 times.
 133 while (isContinuous && i < (size_ - 1)) {
230
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 54 curve_ptr_t current = curves_.at(i);
231
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 54 curve_ptr_t next = curves_.at(i + 1);
232
2/6
✓ Branch 3 taken 29 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 29 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 54 value_end = (*current)(current->max());
233
2/6
✓ Branch 3 taken 29 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 29 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 54 value_start = (*next)(next->min());
234
3/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 25 times.
 54 if (!value_end.isApprox(value_start, MARGIN)) {
235 7 isContinuous = false;
236 }
237 54 i++;
238 }
239 17 } else {
240
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 34 point_derivate_t value_end, value_start;
241
4/4
✓ Branch 1 taken 42 times.
✓ Branch 2 taken 9 times.
✓ Branch 3 taken 31 times.
✓ Branch 4 taken 11 times.
 142 while (isContinuous && i < (size_ - 1)) {
242
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 54 curve_ptr_t current = curves_.at(i);
243
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 54 curve_ptr_t next = curves_.at(i + 1);
244
2/6
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 31 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 54 value_end = current->derivate(current->max(), order);
245
2/6
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 31 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 54 value_start = next->derivate(next->min(), order);
246
3/4
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✓ Branch 4 taken 22 times.
 54 if (!value_end.isApprox(value_start, MARGIN)) {
247 16 isContinuous = false;
248 }
249 54 i++;
250 }
251 28 }
252 59 return isContinuous;
253 }
254
255 /// \brief Get number of curves in piecewise curve.
256 /// \return Number of curves in piecewise curve.
257 258 std::size_t num_curves() const { return curves_.size(); }
258
259 /// \brief Get curve corresponding to time t in piecewise curve.
260 /// Example : A piecewise curve PC made of two curves : c1 for t in [0,1] and
261 /// c2 for t in ]1,2].
262 /// PC.curve_at_time(0.5) will return c1.
263 /// \param t : time to select curve.
264 /// \return Curve corresponding to time t in piecewise curve.
265 ✗ curve_ptr_t curve_at_time(const time_t t) const {
266 ✗ return curves_[find_interval(t)];
267 }
268
269 /// \brief Get curve at specified index in piecewise curve.
270 /// \param idx : Index of curve to return, from 0 to num_curves-1.
271 /// \return curve corresonding to index in piecewise curve.
272 104 curve_ptr_t curve_at_index(const std::size_t idx) const {
273
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 52 times.
 104 if (Safe && idx >= num_curves()) {
274 ✗ throw std::length_error(
275 "curve_at_index: requested index greater than number of curves in "
276 "piecewise_curve instance");
277 }
278 104 return curves_[idx];
279 }
280
281 /// \brief Convert all curves in piecewise curve into bezier curves.
282 /// \return piecewise bezier curve.
283 ///
284 template <typename Bezier>
285 5 piecewise_curve_t convert_piecewise_curve_to_bezier() {
286 5 check_if_not_empty();
287 // check if given Bezier curve have the correct dimension :
288 BOOST_STATIC_ASSERT(
289 boost::is_same<typename Bezier::point_t, point_t>::value);
290 BOOST_STATIC_ASSERT(boost::is_same<typename Bezier::point_derivate_t,
291 point_derivate_t>::value);
292 // Create piecewise curve
293 5 piecewise_curve_t pc_res;
294 // Convert and add all other curves (segments)
295
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 4 times.
 17 for (std::size_t i = 0; i < size_; i++) {
296
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
 12 pc_res.add_curve(bezier_from_curve<Bezier>(*curves_.at(i)));
297 }
298 5 return pc_res;
299 ✗ }
300
301 /// \brief Convert all curves in piecewise curve into cubic hermite curves.
302 /// Curves need to be of degree inferior or equal to three.
303 /// \return piecewise cubic hermite curve.
304 ///
305 template <typename Hermite>
306 4 piecewise_curve_t convert_piecewise_curve_to_cubic_hermite() {
307 4 check_if_not_empty();
308 // check if given Hermite curve have the correct dimension :
309 BOOST_STATIC_ASSERT(
310 boost::is_same<typename Hermite::point_t, point_t>::value);
311 BOOST_STATIC_ASSERT(boost::is_same<typename Hermite::point_derivate_t,
312 point_derivate_t>::value);
313 // Create piecewise curve
314 4 piecewise_curve_t pc_res;
315 // Convert and add all other curves (segments)
316
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 4 times.
 14 for (std::size_t i = 0; i < size_; i++) {
317
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
 10 pc_res.add_curve(hermite_from_curve<Hermite>(*curves_.at(i)));
318 }
319 4 return pc_res;
320 ✗ }
321
322 /// \brief Convert all curves in piecewise curve into polynomial curves.
323 /// \return piecewise polynomial curve.
324 ///
325 template <typename Polynomial>
326 5 piecewise_curve_t convert_piecewise_curve_to_polynomial() {
327 5 check_if_not_empty();
328 // check if given Polynomial curve have the correct dimension :
329 BOOST_STATIC_ASSERT(
330 boost::is_same<typename Polynomial::point_t, point_t>::value);
331 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t,
332 point_derivate_t>::value);
333 // Create piecewise curve
334 5 piecewise_curve_t pc_res;
335 // Convert and add all other curves (segments)
336
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 4 times.
 16 for (std::size_t i = 0; i < size_; i++) {
337
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 11 pc_res.add_curve(polynomial_from_curve<Polynomial>(*curves_.at(i)));
338 }
339 5 return pc_res;
340 ✗ }
341
342 /// \brief Convert discrete points into piecewise polynomial curve with C0
343 /// continuity. \param points : discrete points to convert. \param time_points
344 /// : time corresponding to each point in piecewise curve. \return piecewise
345 /// polynomial curve of C0 continuity.
346 ///
347 template <typename Polynomial>
348 7 static piecewise_curve_t convert_discrete_points_to_polynomial(
349 t_point_t points, t_time_t time_points) {
350
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 7 if (Safe & !(points.size() > 1)) {
351 // std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<"
352 // t="<<t<<std::endl;
353 ✗ throw std::invalid_argument(
354 "piecewise_curve::convert_discrete_points_to_polynomial: Error, less "
355 "than 2 discrete points");
356 }
357
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 7 if (points.size() != time_points.size()) {
358 ✗ throw std::invalid_argument(
359 "piecewise_curve::convert_discrete_points_to_polynomial: Error, "
360 "points and time_points must have the same "
361 "size.");
362 }
363 // check if given Polynomial curve have the correct dimension :
364 BOOST_STATIC_ASSERT(
365 boost::is_same<typename Polynomial::point_t, point_t>::value);
366 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t,
367 point_derivate_t>::value);
368 7 piecewise_curve_t piecewise_res;
369
370
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 3 times.
 34 for (size_t i = 1; i < points.size(); ++i) {
371
3/4
✓ Branch 3 taken 15 times.
✓ Branch 4 taken 1 times.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 29 piecewise_res.add_curve(Polynomial(points[i - 1], points[i],
372 29 time_points[i - 1], time_points[i]));
373 }
374 5 return piecewise_res;
375 2 }
376
377 /// \brief Convert discrete points into piecewise polynomial curve with C1
378 /// continuity. \param points : discrete points to convert. \param
379 /// points_derivative : derivative of order 1 corresponding to each point in
380 /// piecewise curve. \param time_points : time corresponding to each point in
381 /// piecewise curve. \return piecewise polynomial curve of C1 continuity.
382 ///
383 template <typename Polynomial>
384 5 static piecewise_curve_t convert_discrete_points_to_polynomial(
385 t_point_t points, t_point_derivate_t points_derivative,
386 t_time_t time_points) {
387
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 5 if (Safe & !(points.size() > 1)) {
388 // std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<"
389 // t="<<t<<std::endl;
390 ✗ throw std::invalid_argument(
391 "piecewise_curve::convert_discrete_points_to_polynomial: Error, less "
392 "than 2 discrete points");
393 }
394
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 5 if (points.size() != time_points.size()) {
395 ✗ throw std::invalid_argument(
396 "piecewise_curve::convert_discrete_points_to_polynomial: Error, "
397 "points and time_points must have the same "
398 "size.");
399 }
400
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 5 if (points.size() != points_derivative.size()) {
401 ✗ throw std::invalid_argument(
402 "piecewise_curve::convert_discrete_points_to_polynomial: Error, "
403 "points and points_derivative must have the "
404 "same size.");
405 }
406 // check if given Polynomial curve have the correct dimension :
407 BOOST_STATIC_ASSERT(
408 boost::is_same<typename Polynomial::point_t, point_t>::value);
409 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t,
410 point_derivate_t>::value);
411 5 piecewise_curve_t piecewise_res;
412
413
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 2 times.
 20 for (size_t i = 1; i < points.size(); ++i) {
414
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 15 piecewise_res.add_curve(
415
2/2
✓ Branch 4 taken 9 times.
✓ Branch 5 taken 1 times.
 32 Polynomial(points[i - 1], points_derivative[i - 1], points[i],
416 17 points_derivative[i], time_points[i - 1], time_points[i]));
417 }
418 3 return piecewise_res;
419 2 }
420
421 /// \brief Convert discrete points into piecewise polynomial curve with C2
422 /// continuity. \param points : discrete points to convert. \param
423 /// points_derivative : derivative of order 1 corresponding to each point in
424 /// piecewise curve. \param points_second_derivative : derivative of order 2
425 /// corresponding to each point in piecewise curve. \param time_points : time
426 /// corresponding to each point in piecewise curve. \return piecewise
427 /// polynomial curve of C2 continuity.
428 ///
429 template <typename Polynomial>
430 5 static piecewise_curve_t convert_discrete_points_to_polynomial(
431 t_point_t points, t_point_derivate_t points_derivative,
432 t_point_derivate_t points_second_derivative, t_time_t time_points) {
433
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
 5 if (Safe & !(points.size() > 1)) {
434 // std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<"
435 // t="<<t<<std::endl;
436 ✗ throw std::invalid_argument(
437 "piecewise_curve::convert_discrete_points_to_polynomial: Error, less "
438 "than 2 discrete points");
439 }
440
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 5 if (points.size() != time_points.size()) {
441 ✗ throw std::invalid_argument(
442 "piecewise_curve::convert_discrete_points_to_polynomial: Error, "
443 "points and time_points must have the same "
444 "size.");
445 }
446
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 5 if (points.size() != points_derivative.size()) {
447 ✗ throw std::invalid_argument(
448 "piecewise_curve::convert_discrete_points_to_polynomial: Error, "
449 "points and points_derivative must have the "
450 "same size.");
451 }
452
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
 5 if (points.size() != points_second_derivative.size()) {
453 ✗ throw std::invalid_argument(
454 "piecewise_curve::convert_discrete_points_to_polynomial: Error, "
455 "points and points_second_derivative must "
456 "have the same size.");
457 }
458 // check if given Polynomial curve have the correct dimension :
459 BOOST_STATIC_ASSERT(
460 boost::is_same<typename Polynomial::point_t, point_t>::value);
461 BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t,
462 point_derivate_t>::value);
463 5 piecewise_curve_t piecewise_res;
464
465
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 2 times.
 20 for (size_t i = 1; i < points.size(); ++i) {
466
3/4
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 1 times.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 17 piecewise_res.add_curve(Polynomial(
467 17 points[i - 1], points_derivative[i - 1],
468 17 points_second_derivative[i - 1], points[i], points_derivative[i],
469 17 points_second_derivative[i], time_points[i - 1], time_points[i]));
470 }
471 3 return piecewise_res;
472 2 }
473
474 /**
475 * @brief load_piecewise_from_text_file build a piecewise polynomial from a
476 * list of discrete points read from a file. The file should contains one
477 * points per line, optionally with it's derivative and second derivatives.
478 * Each lines should then contains dim, 2*dim or 3*dim values
479 * @param filename the (absolute) name of the file to load
480 * @param dt the time step between each points in the file
481 * @param dim the dimension of the curve
482 * @return a piecewise curves containing polynomial connectiong all the points
483 * in the file
484 */
485 template <typename Polynomial>
486 5 static piecewise_curve_t load_piecewise_from_text_file(
487 const std::string& filename, const time_t dt, const size_t dim) {
488
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (dim <= 0)
489 ✗ throw std::invalid_argument("The dimension should be strictly positive.");
490
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (dt <= 0.)
491 ✗ throw std::invalid_argument("The time step should be strictly positive.");
492
493
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 piecewise_curve_t piecewise_res;
494
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ifstream file;
495
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 file.open(filename.c_str());
496
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 5 point_t last_pos = point_t::Zero(dim), last_vel = point_t::Zero(dim),
497
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 5 last_acc = point_t::Zero(dim), new_pos = point_t::Zero(dim),
498
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 5 new_vel = point_t::Zero(dim), new_acc = point_t::Zero(dim);
499 bool use_vel, use_acc;
500 5 std::string line;
501 // read first line to found out if we use velocity / acceleration :
502
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::getline(file, line);
503
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::istringstream iss_length(line);
504 5 const size_t length =
505
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 std::distance(std::istream_iterator<std::string>(iss_length),
506 10 std::istream_iterator<std::string>());
507
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 3 times.
 5 if (length == dim) {
508 2 use_vel = false;
509 2 use_acc = false;
510
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 2 times.
 3 } else if (length == dim * 2) {
511 1 use_vel = true;
512 1 use_acc = false;
513
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 } else if (length == dim * 3) {
514 1 use_vel = true;
515 1 use_acc = true;
516 } else {
517
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::stringstream error;
518
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 error << "The first line of the file shold contains either " << dim
519
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 << ", " << dim * 2 << " or " << dim * 3
520
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 << "values, got : " << length;
521
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 throw std::invalid_argument(error.str());
522 1 }
523 // initialize the first points of the trajectory:
524 num_t val;
525
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 std::istringstream iss(line);
526
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 4 times.
 16 for (size_t i = 0; i < dim; ++i) {
527
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 iss >> val;
528
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 last_pos[i] = val;
529 }
530
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
 4 if (use_vel) {
531
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 2 times.
 8 for (size_t i = 0; i < dim; ++i) {
532
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 iss >> val;
533
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 last_vel[i] = val;
534 }
535 }
536
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
 4 if (use_acc) {
537
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (size_t i = 0; i < dim; ++i) {
538
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 iss >> val;
539
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 last_acc[i] = val;
540 }
541 }
542
543 size_t current_length;
544 4 size_t line_id = 0;
545 // parse all lines of the file:
546
4/6
✓ Branch 3 taken 7861 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7861 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 7858 times.
✓ Branch 9 taken 3 times.
 15719 while (std::getline(file, line)) {
547 7858 ++line_id;
548
1/2
✓ Branch 1 taken 7858 times.
✗ Branch 2 not taken.
 7858 std::istringstream iss_length(line);
549 7858 current_length =
550
1/2
✓ Branch 1 taken 7858 times.
✗ Branch 2 not taken.
 15716 std::distance(std::istream_iterator<std::string>(iss_length),
551 15716 std::istream_iterator<std::string>());
552
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 7857 times.
 7858 if (current_length != length) {
553
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::stringstream error;
554
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 error << "Cannot parse line " << line_id << " got " << current_length
555
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 << " values instead of " << length;
556
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 throw std::invalid_argument(error.str());
557 1 }
558
1/2
✓ Branch 1 taken 7857 times.
✗ Branch 2 not taken.
 7857 std::istringstream iss(line);
559 // parse the points values from the file:
560
2/2
✓ Branch 0 taken 23571 times.
✓ Branch 1 taken 7857 times.
 31428 for (size_t i = 0; i < dim; ++i) {
561
1/2
✓ Branch 1 taken 23571 times.
✗ Branch 2 not taken.
 23571 iss >> val;
562
1/2
✓ Branch 1 taken 23571 times.
✗ Branch 2 not taken.
 23571 new_pos[i] = val;
563 }
564
2/2
✓ Branch 0 taken 7853 times.
✓ Branch 1 taken 4 times.
 7857 if (use_vel) {
565
2/2
✓ Branch 0 taken 23559 times.
✓ Branch 1 taken 7853 times.
 31412 for (size_t i = 0; i < dim; ++i) {
566
1/2
✓ Branch 1 taken 23559 times.
✗ Branch 2 not taken.
 23559 iss >> val;
567
1/2
✓ Branch 1 taken 23559 times.
✗ Branch 2 not taken.
 23559 new_vel[i] = val;
568 }
569 }
570
2/2
✓ Branch 0 taken 7850 times.
✓ Branch 1 taken 7 times.
 7857 if (use_acc) {
571
2/2
✓ Branch 0 taken 23550 times.
✓ Branch 1 taken 7850 times.
 31400 for (size_t i = 0; i < dim; ++i) {
572
1/2
✓ Branch 1 taken 23550 times.
✗ Branch 2 not taken.
 23550 iss >> val;
573
1/2
✓ Branch 1 taken 23550 times.
✗ Branch 2 not taken.
 23550 new_acc[i] = val;
574 }
575 }
576 // append a new curves connectiong this points
577
2/2
✓ Branch 0 taken 7850 times.
✓ Branch 1 taken 7 times.
 7857 if (use_acc) {
578
1/2
✓ Branch 1 taken 7850 times.
✗ Branch 2 not taken.
 7850 piecewise_res.add_curve(
579 7850 Polynomial(last_pos, last_vel, last_acc, new_pos, new_vel, new_acc,
580 7850 dt * static_cast<time_t>(line_id - 1),
581
1/2
✓ Branch 1 taken 7850 times.
✗ Branch 2 not taken.
 7850 dt * static_cast<time_t>(line_id)));
582
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 4 times.
 7 } else if (use_vel) {
583
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 piecewise_res.add_curve(
584 3 Polynomial(last_pos, last_vel, new_pos, new_vel,
585 3 dt * static_cast<time_t>(line_id - 1),
586
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 dt * static_cast<time_t>(line_id)));
587 } else {
588
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 piecewise_res.add_curve(
589 4 Polynomial(last_pos, new_pos, dt * static_cast<time_t>(line_id - 1),
590
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 dt * static_cast<time_t>(line_id)));
591 }
592
1/2
✓ Branch 1 taken 7857 times.
✗ Branch 2 not taken.
 7857 last_pos = new_pos;
593
1/2
✓ Branch 1 taken 7857 times.
✗ Branch 2 not taken.
 7857 last_vel = new_vel;
594
1/2
✓ Branch 1 taken 7857 times.
✗ Branch 2 not taken.
 7857 last_acc = new_acc;
595 }
596
597
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 file.close();
598 6 return piecewise_res;
599 24 }
600
601 private:
602 /// \brief Get index of the interval corresponding to time t for the
603 /// interpolation. \param t : time where to look for interval. \return Index
604 /// of interval for time t.
605 ///
606 28554 std::size_t find_interval(const Numeric t) const {
607 // time before first control point time.
608
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 14277 times.
 28554 if (t < time_curves_[0]) {
609 ✗ return 0;
610 }
611 // time is after last control point time
612
2/2
✓ Branch 1 taken 6529 times.
✓ Branch 2 taken 7748 times.
 28554 if (t > time_curves_[size_ - 1]) {
613 13058 return size_ - 1;
614 }
615
616 15496 std::size_t left_id = 0;
617 15496 std::size_t right_id = size_ - 1;
618
2/2
✓ Branch 0 taken 15911 times.
✓ Branch 1 taken 7447 times.
 46716 while (left_id <= right_id) {
619 31822 const std::size_t middle_id = left_id + (right_id - left_id) / 2;
620
2/2
✓ Branch 1 taken 7713 times.
✓ Branch 2 taken 8198 times.
 31822 if (time_curves_.at(middle_id) < t) {
621 15426 left_id = middle_id + 1;
622
2/2
✓ Branch 1 taken 7897 times.
✓ Branch 2 taken 301 times.
 16396 } else if (time_curves_.at(middle_id) > t) {
623 15794 right_id = middle_id - 1;
624 } else {
625 602 return middle_id;
626 }
627 }
628 14894 return left_id - 1;
629 }
630
631 28648 void check_if_not_empty() const {
632
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 14324 times.
 28648 if (curves_.size() == 0) {
633 ✗ throw std::runtime_error("Error in piecewise curve : No curve added");
634 }
635 28648 }
636
637 /*Helpers*/
638 public:
639 /// \brief Get dimension of curve.
640 /// \return dimension of curve.
641 310 std::size_t virtual dim() const { return dim_; };
642 /// \brief Get the minimum time for which the curve is defined
643 /// \return \f$t_{min}\f$, lower bound of time range.
644 164 Time virtual min() const { return T_min_; }
645 /// \brief Get the maximum time for which the curve is defined.
646 /// \return \f$t_{max}\f$, upper bound of time range.
647 710 Time virtual max() const { return T_max_; }
648 /// \brief Get the degree of the curve.
649 /// \return \f$degree\f$, the degree of the curve.
650 ✗ virtual std::size_t degree() const {
651 ✗ throw std::runtime_error(
652 "degree() method is not implemented for this type of curve.");
653 }
654 3 std::size_t getNumberCurves() { return curves_.size(); }
655 /*Helpers*/
656
657 /* Attributes */
658 std::size_t dim_; // Dim of curve
659 t_curve_ptr_t curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
660 t_time_t time_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
661 std::size_t size_; // Number of segments in piecewise curve = size of curves_
662 Time T_min_, T_max_;
663 /* Attributes */
664
665 // Serialization of the class
666 friend class boost::serialization::access;
667
668 template <class Archive>
669 72 void serialize(Archive& ar, const unsigned int version) {
670 if (version) {
671 // Do something depending on version ?
672 }
673
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 36 times.
✗ Branch 6 not taken.
 72 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(base_curve_t);
674
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 72 ar& boost::serialization::make_nvp("dim", dim_);
675
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 72 ar& boost::serialization::make_nvp("curves", curves_);
676
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 72 ar& boost::serialization::make_nvp("time_curves", time_curves_);
677
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 72 ar& boost::serialization::make_nvp("size", size_);
678
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 72 ar& boost::serialization::make_nvp("T_min", T_min_);
679
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 72 ar& boost::serialization::make_nvp("T_max", T_max_);
680 72 }
681 }; // End struct piecewise curve
682 } // namespace ndcurves
683
684 DEFINE_CLASS_TEMPLATE_VERSION(
685 SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point,
686 typename Point_derivate, typename CurveType),
687 SINGLE_ARG(ndcurves::piecewise_curve<Time, Numeric, Safe, Point,
688 Point_derivate, CurveType>))
689
690 #endif // _CLASS_PIECEWISE_CURVE
691