diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
index c953b53c7f658b1082576ce05aec58fea4fda092..8b7e5c97f0ceb44ded5be0d8753505213d70cc18 100644
--- a/include/curves/cubic_hermite_spline.h
+++ b/include/curves/cubic_hermite_spline.h
@@ -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)
diff --git a/tests/Main.cpp b/tests/Main.cpp
index b1b217e3eb7f7626c99829567acec35bb2e2ce36..d22c63e79a32bc380bb307bd9021b6f9b751aab4 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -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";