Commit 7d356718 authored by JasonChmn's avatar JasonChmn
Browse files

Documentation cubic hermite spline done

parent 90861652
Pipeline #4393 passed with stage
in 2 minutes and 17 seconds
......@@ -232,13 +232,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
/// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
/// For a polynom of degree N expressed by : \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
/// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.
/// To evaluate the position on curve at time t,we can apply the Horner's scheme :
/// \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.
/// Horner's scheme : for a polynom of degree N expressed by :
/// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
/// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$
/// Using Horner's method, the polynom is transformed into :
/// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$
/// where number of additions = N / number of multiplication = N.
/// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
/// We can apply the Horner's scheme : \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$
/// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
/// \param t : unNormalized time
/// \return \f$x(t)\f$, point corresponding on curve at time t.
///
......
......@@ -15,12 +15,12 @@
namespace curves
{
/// \class CubicHermiteSpline.
/// \brief Represents a set of cubic hermite splines defining a continuous function \f$x(t)\f$.
/// \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$.
/// 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 derivate on \f$P_i\f$ and \f$P_{i+1}\f$ are \f$x'(t_{P_i}) = m_i\f$ and \f$x'(t_{P_{i+1}}) = m_{i+1}\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, Dim, 1>
......@@ -30,24 +30,34 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
>
struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef int Index;
typedef std::vector<Time> Vector_time;
/*Attributes*/
public:
Vector_pair control_points_; // Vector of pair < Point, Tangent > , Dim = Size_.
Vector_time time_control_points_; // Time corresponding to each control point, Dim = Size_.
Vector_time duration_splines_; // its length should be duration_splines_[0] = t_PO_P1, duration_splines_[1] = t_P1_P2, Dim = Size_-1.
/*const*/ Time T_min_;
/*const*/ Time T_max_;
/*const*/ std::size_t size_;
/// Vector of pair < Point, Tangent >.
Vector_pair 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 time_control_points_;
private:
/// 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 duration_splines_;
/// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
Time T_min_;
/// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
Time T_max_;
/// Number of control points.
std::size_t size_;
/*Attributes*/
public:
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint pair(position, derivative) container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint pair(position, derivative) container.
///
template<typename In>
cubic_hermite_spline(In PairsBegin, In PairsEnd)
......@@ -75,11 +85,15 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*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
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::out_of_range("can't evaluate bezier curve, out of range");
throw std::out_of_range("can't evaluate cubic hermite spline, out of range");
}
if (size_ == 1)
{
......@@ -94,17 +108,19 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \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^Nx(t)}{dt^N}\f$, point corresponding on derivative spline of order N at time t.
/// \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
{
return evalCubicHermiteSpline(t, order);
}
/// \brief Set duration of each spline.
/// Set duration of each spline, exemple : time_control_points[0] = duration of first spline,
/// time_control_points[0] = duration of second spline (...).
/// \param time_control_points : Vector containing duration of each spline.
/// \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>
/// If not set, time of control points is set by default. See setTimeSplinesDefault().
/// \param time_control_points : Vector containing time for each control point.
///
void setTimeSplines(const Vector_time & time_control_points)
{
time_control_points_ = time_control_points;
......@@ -118,8 +134,10 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
}
/// \brief Set duration by default of each spline.
/// Set duration by default (duration = T / Number of control points) for each spline.
/// \brief Set duration by default of each spline (called in constructor).
/// Set a linear time from 0 to 1 for each control point with a \f$step=1.0/N\f$
/// where \f$N\f$ is the number of control points.<br>
/// Exemple for 5 control points : vector time_control_points_ will contain \f$(0., 0.25, 0.5, 0.75, 1.0)\f$.
///
void setTimeSplinesDefault()
{
......@@ -138,61 +156,40 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
compute_duration_splines();
}
/// \brief Returns the index of the interval for the interpolation.
/// \brief Get number of control points contained in the trajectory.
/// \return Number of control points.
///
Index findInterval(const Numeric t) const
{
// time before first control point time.
if(t < time_control_points_[0])
{
return 0;
}
// time is after last control point time
if(t > time_control_points_[size_-1])
{
return size_-1;
}
Index size() const { return size_; }
Index left_id = 0;
Index right_id = size_-1;
while(left_id <= right_id)
{
const Index 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;
}
}
return left_id-1;
}
/// \brief Get number of intervals (subsplines) contained in the trajectory.
/// \return Number of intervals (subsplines).
///
Index numIntervals() const { return size()-1; }
/*
Vector_pair control_points_; // Vector of pair < Point, Tangent >
Vector_time time_control_points_; // Time corresponding to each control point.
Vector_time duration_splines_;
*/
// See how to evaluate cubic hermite spline
Point evalCubicHermiteSpline(const Numeric t, std::size_t order_derivated) const
/// \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 order_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 order_derivative) const
{
// TO DO
const Index id = findInterval(t);
// ID is on the last control point
if(id == size_-1)
{
if (order_derivated==0)
if (order_derivative==0)
{
return control_points_.back().first;
}
else if (order_derivated==1)
else if (order_derivative==1)
{
return control_points_.back().second;
}
......@@ -201,13 +198,10 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
return control_points_.back().first*0.; // To modify, create a new Tangent ininitialized with 0.
}
}
const Pair_point_tangent Pair0 = control_points_.at(id);
const Pair_point_tangent Pair1 = control_points_.at(id+1);
const Time & t0 = time_control_points_[id];
const Time & t1 = time_control_points_[id+1];
// 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.]
//
......@@ -217,82 +211,55 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
//
const Time dt = (t1-t0);
const Time alpha = (t - t0)/dt;
assert(0. <= alpha <= 1. && "alpha must be in [0,1]");
Numeric h00, h10, h01, h11;
evalCoeffs(alpha,h00,h10,h01,h11,order_derivated);
evalCoeffs(alpha,h00,h10,h01,h11,order_derivative);
//std::cout << "for val t="<<t<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<" h11="<<h11<<std::endl;
Point p_ = (h00 * Pair0.first + h10 * Pair0.second + h01 * Pair1.first + h11 * Pair1.second);
return p_;
}
/// \brief Returns the number of points contained in the trajectory.
/// \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 order_derivative : order of derivative.
///
Index size() const { return size_; }
/// \brief Returns the number of intervals (splines) contained in the trajectory.
///
Index numIntervals() const { return size()-1; }
private:
/// \brief compute duration of each spline, ex : \f$t_{P_0\_P_1}=t_{P_1}-t_{P_0}f$.
void compute_duration_splines() {
duration_splines_.clear();
Time actual_time;
Time prev_time = *(time_control_points_.begin());
Index i = 0;
for (i=0; i<size()-1; i++)
{
actual_time = time_control_points_.at(i+1);
duration_splines_.push_back(actual_time-prev_time);
prev_time = actual_time;
}
}
/// \brief Check if the absicca
bool check_duration_splines() const
{
Index i = 0;
bool is_positive = true;
while (is_positive && i<duration_splines_.size())
{
is_positive = (duration_splines_.at(i)>0.);
i++;
}
return is_positive;
}
static void evalCoeffs(const Numeric t, Numeric & h00, Numeric & h10, Numeric & h01, Numeric & h11, std::size_t order_derivated)
static void evalCoeffs(const Numeric t, Numeric & h00, Numeric & h10, Numeric & h01, Numeric & h11, std::size_t order_derivative)
{
Numeric t_square = t*t;
Numeric t_cube = t_square*t;
if (order_derivated==0)
if (order_derivative==0)
{
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;
}
else if (order_derivated==1)
else if (order_derivative==1)
{
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;
}
else if (order_derivated==2)
else if (order_derivative==2)
{
h00 = 12*t -6.;
h10 = 6*t -4.;
h01 = -12*t +6.;
h11 = 6*t -2.;
}
else if (order_derivated==3)
else if (order_derivative==3)
{
h00 = 12.;
h10 = 6.;
......@@ -308,7 +275,76 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
}
private:
/// \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.
///
Index findInterval(const Numeric t) const
{
// time before first control point time.
if(t < time_control_points_[0])
{
return 0;
}
// time is after last control point time
if(t > time_control_points_[size_-1])
{
return size_-1;
}
Index left_id = 0;
Index right_id = size_-1;
while(left_id <= right_id)
{
const Index 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;
}
}
return left_id-1;
}
/// \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 compute_duration_splines() {
duration_splines_.clear();
Time actual_time;
Time prev_time = *(time_control_points_.begin());
Index i = 0;
for (i=0; i<size()-1; i++)
{
actual_time = time_control_points_.at(i+1);
duration_splines_.push_back(actual_time-prev_time);
prev_time = actual_time;
}
}
/// \brief Check if duration of each subspline is strictly positive.
/// \return true if all duration of strictly positive, false otherwise.
///
bool check_duration_splines() const
{
Index i = 0;
bool is_positive = true;
while (is_positive && i<duration_splines_.size())
{
is_positive = (duration_splines_.at(i)>0.);
i++;
}
return is_positive;
}
/*Operations*/
/*Helpers*/
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment