diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
index c8f2b64475dbfbb59078654028ab4992c52f17f5..2152fb756bbd09dc049ebed5b442b9076d1f0164 100644
--- a/include/curves/cubic_hermite_spline.h
+++ b/include/curves/cubic_hermite_spline.h
@@ -24,14 +24,14 @@ namespace curves
///
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>
>
struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
- typedef Point Tangent;
- typedef std::pair<Point, Tangent> Pair_point_tangent;
+ typedef std::pair<Point, Tangent> Pair_point_tangent;
typedef std::vector< Pair_point_tangent ,Eigen::aligned_allocator<Point> > Vector_pair;
- typedef int Index;
typedef std::vector<Time> Vector_time;
+ typedef int Index;
/*Attributes*/
public:
@@ -40,6 +40,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// Vector of Time corresponding to time of each N control points : time at \f$P_0, P_1, P_2, ..., P_N\f$.
/// Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.
Vector_time time_control_points_;
+
private:
/// Vector of Time corresponding to time duration of each subspline.<br>
/// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
@@ -56,17 +57,15 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
public:
/// \brief Constructor.
- /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint pair(position, derivative) container.
- /// \param wayPointsEns : an iterator pointing to the last element of a waypoint pair(position, derivative) container.
+ /// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
+ /// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
///
template<typename In>
- cubic_hermite_spline(In PairsBegin, In PairsEnd)
- : T_min_(0.)
- , T_max_(1.)
- , size_(std::distance(PairsBegin, PairsEnd))
+ cubic_hermite_spline(In PairsBegin, In PairsEnd, const Vector_time & time_control_points)
{
// Check size of pairs container.
std::size_t const size(std::distance(PairsBegin, PairsEnd));
+ size_ = size;
if(Safe && size < 1)
{
throw std::length_error("can not create cubic_hermite_spline, number of pairs is inferior to 2.");
@@ -77,7 +76,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
control_points_.push_back(*it);
}
- setTimeSplinesDefault();
+ setTimeSplines(time_control_points);
}
/// \brief Destructor.
@@ -118,26 +117,25 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Set time of each control point of cubic hermite spline.
/// Set duration of each spline, Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with
/// values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.<br>
- /// If not set, time of control points is set by default. See setTimeSplinesDefault().
/// \param time_control_points : Vector containing time for each control point.
///
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();
- if (time_control_points.size() != size())
+ if (time_control_points.size() != size())
{
throw std::length_error("size of time control points should be equal to number of control points");
}
- compute_duration_splines();
- if (!check_duration_splines())
+ computeDurationSplines();
+ if (!checkDurationSplines())
{
throw std::logic_error("time_splines not monotonous, all spline duration should be superior to 0");
}
- }
+ }
- /// \brief Set duration by default of each spline (called in constructor).
+ /// \brief Set duration by default of each spline.
/// Set a linear time from 0 to 1 for each control point with a \f$step=1.0/N\f$
/// where \f$N\f$ is the number of control points.<br>
/// Exemple for 5 control points : vector time_control_points_ will contain \f$(0., 0.25, 0.5, 0.75, 1.0)\f$
@@ -157,7 +155,23 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
time_control_points_.push_back(time);
time += timestep;
}
- compute_duration_splines();
+ computeDurationSplines();
+ }
+
+ /// \brief Get vector of pair (positition, derivative) corresponding to control points.
+ /// \return vector containing control points.
+ ///
+ Vector_pair getControlPoints()
+ {
+ return control_points_;
+ }
+
+ /// \brief Get vector of Time corresponding to Time for each control point.
+ /// \return vector containing time of each control point.
+ ///
+ Vector_time getTimeSplines()
+ {
+ return time_control_points_;
}
/// \brief Get number of control points contained in the trajectory.
@@ -322,7 +336,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
/// Duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} ).
///
- void compute_duration_splines() {
+ void computeDurationSplines() {
duration_splines_.clear();
Time actual_time;
Time prev_time = *(time_control_points_.begin());
@@ -338,7 +352,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Check if duration of each subspline is strictly positive.
/// \return true if all duration of strictly positive, false otherwise.
///
- bool check_duration_splines() const
+ bool checkDurationSplines() const
{
Index i = 0;
bool is_positive = true;
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 8ddce6f4eede912d787f976f1ae97f3855177a28..b0c73feb3ea10f2ff584aa6cd9cd510409c6fc1e 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -138,14 +138,12 @@ t_pair_point_tangent_t getPairsPointTangent(const point_list_t& points, const po
cubic_hermite_spline_t* wrapCubicHermiteSplineConstructor(const point_list_t& points, const point_list_t& tangents, const time_waypoints_t& time_pts)
{
t_pair_point_tangent_t ppt = getPairsPointTangent(points, tangents);
- cubic_hermite_spline_t* chs = new cubic_hermite_spline_t(ppt.begin(), ppt.end());
std::vector<real> time_control_pts;
for( int i =0; i<time_pts.size(); ++i)
{
time_control_pts.push_back(time_pts[i]);
}
- chs->setTimeSplines(time_control_pts);
- return chs;
+ return new cubic_hermite_spline_t(ppt.begin(), ppt.end(), time_control_pts);
}
/* End wrap Cubic hermite spline */
diff --git a/tests/Main.cpp b/tests/Main.cpp
index 84be398f267287ffab220bacc50d1db83a6193e4..1363670a0d82d383e6307eacecd963b67ade2ca7 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -964,25 +964,25 @@ void CubicHermitePairsPositionDerivativeTest(bool& error)
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
+ std::vector< double > time_control_points;
// Two pairs
control_points.clear();
- // first point : p0(0.,0.,0.) m0(0.,1.,0.)
control_points.push_back(Pair_point_tangent(P0,T0));
- // second point : p1(1.,0.,0.) m1(1.,1.,1.)
+ time_control_points.push_back(0.); // Time at P0
control_points.push_back(Pair_point_tangent(P1,T1));
+ time_control_points.push_back(1.); // Time at P1
// Create cubic hermite spline
- cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end());
+ cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end(), time_control_points);
+ cubic_hermite_spline_1Pair.setTimeSplinesDefault();
//Check
- res1 = cubic_hermite_spline_1Pair(0.); // t=0
+ res1 = cubic_hermite_spline_1Pair(0.); // t=0
ComparePoints(P0, res1, errmsg1, error);
- res1 = cubic_hermite_spline_1Pair(1.); // t=1
+ res1 = cubic_hermite_spline_1Pair(1.); // t=1
ComparePoints(P1, res1, errmsg1, error);
- res1 = cubic_hermite_spline_1Pair(0.5); // t=0.5
+ 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
+ // Test derivative : two pairs
res1 = cubic_hermite_spline_1Pair.derivate(0.,1);
ComparePoints(T0, res1, errmsg3, error);
res1 = cubic_hermite_spline_1Pair.derivate(0.5,1);
@@ -992,47 +992,42 @@ void CubicHermitePairsPositionDerivativeTest(bool& 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());
+ time_control_points.clear();
+ time_control_points.push_back(0.); // Time at P0
+ time_control_points.push_back(2.); // Time at P1
+ time_control_points.push_back(5.); // Time at P1
+ cubic_hermite_spline_t cubic_hermite_spline_2Pairs(control_points.begin(), control_points.end(), time_control_points);
//Check
- res1 = cubic_hermite_spline_2Pairs(0.); // t=0
+ 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
+ res1 = cubic_hermite_spline_2Pairs(2.); // t=2
ComparePoints(P1, res1, errmsg2, error);
- res1 = cubic_hermite_spline_2Pairs(0.25); // t=0.25 , same than in two pairs at t=0.5
+ res1 = cubic_hermite_spline_2Pairs(5.); // t=5
+ ComparePoints(P2, res1, errmsg1, error);
+ res1 = cubic_hermite_spline_2Pairs(1.); // t=1.0 , 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
+ // Test derivative : three pairs
res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
ComparePoints(T0, res1, errmsg3, error);
- res1 = cubic_hermite_spline_2Pairs.derivate(0.5,1);
+ res1 = cubic_hermite_spline_2Pairs.derivate(2.,1);
ComparePoints(T1, res1, errmsg3, error);
- res1 = cubic_hermite_spline_2Pairs.derivate(1.,1);
+ res1 = cubic_hermite_spline_2Pairs.derivate(5.,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
+ // 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
+ cubic_hermite_spline_2Pairs.setTimeSplinesDefault();
+ res1 = cubic_hermite_spline_2Pairs(0.); // t=0
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);
+ res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
+ ComparePoints(P1, res1, errmsg2, error);
+ res1 = cubic_hermite_spline_2Pairs(1.); // t=1
+ ComparePoints(P2, res1, errmsg1, 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(6.,1);
+ res1 = cubic_hermite_spline_2Pairs.derivate(0.5,1);
ComparePoints(T1, res1, errmsg3, error);
- res1 = cubic_hermite_spline_2Pairs.derivate(10.,1);
+ res1 = cubic_hermite_spline_2Pairs.derivate(1.,1);
ComparePoints(T2, res1, errmsg3, error);
}