Commit 29b56978 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

cubic_hermite: use bezier implementation internally for all computations

parent 63fb484d
......@@ -9,7 +9,8 @@
#define _CLASS_CUBICHERMITESPLINE
#include "curve_abc.h"
#include "curve_constraint.h"
#include "bezier_curve.h"
#include "piecewise_curve.h"
#include "MathDefs.h"
......@@ -41,6 +42,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
typedef Numeric num_t;
typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
typedef cubic_hermite_spline<Time, Numeric, Safe, point_t> cubic_hermite_spline_t;
typedef bezier_curve<Time, Numeric, Safe, point_t> bezier_t;
typedef typename bezier_t::t_point_t t_point_t;
typedef piecewise_curve<Time, Numeric, Safe, point_t, point_t, bezier_t> piecewise_bezier_t;
public:
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
......@@ -99,7 +103,8 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
if (size_ == 1) {
return control_points_.front().first;
} else {
return evalCubicHermiteSpline(t, 0);
const bezier_t bezier = buildCurrentBezier(t);
return bezier(t);
}
}
......@@ -145,7 +150,15 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
///
virtual Point derivate(const time_t t, const std::size_t order) const {
check_conditions();
return evalCubicHermiteSpline(t, order);
if (Safe & !(T_min_ <= t && t <= T_max_)) {
throw std::invalid_argument("can't derivate cubic hermite spline, out of range");
}
if (size_ == 1) {
return control_points_.front().second;
} else {
const bezier_t bezier = buildCurrentBezier(t);
return bezier.derivate(t, order);
}
}
cubic_hermite_spline_t compute_derivate(const std::size_t /*order*/) const {
......@@ -197,91 +210,6 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
///
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 {
const std::size_t id = findInterval(t);
const pair_point_tangent_t Pair0 = control_points_.at(id);
const pair_point_tangent_t 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.]
//
// 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))
//
const Time dt = (t1 - t0);
const Time alpha = (t - t0) / dt;
if (!(0. <= alpha && alpha <= 1.)) {
throw std::runtime_error("alpha must be in [0,1]");
}
Numeric h00, h10, h01, h11;
evalCoeffs(alpha, h00, h10, h01, h11, degree_derivative);
// 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);
// if derivative, divide by dt^degree_derivative
for (std::size_t i = 0; i < degree_derivative; i++) {
p_ /= dt;
}
return p_;
}
/// \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) {
Numeric t_square = t * t;
Numeric t_cube = t_square * t;
if (degree_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 (degree_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 (degree_derivative == 2) {
h00 = 12 * t - 6.;
h10 = 6 * t - 4.;
h01 = -12 * t + 6.;
h11 = 6 * t - 2.;
} else if (degree_derivative == 3) {
h00 = 12.;
h10 = 6.;
h01 = -12.;
h11 = 6.;
} else {
h00 = 0.;
h10 = 0.;
h01 = 0.;
h11 = 0.;
}
}
private:
/// \brief Get index of the interval (subspline) corresponding to time t for the interpolation.
......@@ -312,6 +240,27 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
return left_id - 1;
}
/**
* @brief buildCurrentBezier set up the current_bezier_ attribut to represent the curve of the interval that contain t.
* This bezier is defined by the following control points: p0, p0 + m0/3, p1 - m1/3, p1
* @param t the time for which the bezier is build
* @return the bezier curve
*/
bezier_t buildCurrentBezier(const time_t t) const{
size_t id_interval = findInterval(t);
const pair_point_tangent_t pair0 = control_points_.at(id_interval);
const pair_point_tangent_t pair1 = control_points_.at(id_interval + 1);
const Time& t0 = time_control_points_[id_interval];
const Time& t1 = time_control_points_[id_interval + 1];
t_point_t control_points;
control_points.reserve(4);
control_points.push_back(pair0.first);
control_points.push_back(pair0.first + pair0.second / 3. * (t1 - t0));
control_points.push_back(pair1.first - pair1.second / 3. * (t1 - t0));
control_points.push_back(pair1.first);
return bezier_t(control_points.begin(), control_points.end(), t0, t1);
}
void check_conditions() const {
if (control_points_.size() == 0) {
throw std::runtime_error(
......
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