Unverified Commit 2fcf1903 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #43 from pFernbach/topic/hermite_as_bezier

Use Bezier formulation for cubic hermite
parents c7a9b3db 22719313
......@@ -60,7 +60,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
template <typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min = 0., const time_t T_max = 1.,
const time_t mult_T = 1.)
: T_min_(T_min),
: dim_(PointsBegin->size()),
T_min_(T_min),
T_max_(T_max),
mult_T_(mult_T),
size_(std::distance(PointsBegin, PointsEnd)),
......@@ -74,12 +75,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
throw std::invalid_argument("can't create bezier min bound is higher than max bound");
}
for (; it != PointsEnd; ++it) {
if(Safe && static_cast<size_t>(it->size()) != dim_)
throw std::invalid_argument("All the control points must have the same dimension.");
control_points_.push_back(*it);
}
// set dim
if (control_points_.size() != 0) {
dim_ = PointsBegin->size();
}
}
/// \brief Constructor
......@@ -92,7 +91,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
template <typename In>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T_min = 0.,
const time_t T_max = 1., const time_t mult_T = 1.)
: T_min_(T_min),
: dim_(PointsBegin->size()),
T_min_(T_min),
T_max_(T_max),
mult_T_(mult_T),
size_(std::distance(PointsBegin, PointsEnd) + 4),
......@@ -103,12 +103,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) {
if(Safe && static_cast<size_t>(cit->size()) != dim_)
throw std::invalid_argument("All the control points must have the same dimension.");
control_points_.push_back(*cit);
}
// set dim
if (control_points_.size() != 0) {
dim_ = PointsBegin->size();
}
}
bezier_curve(const bezier_curve& other)
......
......@@ -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"
......@@ -37,9 +38,13 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
typedef std::pair<Point, Point> 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;
typedef Time time_t;
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.
......@@ -58,15 +63,15 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
if (Safe && size_ < 1) {
throw std::length_error("can not create cubic_hermite_spline, number of pairs is inferior to 2.");
}
// Set dimension according to size of points
dim_ = PairsBegin->first.size();
// Push all pairs in controlPoints
In it(PairsBegin);
for (; it != PairsEnd; ++it) {
if(Safe && (static_cast<size_t>(it->first.size()) != dim_ || static_cast<size_t>(it->second.size()) != dim_))
throw std::invalid_argument("All the control points and their derivatives must have the same dimension.");
control_points_.push_back(*it);
}
// Set dimension according to size of points
if (control_points_.size() != 0) {
dim_ = control_points_[0].first.size();
}
// Set time
setTime(time_control_points);
}
......@@ -90,7 +95,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
/// \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 {
virtual Point operator()(const time_t t) const {
check_conditions();
if (Safe & !(T_min_ <= t && t <= T_max_)) {
throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
......@@ -98,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);
}
}
......@@ -142,20 +148,33 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
/// \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 {
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 {
throw std::logic_error("Compute derivate for cubic hermite spline is not implemented yet.");
piecewise_bezier_t compute_derivate(const std::size_t order) const {
piecewise_bezier_t res;
for(size_t i = 0 ; i < size_ - 1 ; ++i){
const bezier_t curve = buildCurrentBezier(time_control_points_[i]);
res.add_curve(curve.compute_derivate(order));
}
return res;
}
/// \brief Compute the derived curve at order N.
/// \param order : order of derivative.
/// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
cubic_hermite_spline_t* compute_derivate_ptr(const std::size_t order) const {
return new cubic_hermite_spline_t(compute_derivate(order));
piecewise_bezier_t* compute_derivate_ptr(const std::size_t order) const {
return new piecewise_bezier_t(compute_derivate(order));
}
/// \brief Set time of each control point of cubic hermite spline.
......@@ -196,117 +215,21 @@ 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);
// ID is on the last control point
if (id == size_ - 1) {
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.
}
}
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.
/// \param t : time where to look for interval.
/// \return Index of interval for time t.
///
std::size_t findInterval(const Numeric t) const {
std::size_t findInterval(const time_t t) const {
// time before first control point time.
if (t < time_control_points_[0]) {
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;
if (t >= time_control_points_[size_ - 1]) {
return size_ - 2;
}
std::size_t left_id = 0;
std::size_t right_id = size_ - 1;
while (left_id <= right_id) {
......@@ -322,6 +245,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(
......
......@@ -600,7 +600,7 @@ class TestCurves(unittest.TestCase):
a.max()
a(0.4)
self.assertTrue((a(0.) == array([1., 2., 3.])).all())
self.assertTrue((a.derivate(0., 1) == array([[2., 2., 2.]]).transpose()).all())
self.assertTrue(isclose(a.derivate(0., 1), array([[2., 2., 2.]]).transpose()).all())
self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
return
......
......@@ -1071,6 +1071,22 @@ void CubicHermitePairsPositionDerivativeTest(bool& error) {
ComparePoints(t1, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(1., 1);
ComparePoints(t2, res1, errmsg3, error);
pointX_t p_derivate, p_compute_derivate;
for(size_t order = 1 ; order < 5 ; ++order){
std::stringstream ss;
ss <<"in Cubic Hermite 2 points, "
"compute_derivate do not lead to the same results as derivate for order = ";
ss << order << std::endl;
curve_ptr_t derivate_ptr(cubic_hermite_spline_2Pairs.compute_derivate_ptr(order));
double t = 0.;
while(t <= 1.){
p_derivate = cubic_hermite_spline_2Pairs.derivate(t, order);
p_compute_derivate = derivate_ptr->operator()(t);
ComparePoints(p_derivate, p_compute_derivate, ss.str(), error);
t += 0.1;
}
}
} catch (...) {
error = true;
std::cout << "Error in CubicHermitePairsPositionDerivativeTest" << std::endl;
......
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