Skip to content
Snippets Groups Projects
Commit 7d356718 authored by JasonChmn's avatar JasonChmn
Browse files

Documentation cubic hermite spline done

parent 90861652
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/curves/bezier_curve.h
+ 6
− 5
View file @ 7d356718
......@@ -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.
///
......
include/curves/cubic_hermite_spline.h
+ 150
− 114
View file @ 7d356718
......@@ -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*/
......
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