cubic_hermite_spline.h 15.4 KB
Newer Older
1
2
3
4
5
6
7
/**
* \file cubic_hermite_spline.h
* \brief class allowing to create a cubic hermite spline of any dimension.
* \author Justin Carpentier <jcarpent@laas.fr> modified by Jason Chemin <jchemin@laas.fr>
* \date 05/2019
*/

8
9
10
11
12
13
14
15
16
17
18
19
20
#ifndef _CLASS_CUBICHERMITESPLINE
#define _CLASS_CUBICHERMITESPLINE

#include "curve_abc.h"
#include "curve_constraint.h"

#include "MathDefs.h"

#include <vector>
#include <stdexcept>

#include <iostream>

21
22
23
24
25
#include "serialization/archive.hpp"
#include "serialization/eigen-matrix.hpp"

#include <boost/serialization/utility.hpp>

26
27
namespace curves
{
28
29
30
31
32
33
34
35
36
37
  /// \class CubicHermiteSpline.
  /// \brief Represents a set of cubic hermite splines defining a continuous function \f$p(t)\f$.
  /// A hermite cubic spline is a minimal degree polynom interpolating a function in two 
  /// points \f$P_i\f$ and \f$P_{i+1}\f$ with its tangent \f$m_i\f$ and \f$m_{i+1}\f$.<br>
  /// A hermite cubic spline :
  /// - crosses each of the waypoint given in its initialization (\f$P_0\f$, \f$P_1\f$,...,\f$P_N\f$).
  /// - has its derivatives on \f$P_i\f$ and \f$P_{i+1}\f$ are \f$p'(t_{P_i}) = m_i\f$ and \f$p'(t_{P_{i+1}}) = m_{i+1}\f$.
  ///
  template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
           typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
38
           typename Tangent= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
39
40
41
  struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>,
                                public serialization::Serializable< cubic_hermite_spline<Time, Numeric, Dim, Safe, Point, Tangent> >
  {
42
43
44
    typedef std::pair<Point, Tangent> pair_point_tangent_t; 
    typedef std::vector< pair_point_tangent_t ,Eigen::aligned_allocator<Point> > t_pair_point_tangent_t;
    typedef std::vector<Time> vector_time_t;
45
    typedef Numeric num_t;
46

47
    public:
48

49
50
51
52
53
54
55
56
57
58
59
      cubic_hermite_spline(){}

      /// \brief Constructor.
      /// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
      /// \param wayPointsEns   : an iterator pointing to the last  element of a pair(position, derivative) container.
      /// \param time_control_points : vector containing time for each waypoint.
      ///
      template<typename In>
      cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t & time_control_points)
        : size_(std::distance(PairsBegin, PairsEnd)), degree_(3)
      {
JasonChmn's avatar
JasonChmn committed
60
        // Check size of pairs container.
JasonChmn's avatar
JasonChmn committed
61
        if(Safe && size_ < 1)
62
        {
63
          throw std::length_error("can not create cubic_hermite_spline, number of pairs is inferior to 2.");
64
65
66
67
68
        }
        // Push all pairs in controlPoints
        In it(PairsBegin);
        for(; it != PairsEnd; ++it)
        {
69
          control_points_.push_back(*it);
70
        }
71
        setTime(time_control_points);
72
73
      }

74

75
      cubic_hermite_spline(const cubic_hermite_spline& other)
JasonChmn's avatar
JasonChmn committed
76
        : control_points_(other.control_points_), time_control_points_(other.time_control_points_),
77
          duration_splines_(other.duration_splines_), T_min_(other.T_min_), T_max_(other.T_max_), 
JasonChmn's avatar
JasonChmn committed
78
          size_(other.size_), degree_(other.degree_)
79
      {}
80

81

82
83
84
85
86
87
88
89
90
91
92
      /// \brief Destructor.
      virtual ~cubic_hermite_spline(){}

      /*Operations*/
      public:
      ///  \brief Evaluation of the cubic hermite spline at time t.
      ///  \param t : time when to evaluate the spline.
      ///  \return \f$p(t)\f$ point corresponding on spline at time t.
      ///
      virtual Point operator()(const Time t) const
      {
93
        if(Safe &! (T_min_ <= t && t <= T_max_))
94
        {
95
          throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
96
97
98
        }
        if (size_ == 1)
        {
99
          return control_points_.front().first;
100
101
102
        }
        else
        {
103
          return evalCubicHermiteSpline(t, 0);
104
        }
105
106
107
108
109
110
111
112
113
      }

      ///  \brief Evaluate the derivative of order N of spline at time t.
      ///  \param t : time when to evaluate the spline.
      ///  \param order : order of derivative.
      ///  \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
      ///
      virtual Point derivate(const Time t, const std::size_t order) const
      {   
114
        return evalCubicHermiteSpline(t, order);
115
116
117
118
119
120
121
122
123
      }

      /// \brief Set time of each control point of cubic hermite spline.
      /// Set duration of each spline, Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with 
      /// values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.<br>
      /// \param time_control_points : Vector containing time for each control point.
      ///
      void setTime(const vector_time_t & time_control_points)
      {
124
        time_control_points_ = time_control_points;
125
126
        T_min_ = time_control_points_.front();
        T_max_ = time_control_points_.back();
127
        if (time_control_points.size() != size())
128
        {
129
          throw std::length_error("size of time control points should be equal to number of control points");
130
        }
131
132
        computeDurationSplines();
        if (!checkDurationSplines())
133
        {
134
          throw std::invalid_argument("time_splines not monotonous, all spline duration should be superior to 0");
135
        }
136
      }
137

138
139
140
141
142
      /// \brief Get vector of pair (positition, derivative) corresponding to control points.
      /// \return vector containing control points.
      ///
      t_pair_point_tangent_t getControlPoints()
      {
143
        return control_points_;
144
      }
145

146
147
148
149
150
      /// \brief Get vector of Time corresponding to Time for each control point.
      /// \return vector containing time of each control point.
      ///
      vector_time_t getTime()
      {
151
        return time_control_points_;
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
      }

      /// \brief Get number of control points contained in the trajectory.
      /// \return number of control points.
      ///
      std::size_t size() const { return size_; }

      /// \brief Get number of intervals (subsplines) contained in the trajectory.
      /// \return number of intervals (subsplines).
      ///
      std::size_t numIntervals() const { return size()-1; }


      /// \brief Evaluate value of cubic hermite spline or its derivate at specified order at time \f$t\f$.
      /// A cubic hermite spline on unit interval \f$[0,1]\f$ and given two control points defined by 
      /// their position and derivative \f$\{p_0,m_0\}\f$ and \f$\{p_1,m_1\}\f$, is defined by the polynom : <br>
      ///     \f$p(t)=h_{00}(t)P_0 + h_{10}(t)m_0 + h_{01}(t)p_1 + h_{11}(t)m_1\f$<br>
      /// To extend this formula to a cubic hermite spline on any arbitrary interval, 
      /// we define \f$\alpha=(t-t_0)/(t_1-t_0) \in [0, 1]\f$ where \f$t \in [t_0, t_1]\f$.<br>
      /// Polynom \f$p(t) \in [t_0, t_1]\f$ becomes \f$p(\alpha) \in [0, 1]\f$
      /// and \f$p(\alpha) = p((t-t_0)/(t_1-t_0))\f$.
      /// \param t : time when to evaluate the curve.
      /// \param degree_derivative : Order of derivate of cubic hermite spline (set value to 0 if you do not want derivate)
      /// \return point corresponding \f$p(t)\f$ on spline at time t or its derivate order N \f$\frac{d^Np(t)}{dt^N}\f$.
      ///
      Point evalCubicHermiteSpline(const Numeric t, std::size_t degree_derivative) const
      {
JasonChmn's avatar
JasonChmn committed
179
        const std::size_t id = findInterval(t);
180
181
182
        // ID is on the last control point
        if(id == size_-1)
        {
183
184
185
186
187
188
189
190
191
192
193
194
          if (degree_derivative==0)
          {
            return control_points_.back().first;
          }
          else if (degree_derivative==1)
          {
            return control_points_.back().second;
          }
          else
          {
            return control_points_.back().first*0.; // To modify, create a new Tangent ininitialized with 0.
          }
195
        }
196
197
        const pair_point_tangent_t Pair0 = control_points_.at(id);
        const pair_point_tangent_t Pair1 = control_points_.at(id+1);
198
199
        const Time & t0 = time_control_points_[id];
        const Time & t1 = time_control_points_[id+1]; 
200
201
202
203
204
205
206
        // Polynom for a cubic hermite spline defined on [0., 1.] is : 
        //      p(t) = h00(t)*p0 + h10(t)*m0 + h01(t)*p1 + h11(t)*m1 with t in [0., 1.]
        //
        // For a cubic hermite spline defined on [t0, t1], we define alpha=(t-t0)/(t1-t0) in [0., 1.].
        // Polynom p(t) defined on [t0, t1] becomes p(alpha) defined on [0., 1.]
        //      p(alpha) = p((t-t0)/(t1-t0))
        //
207
208
        const Time dt = (t1-t0);
        const Time alpha = (t - t0)/dt;
209
        assert(0. <= alpha && alpha <= 1. && "alpha must be in [0,1]");
210
        Numeric h00, h10, h01, h11;
JasonChmn's avatar
JasonChmn committed
211
        evalCoeffs(alpha,h00,h10,h01,h11,degree_derivative);
212
213
        //std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<" h11="<<h11<<std::endl;
        Point p_ = (h00 * Pair0.first + h10 * dt * Pair0.second + h01 * Pair1.first + h11 * dt * Pair1.second);
JasonChmn's avatar
JasonChmn committed
214
215
        // if derivative, divide by dt^degree_derivative
        for (std::size_t i=0; i<degree_derivative; i++)
216
        {
217
          p_ /= dt;
218
        }
219
        return p_;
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
      }

      /// \brief Evaluate coefficient for polynom of cubic hermite spline.
      /// Coefficients of polynom :<br>
      ///  - \f$h00(t)=2t^3-3t^2+1\f$;
      ///  - \f$h10(t)=t^3-2t^2+t\f$;
      ///  - \f$h01(t)=-2t^3+3t^2\f$;
      ///  - \f$h11(t)=t^3-t^2\f$.<br>
      /// From it, we can calculate their derivate order N : 
      /// \f$\frac{d^Nh00(t)}{dt^N}\f$, \f$\frac{d^Nh10(t)}{dt^N}\f$,\f$\frac{d^Nh01(t)}{dt^N}\f$, \f$\frac{d^Nh11(t)}{dt^N}\f$.
      /// \param t : time to calculate coefficients.
      /// \param h00 : variable to store value of coefficient.
      /// \param h10 : variable to store value of coefficient.
      /// \param h01 : variable to store value of coefficient.
      /// \param h11 : variable to store value of coefficient.
      /// \param degree_derivative : order of derivative.
      ///
      static void evalCoeffs(const Numeric t, Numeric & h00, Numeric & h10, Numeric & h01, Numeric & h11, std::size_t degree_derivative)
      {
239
240
        Numeric t_square = t*t;
        Numeric t_cube = t_square*t;
JasonChmn's avatar
JasonChmn committed
241
        if (degree_derivative==0)
242
        {
243
244
245
246
          h00 =  2*t_cube     -3*t_square     +1.;
          h10 =  t_cube       -2*t_square     +t;
          h01 = -2*t_cube     +3*t_square;
          h11 =  t_cube       -t_square;
247
        }
JasonChmn's avatar
JasonChmn committed
248
        else if (degree_derivative==1)
249
        {
250
251
252
253
          h00 =  6*t_square   -6*t;
          h10 =  3*t_square   -4*t    +1.;
          h01 = -6*t_square   +6*t;
          h11 =  3*t_square   -2*t;
254
        }
JasonChmn's avatar
JasonChmn committed
255
        else if (degree_derivative==2)
256
        {
257
258
259
260
          h00 =  12*t     -6.;
          h10 =  6*t      -4.;  
          h01 = -12*t     +6.;
          h11 =  6*t      -2.;
261
        }
JasonChmn's avatar
JasonChmn committed
262
        else if (degree_derivative==3)
263
        {
264
265
266
267
          h00 =  12.;
          h10 =  6.;  
          h01 = -12.;
        h11 =  6.;
268
269
270
        }
        else 
        {
271
272
273
274
          h00 =  0.;
          h10 =  0.;  
          h01 =  0.;
          h11 =  0.;
275
        }
276
      }
277

278
    private:
279
280
281
282
283
284
      /// \brief Get index of the interval (subspline) corresponding to time t for the interpolation.
      /// \param t : time where to look for interval.
      /// \return Index of interval for time t.
      ///
      std::size_t findInterval(const Numeric t) const
      {
285
286
287
        // time before first control point time.
        if(t < time_control_points_[0])
        {
288
          return 0;
289
290
291
292
        }
        // time is after last control point time
        if(t > time_control_points_[size_-1])
        {
293
          return size_-1;
294
295
        }

JasonChmn's avatar
JasonChmn committed
296
297
        std::size_t left_id = 0;
        std::size_t right_id = size_-1;
298
299
        while(left_id <= right_id)
        {
300
301
302
303
304
305
306
307
308
309
310
311
312
          const std::size_t middle_id = left_id + (right_id - left_id)/2;
          if(time_control_points_.at(middle_id) < t)
          {
            left_id = middle_id+1;
          }
          else if(time_control_points_.at(middle_id) > t)
          {
            right_id = middle_id-1;
          }
          else
          {
            return middle_id;
          }
313
314
        }
        return left_id-1;
315
      }
316

317
318
319
320
321
      /// \brief compute duration of each spline.
      /// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
      /// Duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} ).
      ///
      void computeDurationSplines() {
322
323
324
        duration_splines_.clear();
        Time actual_time;
        Time prev_time = *(time_control_points_.begin());
JasonChmn's avatar
JasonChmn committed
325
        std::size_t i = 0;
326
327
        for (i=0; i<size()-1; i++)
        {
328
329
330
          actual_time = time_control_points_.at(i+1);
          duration_splines_.push_back(actual_time-prev_time);
          prev_time = actual_time;
331
        }
332
      }
333

334
335
336
337
338
      /// \brief Check if duration of each subspline is strictly positive.
      /// \return true if all duration of strictly positive, false otherwise.
      ///
      bool checkDurationSplines() const
      {
JasonChmn's avatar
JasonChmn committed
339
        std::size_t i = 0;
340
341
342
        bool is_positive = true;
        while (is_positive && i<duration_splines_.size())
        {
343
344
          is_positive = (duration_splines_.at(i)>0.);
          i++;
345
346
        }
        return is_positive;
347
348
      }
      /*Operations*/  
349
350

    /*Helpers*/
JasonChmn's avatar
JasonChmn committed
351
    public:
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
      /// \brief Get the minimum time for which the curve is defined
      /// \return \f$t_{min}\f$, lower bound of time range.
      Time virtual min() const{return time_control_points_.front();}
      /// \brief Get the maximum time for which the curve is defined.
      /// \return \f$t_{max}\f$, upper bound of time range.
      Time virtual max() const{return time_control_points_.back();}
      /*Helpers*/

      /*Attributes*/
      /// Vector of pair < Point, Tangent >.
      t_pair_point_tangent_t control_points_;
      /// Vector of Time corresponding to time of each N control points : time at \f$P_0, P_1, P_2, ..., P_N\f$.
      /// Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.
      vector_time_t time_control_points_;

      /// Vector of Time corresponding to time duration of each subspline.<br>
      /// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
      /// duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} )<br>
      /// It contains \f$N-1\f$ durations. 
      vector_time_t duration_splines_;
      /// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
      /*const*/ Time T_min_;
      /// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
      /*const*/ Time T_max_;
      /// Number of control points (pairs).
      std::size_t size_;
      /// Degree (Cubic so degree 3)
      std::size_t degree_;
      /*Attributes*/

      // Serialization of the class
      friend class boost::serialization::access;

      template<class Archive>
      void serialize(Archive& ar, const unsigned int version){
387
        if (version) {
388
          // Do something depending on version ?
389
        }
390
391
392
393
394
395
396
        ar & boost::serialization::make_nvp("control_points", control_points_);
        ar & boost::serialization::make_nvp("time_control_points", time_control_points_);
        ar & boost::serialization::make_nvp("duration_splines", duration_splines_);
        ar & boost::serialization::make_nvp("T_min", T_min_);
        ar & boost::serialization::make_nvp("T_max", T_max_);
        ar & boost::serialization::make_nvp("size", size_);
        ar & boost::serialization::make_nvp("degree", degree_);
397
398
      }
  }; // End struct Cubic hermite spline
399
400
} // namespace curve
#endif //_CLASS_CUBICHERMITESPLINE