Commit 8065116e authored by JasonChmn's avatar JasonChmn
Browse files

Modified constructor (set time directly in constructor) / add getters for...

Modified constructor (set time directly in constructor) / add getters for control points and their time corresponding
parent c445468c
......@@ -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;
......
......@@ -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 */
......
......@@ -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);
}
......
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