Commit 90861652 authored by JasonChmn's avatar JasonChmn
Browse files

Fix cubic hermite spline, unit test done and successful

parent d044515a
......@@ -39,7 +39,8 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
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*/ Time T_min_;
/*const*/ Time T_max_;
/*const*/ std::size_t size_;
/*Attributes*/
......@@ -50,7 +51,8 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
template<typename In>
cubic_hermite_spline(In PairsBegin, In PairsEnd)
: T_(1.)
: T_min_(0.)
, T_max_(1.)
, size_(std::distance(PairsBegin, PairsEnd))
{
// Check size of pairs container.
......@@ -75,7 +77,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
public:
virtual Point operator()(const Time t) const
{
if(Safe &! (0 <= t && t <= T_))
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::out_of_range("can't evaluate bezier curve, out of range");
}
......@@ -103,9 +105,11 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// 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
void setTimeSplines(const Vector_time & time_control_points)
{
time_control_points_ = time_control_points;
T_min_ = time_control_points_.front();
T_max_ = time_control_points_.back();
assert(time_control_points.size() == size());
compute_duration_splines();
if (!check_duration_splines())
......@@ -120,7 +124,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
void setTimeSplinesDefault()
{
time_control_points_.clear();
Time timestep = T_ / (control_points_.size()-1);
T_min_ = 0.;
T_max_ = 1.;
Time timestep = (T_max_- T_min_) / (control_points_.size()-1);
Time time = 0.;
Index i = 0;
for (i=0; i<size(); i++)
......@@ -202,6 +208,13 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
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;
......@@ -209,8 +222,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
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;
//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_;
......@@ -245,11 +257,14 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Check if the absicca
bool check_duration_splines() const
{
if (duration_splines_.size()>0) {
return false;
}else{
return (duration_splines_.array > 0.).all();
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)
......
......@@ -941,60 +941,92 @@ void BezierSplitCurve(bool& error)
/* cubic hermite spline function test */
void CubicHermiteTwoPairsPositionVelocityTest(bool& error)
void CubicHermitePairsPositionDerivativeTest(bool& error)
{
std::string errmsg1("in Cubic Hermite 2 points, Error While checking that given wayPoints are crossed (expected / obtained) : ");
std::string errmsg1("in Cubic Hermite 2 pairs (pos,vel), 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::string errmsg3("in Cubic Hermite 2 points, Error While checking value of tangent 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
point_t P0(0.,0.,0.);
point_t P1(1.,2.,3.);
point_t P2(4.,4.,4.);
tangent_t T0(0.5,0.5,0.5);
tangent_t T1(0.1,0.2,-0.5);
tangent_t T2(0.1,0.2,0.3);
// Test time control points by default => with N control points :
// Time at P0= 0. | Time at P1= 1.0/(N-1) | Time at P2= 2.0/(N-1) | ... | Time at P_(N-1)= (N-1)/(N-1)= 1.0
// Two pairs
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.)));
control_points.push_back(Pair_point_tangent(P0,T0));
// 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.)));
control_points.push_back(Pair_point_tangent(P1,T1));
// Create cubic hermite spline
cubic_hermite_spline_t cubic_hermite_spline2D(control_points.begin(), control_points.end());
cubic_hermite_spline_t cubic_hermite_spline_1Pair(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());
res1 = cubic_hermite_spline_1Pair(0.); // t=0
ComparePoints(P0, res1, errmsg1, error);
res1 = cubic_hermite_spline_1Pair(1.); // t=1
ComparePoints(P1, res1, errmsg1, error);
res1 = cubic_hermite_spline_1Pair(0.5); // t=0.5
ComparePoints(point_t(0.55,1.0375,1.625), res1, errmsg2, error);
// Test derivative : two pairs, time default
res1 = cubic_hermite_spline_1Pair.derivate(0.,1);
ComparePoints(T0, res1, errmsg3, error);
res1 = cubic_hermite_spline_1Pair.derivate(0.5,1);
ComparePoints(point_t(1.35,2.825,4.5), res1, errmsg3, error);
res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
ComparePoints(T1, res1, errmsg3, error);
// Three pairs
control_points.push_back(Pair_point_tangent(P2,T2));
cubic_hermite_spline_t cubic_hermite_spline_2Pairs(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);
res1 = cubic_hermite_spline_2Pairs(0.); // t=0
ComparePoints(P0, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(1.); // t=1
ComparePoints(P2, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
ComparePoints(P1, res1, errmsg2, error);
res1 = cubic_hermite_spline_2Pairs(0.25); // t=0.25 , same than in two pairs at t=0.5
ComparePoints(point_t(0.55,1.0375,1.625), res1, errmsg2, error);
// Test derivative : three pairs, time default
res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
ComparePoints(T0, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(0.5,1);
ComparePoints(T1, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(1.,1);
ComparePoints(T2, res1, errmsg3, error);
// Test setting time control points with 3 points :
// Time at P0= t0 | Time at P1= t1 | Time at P2= t2
// Three pairs
std::vector< double > time_control_points;
time_control_points.push_back(5.); // Time at P0
time_control_points.push_back(6.); // Time at P1
time_control_points.push_back(10.); // Time at P2
cubic_hermite_spline_2Pairs.setTimeSplines(time_control_points); // Set time of control points
// check
res1 = cubic_hermite_spline_2Pairs(5.); // Test time at P0
ComparePoints(P0, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(6.); // Test time at P1
ComparePoints(P1, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(10.); // Test time at P2
ComparePoints(P2, res1, errmsg2, error);
// Test derivative : three pairs, time set
res1 = cubic_hermite_spline_2Pairs.derivate(5.,1);
ComparePoints(T0, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(6.,1);
ComparePoints(T1, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(10.,1);
ComparePoints(T2, res1, errmsg3, error);
}
......@@ -1002,7 +1034,6 @@ 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
......@@ -1022,8 +1053,7 @@ int main(int /*argc*/, char** /*argv[]*/)
BezierToPolynomConversionTest(error);
BezierEvalDeCasteljau(error);
BezierSplitCurve(error);
*/
CubicHermiteTwoPairsPositionVelocityTest(error);
CubicHermitePairsPositionDerivativeTest(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