| Directory: | ./ |
|---|---|
| File: | include/ndcurves/cubic_hermite_spline.h |
| Date: | 2025-03-05 17:18:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 137 | 148 | 92.6% |
| Branches: | 100 | 190 | 52.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /** | ||
| 2 | * \file cubic_hermite_spline.h | ||
| 3 | * \brief class allowing to create a cubic hermite spline of any dimension. | ||
| 4 | * \author Justin Carpentier <jcarpent@laas.fr> modified by Jason Chemin | ||
| 5 | * <jchemin@laas.fr> \date 05/2019 | ||
| 6 | */ | ||
| 7 | |||
| 8 | #ifndef _CLASS_CUBICHERMITESPLINE | ||
| 9 | #define _CLASS_CUBICHERMITESPLINE | ||
| 10 | |||
| 11 | #include <boost/serialization/utility.hpp> // To serialize std::pair | ||
| 12 | #include <iostream> | ||
| 13 | #include <stdexcept> | ||
| 14 | #include <vector> | ||
| 15 | |||
| 16 | #include "MathDefs.h" | ||
| 17 | #include "bezier_curve.h" | ||
| 18 | #include "curve_abc.h" | ||
| 19 | #include "piecewise_curve.h" | ||
| 20 | |||
| 21 | namespace ndcurves { | ||
| 22 | /// \class CubicHermiteSpline. | ||
| 23 | /// \brief Represents a set of cubic hermite splines defining a continuous | ||
| 24 | /// function \f$p(t)\f$. A hermite cubic spline is a minimal degree polynom | ||
| 25 | /// interpolating a function in two points \f$P_i\f$ and \f$P_{i+1}\f$ with its | ||
| 26 | /// tangent \f$m_i\f$ and \f$m_{i+1}\f$.<br> A hermite cubic spline : | ||
| 27 | /// - crosses each of the waypoint given in its initialization (\f$P_0\f$, | ||
| 28 | /// \f$P_1\f$,...,\f$P_N\f$). | ||
| 29 | /// - has its derivatives on \f$P_i\f$ and \f$P_{i+1}\f$ are \f$p'(t_{P_i}) = | ||
| 30 | /// m_i\f$ and \f$p'(t_{P_{i+1}}) = m_{i+1}\f$. | ||
| 31 | /// | ||
| 32 | template <typename Time = double, typename Numeric = Time, bool Safe = false, | ||
| 33 | typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> > | ||
| 34 | struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> { | ||
| 35 | typedef Point point_t; | ||
| 36 | typedef std::pair<Point, Point> pair_point_tangent_t; | ||
| 37 | typedef std::vector<pair_point_tangent_t, | ||
| 38 | Eigen::aligned_allocator<pair_point_tangent_t> > | ||
| 39 | t_pair_point_tangent_t; | ||
| 40 | typedef std::vector<Time> vector_time_t; | ||
| 41 | typedef Time time_t; | ||
| 42 | typedef Numeric num_t; | ||
| 43 | typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class | ||
| 44 | typedef cubic_hermite_spline<Time, Numeric, Safe, point_t> | ||
| 45 | cubic_hermite_spline_t; | ||
| 46 | typedef bezier_curve<Time, Numeric, Safe, point_t> bezier_t; | ||
| 47 | typedef typename bezier_t::t_point_t t_point_t; | ||
| 48 | typedef piecewise_curve<Time, Numeric, Safe, point_t, point_t, bezier_t> | ||
| 49 | piecewise_bezier_t; | ||
| 50 | |||
| 51 | public: | ||
| 52 | /// \brief Empty constructor. Curve obtained this way can not perform other | ||
| 53 | /// class functions. | ||
| 54 | /// | ||
| 55 |
1/2✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
|
22 | cubic_hermite_spline() : dim_(0), T_min_(0), T_max_(0) {} |
| 56 | |||
| 57 | /// \brief Constructor. | ||
| 58 | /// \param PairsBegin : an iterator pointing to the first element of a | ||
| 59 | /// pair(position, derivative) container. \param PairsEnd : an iterator | ||
| 60 | /// pointing to the last element of a pair(position, derivative) container. | ||
| 61 | /// \param time_control_points : vector containing time for each waypoint. | ||
| 62 | /// | ||
| 63 | template <typename In> | ||
| 64 | 49 | cubic_hermite_spline(In PairsBegin, In PairsEnd, | |
| 65 | const vector_time_t& time_control_points) | ||
| 66 |
2/4✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 29 times.
✗ Branch 8 not taken.
|
49 | : size_(std::distance(PairsBegin, PairsEnd)), degree_(3) { |
| 67 | // Check size of pairs container. | ||
| 68 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 29 times.
|
49 | if (Safe && size_ < 1) { |
| 69 | ✗ | throw std::length_error( | |
| 70 | "can not create cubic_hermite_spline, number of pairs is inferior to " | ||
| 71 | "2."); | ||
| 72 | } | ||
| 73 | // Set dimension according to size of points | ||
| 74 | 49 | dim_ = PairsBegin->first.size(); | |
| 75 | // Push all pairs in controlPoints | ||
| 76 | 49 | In it(PairsBegin); | |
| 77 |
2/2✓ Branch 2 taken 59 times.
✓ Branch 3 taken 29 times.
|
149 | for (; it != PairsEnd; ++it) { |
| 78 |
2/4✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 59 times.
|
200 | if (Safe && (static_cast<size_t>(it->first.size()) != dim_ || |
| 79 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 59 times.
|
100 | static_cast<size_t>(it->second.size()) != dim_)) |
| 80 | ✗ | throw std::invalid_argument( | |
| 81 | "All the control points and their derivatives must have the same " | ||
| 82 | "dimension."); | ||
| 83 |
1/2✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
|
100 | control_points_.push_back(*it); |
| 84 | } | ||
| 85 | // Set time | ||
| 86 |
1/2✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
|
49 | setTime(time_control_points); |
| 87 | 49 | } | |
| 88 | |||
| 89 | 13 | cubic_hermite_spline(const cubic_hermite_spline& other) | |
| 90 | 13 | : dim_(other.dim_), | |
| 91 | 13 | control_points_(other.control_points_), | |
| 92 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | time_control_points_(other.time_control_points_), |
| 93 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | duration_splines_(other.duration_splines_), |
| 94 | 13 | T_min_(other.T_min_), | |
| 95 | 13 | T_max_(other.T_max_), | |
| 96 | 13 | size_(other.size_), | |
| 97 |
1/2✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
|
26 | degree_(other.degree_) {} |
| 98 | |||
| 99 | /// \brief Destructor. | ||
| 100 | 140 | virtual ~cubic_hermite_spline() {} | |
| 101 | |||
| 102 | /*Operations*/ | ||
| 103 | public: | ||
| 104 | /// \brief Evaluation of the cubic hermite spline at time t. | ||
| 105 | /// \param t : time when to evaluate the spline. | ||
| 106 | /// \return \f$p(t)\f$ point corresponding on spline at time t. | ||
| 107 | /// | ||
| 108 | 4354 | virtual Point operator()(const time_t t) const { | |
| 109 | 4354 | check_conditions(); | |
| 110 |
3/6✓ Branch 0 taken 2177 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2177 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 2177 times.
|
4354 | if (Safe & !(T_min_ <= t && t <= T_max_)) { |
| 111 | ✗ | throw std::invalid_argument( | |
| 112 | "can't evaluate cubic hermite spline, out of range"); | ||
| 113 | } | ||
| 114 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2177 times.
|
4354 | if (size_ == 1) { |
| 115 | ✗ | return control_points_.front().first; | |
| 116 | } else { | ||
| 117 |
1/2✓ Branch 1 taken 2177 times.
✗ Branch 2 not taken.
|
4354 | const bezier_t bezier = buildCurrentBezier(t); |
| 118 |
1/2✓ Branch 1 taken 2177 times.
✗ Branch 2 not taken.
|
4354 | return bezier(t); |
| 119 | } | ||
| 120 | } | ||
| 121 | |||
| 122 | /** | ||
| 123 | * @brief isApprox check if other and *this are approximately equals. | ||
| 124 | * Only two curves of the same class can be approximately equals, for | ||
| 125 | * comparison between different type of curves see isEquivalent | ||
| 126 | * @param other the other curve to check | ||
| 127 | * @param prec the precision threshold, default | ||
| 128 | * Eigen::NumTraits<Numeric>::dummy_precision() | ||
| 129 | * @return true is the two curves are approximately equals | ||
| 130 | */ | ||
| 131 | 14 | bool isApprox( | |
| 132 | const cubic_hermite_spline_t& other, | ||
| 133 | const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const { | ||
| 134 |
1/2✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
|
28 | bool equal = ndcurves::isApprox<num_t>(T_min_, other.min()) && |
| 135 | 14 | ndcurves::isApprox<num_t>(T_max_, other.max()) && | |
| 136 |
3/6✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
|
28 | dim_ == other.dim() && degree_ == other.degree() && |
| 137 | 14 | size_ == other.size() && | |
| 138 |
2/4✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
|
42 | time_control_points_ == other.time_control_points_ && |
| 139 |
1/2✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
|
14 | duration_splines_ == other.duration_splines_; |
| 140 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7 times.
|
14 | if (!equal) return false; |
| 141 |
2/2✓ Branch 0 taken 14 times.
✓ Branch 1 taken 6 times.
|
40 | for (std::size_t i = 0; i < size_; ++i) { |
| 142 | 28 | if ((!control_points_[i].first.isApprox(other.control_points_[i].first, | |
| 143 |
4/4✓ Branch 0 taken 13 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 13 times.
|
54 | prec)) || |
| 144 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 13 times.
|
26 | (!control_points_[i].second.isApprox(other.control_points_[i].second, |
| 145 | prec))) | ||
| 146 | 2 | return false; | |
| 147 | } | ||
| 148 | 12 | return true; | |
| 149 | } | ||
| 150 | |||
| 151 | 4 | virtual bool isApprox( | |
| 152 | const curve_abc_t* other, | ||
| 153 | const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const { | ||
| 154 | 4 | const cubic_hermite_spline_t* other_cast = | |
| 155 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
4 | dynamic_cast<const cubic_hermite_spline_t*>(other); |
| 156 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
4 | if (other_cast) |
| 157 | 4 | return isApprox(*other_cast, prec); | |
| 158 | else | ||
| 159 | ✗ | return false; | |
| 160 | } | ||
| 161 | |||
| 162 | 10 | virtual bool operator==(const cubic_hermite_spline_t& other) const { | |
| 163 | 10 | return isApprox(other); | |
| 164 | } | ||
| 165 | |||
| 166 | 4 | virtual bool operator!=(const cubic_hermite_spline_t& other) const { | |
| 167 | 4 | return !(*this == other); | |
| 168 | } | ||
| 169 | |||
| 170 | /// \brief Evaluate the derivative of order N of spline at time t. | ||
| 171 | /// \param t : time when to evaluate the spline. | ||
| 172 | /// \param order : order of derivative. | ||
| 173 | /// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative | ||
| 174 | /// spline of order N at time t. | ||
| 175 | /// | ||
| 176 | 412 | virtual Point derivate(const time_t t, const std::size_t order) const { | |
| 177 | 412 | check_conditions(); | |
| 178 |
3/6✓ Branch 0 taken 206 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 206 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 206 times.
|
412 | if (Safe & !(T_min_ <= t && t <= T_max_)) { |
| 179 | ✗ | throw std::invalid_argument( | |
| 180 | "can't derivate cubic hermite spline, out of range"); | ||
| 181 | } | ||
| 182 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 206 times.
|
412 | if (size_ == 1) { |
| 183 | ✗ | return control_points_.front().second; | |
| 184 | } else { | ||
| 185 |
1/2✓ Branch 1 taken 206 times.
✗ Branch 2 not taken.
|
412 | const bezier_t bezier = buildCurrentBezier(t); |
| 186 |
1/2✓ Branch 1 taken 206 times.
✗ Branch 2 not taken.
|
412 | return bezier.derivate(t, order); |
| 187 | } | ||
| 188 | } | ||
| 189 | |||
| 190 | 8 | piecewise_bezier_t compute_derivate(const std::size_t order) const { | |
| 191 | 8 | piecewise_bezier_t res; | |
| 192 |
2/2✓ Branch 0 taken 8 times.
✓ Branch 1 taken 4 times.
|
24 | for (size_t i = 0; i < size_ - 1; ++i) { |
| 193 |
1/2✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
|
16 | const bezier_t curve = buildCurrentBezier(time_control_points_[i]); |
| 194 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
16 | res.add_curve(curve.compute_derivate(order)); |
| 195 | } | ||
| 196 | 8 | return res; | |
| 197 | } | ||
| 198 | |||
| 199 | /// \brief Compute the derived curve at order N. | ||
| 200 | /// \param order : order of derivative. | ||
| 201 | /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the | ||
| 202 | /// curve. | ||
| 203 | 8 | piecewise_bezier_t* compute_derivate_ptr(const std::size_t order) const { | |
| 204 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
8 | return new piecewise_bezier_t(compute_derivate(order)); |
| 205 | } | ||
| 206 | |||
| 207 | /// \brief Set time of each control point of cubic hermite spline. | ||
| 208 | /// Set duration of each spline, Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ | ||
| 209 | /// with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ | ||
| 210 | /// respectively.<br> \param time_control_points : Vector containing time for | ||
| 211 | /// each control point. | ||
| 212 | /// | ||
| 213 | 30 | void setTime(const vector_time_t& time_control_points) { | |
| 214 | 30 | time_control_points_ = time_control_points; | |
| 215 | 30 | T_min_ = time_control_points_.front(); | |
| 216 | 30 | T_max_ = time_control_points_.back(); | |
| 217 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 30 times.
|
30 | if (time_control_points.size() != size()) { |
| 218 | ✗ | throw std::length_error( | |
| 219 | "size of time control points should be equal to number of control " | ||
| 220 | "points"); | ||
| 221 | } | ||
| 222 | 30 | computeDurationSplines(); | |
| 223 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 30 times.
|
30 | if (!checkDurationSplines()) { |
| 224 | ✗ | throw std::invalid_argument( | |
| 225 | "time_splines not monotonous, all spline duration should be superior " | ||
| 226 | "to 0"); | ||
| 227 | } | ||
| 228 | 30 | } | |
| 229 | |||
| 230 | /// \brief Get vector of pair (positition, derivative) corresponding to | ||
| 231 | /// control points. \return vector containing control points. | ||
| 232 | /// | ||
| 233 | t_pair_point_tangent_t getControlPoints() { return control_points_; } | ||
| 234 | |||
| 235 | /// \brief Get vector of Time corresponding to Time for each control point. | ||
| 236 | /// \return vector containing time of each control point. | ||
| 237 | /// | ||
| 238 | 1 | vector_time_t getTime() { return time_control_points_; } | |
| 239 | |||
| 240 | /// \brief Get number of control points contained in the trajectory. | ||
| 241 | /// \return number of control points. | ||
| 242 | /// | ||
| 243 | 198 | std::size_t size() const { return size_; } | |
| 244 | |||
| 245 | /// \brief Get number of intervals (subsplines) contained in the trajectory. | ||
| 246 | /// \return number of intervals (subsplines). | ||
| 247 | /// | ||
| 248 | std::size_t numIntervals() const { return size() - 1; } | ||
| 249 | |||
| 250 | private: | ||
| 251 | /// \brief Get index of the interval (subspline) corresponding to time t for | ||
| 252 | /// the interpolation. \param t : time where to look for interval. \return | ||
| 253 | /// Index of interval for time t. | ||
| 254 | /// | ||
| 255 | 4782 | std::size_t findInterval(const time_t t) const { | |
| 256 | // time before first control point time. | ||
| 257 |
2/2✓ Branch 1 taken 77 times.
✓ Branch 2 taken 2314 times.
|
4782 | if (t <= time_control_points_[0]) { |
| 258 | 154 | return 0; | |
| 259 | } | ||
| 260 | // time is after last control point time | ||
| 261 |
2/2✓ Branch 1 taken 32 times.
✓ Branch 2 taken 2282 times.
|
4628 | if (t >= time_control_points_[size_ - 1]) { |
| 262 | 64 | return size_ - 2; | |
| 263 | } | ||
| 264 | 4564 | std::size_t left_id = 0; | |
| 265 | 4564 | std::size_t right_id = size_ - 1; | |
| 266 |
2/2✓ Branch 0 taken 4552 times.
✓ Branch 1 taken 2270 times.
|
13644 | while (left_id <= right_id) { |
| 267 | 9104 | const std::size_t middle_id = left_id + (right_id - left_id) / 2; | |
| 268 |
2/2✓ Branch 1 taken 2270 times.
✓ Branch 2 taken 2282 times.
|
9104 | if (time_control_points_.at(middle_id) < t) { |
| 269 | 4540 | left_id = middle_id + 1; | |
| 270 |
2/2✓ Branch 1 taken 2270 times.
✓ Branch 2 taken 12 times.
|
4564 | } else if (time_control_points_.at(middle_id) > t) { |
| 271 | 4540 | right_id = middle_id - 1; | |
| 272 | } else { | ||
| 273 | 24 | return middle_id; | |
| 274 | } | ||
| 275 | } | ||
| 276 | 4540 | return left_id - 1; | |
| 277 | } | ||
| 278 | |||
| 279 | /** | ||
| 280 | * @brief buildCurrentBezier set up the current_bezier_ attribut to represent | ||
| 281 | * the curve of the interval that contain t. This bezier is defined by the | ||
| 282 | * following control points: p0, p0 + m0/3, p1 - m1/3, p1 | ||
| 283 | * @param t the time for which the bezier is build | ||
| 284 | * @return the bezier curve | ||
| 285 | */ | ||
| 286 | 4782 | bezier_t buildCurrentBezier(const time_t t) const { | |
| 287 |
1/2✓ Branch 1 taken 2391 times.
✗ Branch 2 not taken.
|
4782 | size_t id_interval = findInterval(t); |
| 288 |
2/4✓ Branch 1 taken 2391 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2391 times.
✗ Branch 5 not taken.
|
4782 | const pair_point_tangent_t pair0 = control_points_.at(id_interval); |
| 289 |
2/4✓ Branch 1 taken 2391 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2391 times.
✗ Branch 5 not taken.
|
4782 | const pair_point_tangent_t pair1 = control_points_.at(id_interval + 1); |
| 290 | 4782 | const Time& t0 = time_control_points_[id_interval]; | |
| 291 | 4782 | const Time& t1 = time_control_points_[id_interval + 1]; | |
| 292 |
1/2✓ Branch 1 taken 2391 times.
✗ Branch 2 not taken.
|
4782 | t_point_t control_points; |
| 293 |
1/2✓ Branch 1 taken 2391 times.
✗ Branch 2 not taken.
|
4782 | control_points.reserve(4); |
| 294 |
1/2✓ Branch 1 taken 2391 times.
✗ Branch 2 not taken.
|
4782 | control_points.push_back(pair0.first); |
| 295 |
5/10✓ Branch 1 taken 2391 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2391 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2391 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2391 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2391 times.
✗ Branch 14 not taken.
|
4782 | control_points.push_back(pair0.first + pair0.second / 3. * (t1 - t0)); |
| 296 |
5/10✓ Branch 1 taken 2391 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2391 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2391 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2391 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2391 times.
✗ Branch 14 not taken.
|
4782 | control_points.push_back(pair1.first - pair1.second / 3. * (t1 - t0)); |
| 297 |
1/2✓ Branch 1 taken 2391 times.
✗ Branch 2 not taken.
|
4782 | control_points.push_back(pair1.first); |
| 298 |
1/2✓ Branch 3 taken 2391 times.
✗ Branch 4 not taken.
|
9564 | return bezier_t(control_points.begin(), control_points.end(), t0, t1); |
| 299 | } | ||
| 300 | |||
| 301 | /// \brief Check if control points list is not empty and dimension of point | ||
| 302 | /// superior to zero. | ||
| 303 | /// | ||
| 304 | 4766 | void check_conditions() const { | |
| 305 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2383 times.
|
4766 | if (control_points_.size() == 0) { |
| 306 | ✗ | throw std::runtime_error( | |
| 307 | "Error in cubic hermite : there is no control points set / did you " | ||
| 308 | "use empty constructor ?"); | ||
| 309 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2383 times.
|
4766 | } else if (dim_ == 0) { |
| 310 | ✗ | throw std::runtime_error( | |
| 311 | "Error in cubic hermite : Dimension of points is zero / did you use " | ||
| 312 | "empty constructor ?"); | ||
| 313 | } | ||
| 314 | } | ||
| 315 | |||
| 316 | /// \brief compute duration of each spline. | ||
| 317 | /// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., | ||
| 318 | /// T_{P_N}\f$ respectively, Duration of each subspline is : ( | ||
| 319 | /// T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} ). | ||
| 320 | /// | ||
| 321 | 30 | void computeDurationSplines() { | |
| 322 | 30 | duration_splines_.clear(); | |
| 323 | Time actual_time; | ||
| 324 | 30 | Time prev_time = *(time_control_points_.begin()); | |
| 325 | 30 | std::size_t i = 0; | |
| 326 |
2/2✓ Branch 1 taken 32 times.
✓ Branch 2 taken 30 times.
|
62 | for (i = 0; i < size() - 1; i++) { |
| 327 | 32 | actual_time = time_control_points_.at(i + 1); | |
| 328 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
32 | duration_splines_.push_back(actual_time - prev_time); |
| 329 | 32 | prev_time = actual_time; | |
| 330 | } | ||
| 331 | 30 | } | |
| 332 | |||
| 333 | /// \brief Check if duration of each subspline is strictly positive. | ||
| 334 | /// \return true if all duration of strictly positive, false otherwise. | ||
| 335 | /// | ||
| 336 | 30 | bool checkDurationSplines() const { | |
| 337 | 30 | std::size_t i = 0; | |
| 338 | 30 | bool is_positive = true; | |
| 339 |
5/6✓ Branch 0 taken 62 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 32 times.
✓ Branch 4 taken 30 times.
✓ Branch 5 taken 32 times.
✓ Branch 6 taken 30 times.
|
62 | while (is_positive && i < duration_splines_.size()) { |
| 340 | 32 | is_positive = (duration_splines_.at(i) > 0.); | |
| 341 | 32 | i++; | |
| 342 | } | ||
| 343 | 30 | return is_positive; | |
| 344 | } | ||
| 345 | /*Operations*/ | ||
| 346 | |||
| 347 | /*Helpers*/ | ||
| 348 | public: | ||
| 349 | /// \brief Get dimension of curve. | ||
| 350 | /// \return dimension of curve. | ||
| 351 | 60 | std::size_t virtual dim() const { return dim_; } | |
| 352 | /// \brief Get the minimum time for which the curve is defined | ||
| 353 | /// \return \f$t_{min}\f$, lower bound of time range. | ||
| 354 | 160 | Time virtual min() const { return time_control_points_.front(); } | |
| 355 | /// \brief Get the maximum time for which the curve is defined. | ||
| 356 | /// \return \f$t_{max}\f$, upper bound of time range. | ||
| 357 | 364 | Time virtual max() const { return time_control_points_.back(); } | |
| 358 | /// \brief Get the degree of the curve. | ||
| 359 | /// \return \f$degree\f$, the degree of the curve. | ||
| 360 | 48 | virtual std::size_t degree() const { return degree_; } | |
| 361 | /*Helpers*/ | ||
| 362 | |||
| 363 | /*Attributes*/ | ||
| 364 | /// Dim of curve | ||
| 365 | std::size_t dim_; | ||
| 366 | /// Vector of pair < Point, Tangent >. | ||
| 367 | t_pair_point_tangent_t control_points_; | ||
| 368 | /// Vector of Time corresponding to time of each N control points : time at | ||
| 369 | /// \f$P_0, P_1, P_2, ..., P_N\f$. Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ | ||
| 370 | /// with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ | ||
| 371 | /// respectively. | ||
| 372 | vector_time_t time_control_points_; | ||
| 373 | /// Vector of Time corresponding to time duration of each subspline.<br> | ||
| 374 | /// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., | ||
| 375 | /// T_{P_N}\f$ respectively, duration of each subspline is : ( | ||
| 376 | /// T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} )<br> It | ||
| 377 | /// contains \f$N-1\f$ durations. | ||
| 378 | vector_time_t duration_splines_; | ||
| 379 | /// Starting time of cubic hermite spline : T_min_ is equal to first time of | ||
| 380 | /// control points. | ||
| 381 | /*const*/ Time T_min_; | ||
| 382 | /// Ending time of cubic hermite spline : T_max_ is equal to last time of | ||
| 383 | /// control points. | ||
| 384 | /*const*/ Time T_max_; | ||
| 385 | /// Number of control points (pairs). | ||
| 386 | std::size_t size_; | ||
| 387 | /// Degree (Cubic so degree 3) | ||
| 388 | std::size_t degree_; | ||
| 389 | /*Attributes*/ | ||
| 390 | |||
| 391 | // Serialization of the class | ||
| 392 | friend class boost::serialization::access; | ||
| 393 | |||
| 394 | template <class Archive> | ||
| 395 | 44 | void serialize(Archive& ar, const unsigned int version) { | |
| 396 | if (version) { | ||
| 397 | // Do something depending on version ? | ||
| 398 | } | ||
| 399 |
1/2✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
|
44 | ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t); |
| 400 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
44 | ar& boost::serialization::make_nvp("dim", dim_); |
| 401 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
44 | ar& boost::serialization::make_nvp("control_points", control_points_); |
| 402 |
1/2✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
|
44 | ar& boost::serialization::make_nvp("time_control_points", |
| 403 | 44 | time_control_points_); | |
| 404 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
44 | ar& boost::serialization::make_nvp("duration_splines", duration_splines_); |
| 405 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
44 | ar& boost::serialization::make_nvp("T_min", T_min_); |
| 406 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
44 | ar& boost::serialization::make_nvp("T_max", T_max_); |
| 407 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
44 | ar& boost::serialization::make_nvp("size", size_); |
| 408 |
1/2✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
|
44 | ar& boost::serialization::make_nvp("degree", degree_); |
| 409 | } | ||
| 410 | }; // End struct Cubic hermite spline | ||
| 411 | } // namespace ndcurves | ||
| 412 | |||
| 413 | DEFINE_CLASS_TEMPLATE_VERSION( | ||
| 414 | SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point), | ||
| 415 | SINGLE_ARG(ndcurves::cubic_hermite_spline<Time, Numeric, Safe, Point>)) | ||
| 416 | #endif //_CLASS_CUBICHERMITESPLINE | ||
| 417 |