Commit d044515a authored by JasonChmn's avatar JasonChmn
Browse files

Implementation of cubic hermite spline, start to do unit test

parent b527134b
......@@ -11,6 +11,7 @@ SET(${PROJECT_NAME}_HEADERS
curve_constraint.h
quintic_spline.h
linear_variable.h
cubic_hermite_spline.h
)
INSTALL(FILES
......
#ifndef _CLASS_CUBICHERMITESPLINE
#define _CLASS_CUBICHERMITESPLINE
#include "curve_abc.h"
#include "bernstein.h"
#include "curve_constraint.h"
#include "MathDefs.h"
#include <vector>
#include <stdexcept>
#include <iostream>
namespace curves
{
/// \class CubicHermiteSpline.
/// \brief Represents a set of cubic hermite splines defining a continuous function \f$x(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$.
/// 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$.
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Dim, 1>
, typename Tangent= Eigen::Matrix<Numeric, Dim, 1>
, typename Pair_point_tangent= std::pair<Point, Tangent>
, typename Vector_pair= std::vector< Pair_point_tangent ,Eigen::aligned_allocator<Point> >
>
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_;
/*const*/ 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.
///
template<typename In>
cubic_hermite_spline(In PairsBegin, In PairsEnd)
: T_(1.)
, size_(std::distance(PairsBegin, PairsEnd))
{
// Check size of pairs container.
std::size_t const size(std::distance(PairsBegin, PairsEnd));
if(Safe && size < 1)
{
throw std::length_error("can't create cubic_hermite_spline, number of pairs is inferior to 2.");
}
// Push all pairs in controlPoints
In it(PairsBegin);
for(; it != PairsEnd; ++it)
{
control_points_.push_back(*it);
}
setTimeSplinesDefault();
}
/// \brief Destructor.
virtual ~cubic_hermite_spline(){}
/*Operations*/
public:
virtual Point operator()(const Time t) const
{
if(Safe &! (0 <= t && t <= T_))
{
throw std::out_of_range("can't evaluate bezier curve, out of range");
}
if (size_ == 1)
{
return control_points_.front().first;
}
else
{
return evalCubicHermiteSpline(t, 0);
}
}
/// \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.
///
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.
void setTimeSplines(const Vector_time & time_control_points) const
{
time_control_points_ = time_control_points;
assert(time_control_points.size() == size());
compute_duration_splines();
if (!check_duration_splines())
{
throw std::logic_error("time_splines not monotonous, all spline duration should be superior to 0");
}
}
/// \brief Set duration by default of each spline.
/// Set duration by default (duration = T / Number of control points) for each spline.
///
void setTimeSplinesDefault()
{
time_control_points_.clear();
Time timestep = T_ / (control_points_.size()-1);
Time time = 0.;
Index i = 0;
for (i=0; i<size(); i++)
{
//time_control_points_.push_back(time);
time_control_points_.push_back(time);
time += timestep;
}
compute_duration_splines();
}
/// \brief Returns the index of the interval for the interpolation.
///
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;
}
/*
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
{
// TO DO
const Index id = findInterval(t);
// ID is on the last control point
if(id == size_-1)
{
if (order_derivated==0)
{
return control_points_.back().first;
}
else if (order_derivated==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 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];
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);
//std::cout << "for val t="<<t<<" coef : h00="<<h00<<" h01="<<h01<<" h10="<<h10<<" h11="<<h11<<std::endl;
h10 *= dt; h11 *= dt;
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.
///
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
{
if (duration_splines_.size()>0) {
return false;
}else{
return (duration_splines_.array > 0.).all();
}
}
static void evalCoeffs(const Numeric t, Numeric & h00, Numeric & h10, Numeric & h01, Numeric & h11, std::size_t order_derivated)
{
Numeric t_square = t*t;
Numeric t_cube = t_square*t;
if (order_derivated==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)
{
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)
{
h00 = 12*t -6.;
h10 = 6*t -4.;
h01 = -12*t +6.;
h11 = 6*t -2.;
}
else if (order_derivated==3)
{
h00 = 12.;
h10 = 6.;
h01 = -12.;
h11 = 0.;
}
else
{
h00 = 0.;
h10 = 0.;
h01 = 0.;
h11 = 0.;
}
}
/*Operations*/
/*Helpers*/
public:
/// \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*/
};
} // namespace curve
#endif //_CLASS_CUBICHERMITESPLINE
\ No newline at end of file
......@@ -24,7 +24,8 @@ namespace curves
{
/// \brief Creates coefficient vector of a cubic spline defined on the interval
/// \f$[t_{min}, t_{max}]\f$. It follows the equation :
/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$.
/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$
/// with a, b, c and d the control points.
///
template<typename Point, typename T_Point>
T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const &d)
......
......@@ -6,6 +6,7 @@
#include "curves/helpers/effector_spline.h"
#include "curves/helpers/effector_spline_rotation.h"
#include "curves/bezier_polynom_conversion.h"
#include "curves/cubic_hermite_spline.h"
#include <string>
#include <iostream>
......@@ -16,6 +17,7 @@ using namespace std;
namespace curves
{
typedef Eigen::Vector3d point_t;
typedef Eigen::Vector3d tangent_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef polynom <double, double, 3, true, point_t, t_point_t> polynom_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
......@@ -32,6 +34,9 @@ typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one;
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
typedef cubic_hermite_spline <double, double, 3, true, point_t> cubic_hermite_spline_t;
typedef std::pair<point_t, tangent_t> Pair_point_tangent;
bool QuasiEqual(const double a, const double b, const float margin)
{
if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0))
......@@ -66,7 +71,6 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const
}
/*Cubic Function tests*/
void CubicFunctionTest(bool& error)
{
std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
......@@ -140,7 +144,6 @@ void CubicFunctionTest(bool& error)
}
/*bezier_curve Function tests*/
void BezierCurveTest(bool& error)
{
std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
......@@ -240,7 +243,7 @@ void BezierCurveTest(bool& error)
}
#include <ctime>
void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
void BezierCurveTestCompareHornerAndBernstein(bool&) // error
{
using namespace std;
std::vector<double> values;
......@@ -506,7 +509,6 @@ void ExactCubicNoErrorTest(bool& error)
}
}
/*Exact Cubic Function tests*/
void ExactCubicTwoPointsTest(bool& error)
{
......@@ -544,7 +546,7 @@ void ExactCubicOneDimTest(bool& error)
ComparePoints(one, res1, errmsg, error);
}
void CheckWayPointConstraint(const std::string& errmsg, const double step, const curves::T_Waypoint& /*wayPoints*/, const exact_cubic_t* curve, bool& error )
void CheckWayPointConstraint(const std::string& errmsg, const double step, const curves::T_Waypoint&, const exact_cubic_t* curve, bool& error )
{
point_t res1;
for(double i = 0; i <= 1; i = i + step)
......@@ -794,18 +796,22 @@ void TestReparametrization(bool& error)
}
}
point_t randomPoint(const double min, const double max ){
point_t randomPoint(const double min, const double max )
{
point_t p;
for(size_t i = 0 ; i < 3 ; ++i)
p[i] = (rand()/(double)RAND_MAX ) * (max-min) + min;
return p;
}
void BezierEvalDeCasteljau(bool& error){
void BezierEvalDeCasteljau(bool& error)
{
using namespace std;
std::vector<double> values;
for (int i =0; i < 100000; ++i)
{
values.push_back(rand()/RAND_MAX);
}
//first compare regular evaluation (low dim pol)
point_t a(1,2,3);
......@@ -828,7 +834,8 @@ void BezierEvalDeCasteljau(bool& error){
for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
{
if(cf.evalDeCasteljau(*cit) != cf(*cit)){
if(cf.evalDeCasteljau(*cit) != cf(*cit))
{
error = true;
std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
}
......@@ -845,7 +852,8 @@ void BezierEvalDeCasteljau(bool& error){
for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
{
if(cf.evalDeCasteljau(*cit) != cf(*cit)){
if(cf.evalDeCasteljau(*cit) != cf(*cit))
{
error = true;
std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
}
......@@ -853,22 +861,24 @@ void BezierEvalDeCasteljau(bool& error){
}
/**
* @brief BezierSplitCurve test the 'split' method of bezier curve
* @param error
*/
void BezierSplitCurve(bool& error){
void BezierSplitCurve(bool& error)
{
// test for degree 5
int n = 5;
double t_min = 0.2;
double t_max = 10;
for(size_t i = 0 ; i < 1 ; ++i){
for(size_t i = 0 ; i < 1 ; ++i)
{
// build a random curve and split it at random time :
//std::cout<<"build a random curve"<<std::endl;
point_t a;
std::vector<point_t> wps;
for(size_t j = 0 ; j <= n ; ++j){
for(size_t j = 0 ; j <= n ; ++j)
{
wps.push_back(randomPoint(-10.,10.));
}
double t = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
......@@ -930,11 +940,69 @@ void BezierSplitCurve(bool& error){
}
/* cubic hermite spline function test */
void CubicHermiteTwoPairsPositionVelocityTest(bool& error)
{
std::string errmsg1("in Cubic Hermite 2 points, Error While checking that given wayPoints are crossed (expected / obtained) : ");
std::string errmsg2("in Cubic Hermite 2 points, Error While checking value of point on curve : ");
std::vector< Pair_point_tangent > control_points;
point_t res1;
// 1 Dimension
control_points.clear();
// first point : p0(0.,0.,0.) m0(0.,1.,0.)
control_points.push_back(Pair_point_tangent(point_t(0.,0.,0.),tangent_t(0.5,0.,0.)));
// second point : p1(1.,0.,0.) m1(1.,1.,1.)
control_points.push_back(Pair_point_tangent(point_t(1.,0.,0.),tangent_t(0.1,0.,0.)));
// Create cubic hermite spline
cubic_hermite_spline_t cubic_hermite_spline1D(control_points.begin(), control_points.end());
//Check
res1 = cubic_hermite_spline1D(0); // t=0
ComparePoints(point_t(0.,0.,0.), res1, errmsg1, error);
res1 = cubic_hermite_spline1D(1); // t=1
ComparePoints(point_t(1.,0.,0.), res1, errmsg1, error);
res1 = cubic_hermite_spline1D(0.5); // t=0.5
ComparePoints(point_t(0.55,0.,0.0), res1, errmsg2, error);
// 2 Dimension
control_points.clear();
// first point : p0(0.,0.,0.) m0(0.,1.,0.)
control_points.push_back(Pair_point_tangent(point_t(0.,0.,0.),tangent_t(0.5,0.5,0.)));
// second point : p1(1.,0.,0.) m1(1.,1.,1.)
control_points.push_back(Pair_point_tangent(point_t(1.,1.,0.),tangent_t(0.5,0.1,0.)));
// Create cubic hermite spline
cubic_hermite_spline_t cubic_hermite_spline2D(control_points.begin(), control_points.end());
//Check
res1 = cubic_hermite_spline2D(0); // t=0
ComparePoints(point_t(0.,0.,0.), res1, errmsg1, error);
res1 = cubic_hermite_spline2D(1); // t=1
ComparePoints(point_t(1.,1.,0.), res1, errmsg1, error);
res1 = cubic_hermite_spline2D(0.5); // t=0.5
ComparePoints(point_t(0.5,0.55,0.0), res1, errmsg2, error);
// 2 Dimension
control_points.clear();
// first point : p0(0.,0.,0.) m0(0.,1.,0.)
control_points.push_back(Pair_point_tangent(point_t(0.,0.,0.),tangent_t(0.5,0.5,0.5)));
// second point : p1(1.,0.,0.) m1(1.,1.,1.)
control_points.push_back(Pair_point_tangent(point_t(1.,1.,1.),tangent_t(0.1,0.1,-0.5)));
// Create cubic hermite spline
cubic_hermite_spline_t cubic_hermite_spline3D(control_points.begin(), control_points.end());
//Check
res1 = cubic_hermite_spline3D(0); // t=0
ComparePoints(point_t(0.,0.,0.), res1, errmsg1, error);
res1 = cubic_hermite_spline3D(1); // t=1
ComparePoints(point_t(1.,1.,1.), res1, errmsg1, error);
res1 = cubic_hermite_spline3D(0.5); // t=0.5
ComparePoints(point_t(0.55,0.55,0.625), res1, errmsg2, error);
}
int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "performing tests... \n";
bool error = false;
/*
CubicFunctionTest(error);
ExactCubicNoErrorTest(error);
ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
......@@ -954,6 +1022,8 @@ int main(int /*argc*/, char** /*argv[]*/)
BezierToPolynomConversionTest(error);
BezierEvalDeCasteljau(error);
BezierSplitCurve(error);
*/
CubicHermiteTwoPairsPositionVelocityTest(error);
if(error)
{
std::cout << "There were some errors\n";
......
Supports Markdown
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