diff --git a/include/spline/cubic_zero_vel_acc.h b/include/spline/cubic_zero_vel_acc.h
index 57026c940ece0e6f2ad134cf3d5321a23d667773..e35168d93ce221d71f5cb0a5b9d8de1c35c101da 100644
--- a/include/spline/cubic_zero_vel_acc.h
+++ b/include/spline/cubic_zero_vel_acc.h
@@ -78,8 +78,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
In it(wayPointsBegin), next(wayPointsBegin);
- ++next;
- Numeric t_previous((*it).first);
+ ++next;
for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
{
diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index 0376292991d246490a70b0e1634d85f6276992ec..bbe488b62ed392a2c252d9f0e4aea2e4505d94a6 100644
--- a/include/spline/exact_cubic.h
+++ b/include/spline/exact_cubic.h
@@ -45,9 +45,9 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef Time time_t;
typedef Numeric num_t;
typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
- typedef typename std::vector<spline_t*> T_cubic;
- typedef typename T_cubic::iterator IT_cubic;
- typedef typename T_cubic::const_iterator CIT_cubic;
+ typedef typename std::vector<spline_t> t_spline_t;
+ typedef typename t_spline_t::iterator it_spline_t;
+ typedef typename t_spline_t::const_iterator cit_spline_t;
/* Constructors - destructors */
public:
@@ -56,89 +56,92 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
- {
- std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
- if(Safe && size < 1)
- {
- throw; // TODO
- }
-
- // refer to the paper to understand all this.
- MatrixX h1 = MatrixX::Zero(size, size);
- MatrixX h2 = MatrixX::Zero(size, size);
- MatrixX h3 = MatrixX::Zero(size, size);
- MatrixX h4 = MatrixX::Zero(size, size);
- MatrixX h5 = MatrixX::Zero(size, size);
- MatrixX h6 = MatrixX::Zero(size, size);
-
- MatrixX a = MatrixX::Zero(size, Dim);
- MatrixX b = MatrixX::Zero(size, Dim);
- MatrixX c = MatrixX::Zero(size, Dim);
- MatrixX d = MatrixX::Zero(size, Dim);
- MatrixX x = MatrixX::Zero(size, Dim);
-
-
- In it(wayPointsBegin), next(wayPointsBegin);
- ++next;
-
- for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
- {
- num_t const dTi((*next).first - (*it).first);
- num_t const dTi_sqr(dTi * dTi);
- num_t const dTi_cube(dTi_sqr * dTi);
- // filling matrices values
- h3(i,i) = -3 / dTi_sqr;
- h3(i,i+1) = 3 / dTi_sqr;
- h4(i,i) = -2 / dTi;
- h4(i,i+1) = -1 / dTi;
- h5(i,i) = 2 / dTi_cube;
- h5(i,i+1) = -2 / dTi_cube;
- h6(i,i) = 1 / dTi_sqr;
- h6(i,i+1) = 1 / dTi_sqr;
- if( i+2 < size)
- {
- In it2(next); ++ it2;
- num_t const dTi_1((*it2).first - (*next).first);
- num_t const dTi_1sqr(dTi_1 * dTi_1);
- // this can be optimized but let's focus on clarity as long as not needed
- h1(i+1, i) = 2 / dTi;
- h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
- h1(i+1, i+2) = 2 / dTi_1;
- h2(i+1, i) = -6 / dTi_sqr;
- h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
- h2(i+1, i+2) = 6 / dTi_1sqr;
- }
- x.row(i)= (*it).second.transpose();
- }
- // adding last x
- x.row(size-1)= (*it).second.transpose();
- a= x;
- PseudoInverse(h1);
- b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
- c = h3 * x + h4 * b;
- d = h5 * x + h6 * b;
- it= wayPointsBegin, next=wayPointsBegin; ++ next;
- for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
- {
- add_cubic(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first);
- }
- add_cubic(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1),(*it).first, (*it).first);
+ : subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd))
+ {
}
///\brief Destructor
- ~exact_cubic()
+ ~exact_cubic(){}
+
+ protected:
+ template<typename In>
+ t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
{
- for(IT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
+ std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
+ if(Safe && size < 1)
{
- delete(*it);
+ throw; // TODO
}
- }
+ t_spline_t subSplines; subSplines.reserve(size);
+
+ // refer to the paper to understand all this.
+ MatrixX h1 = MatrixX::Zero(size, size);
+ MatrixX h2 = MatrixX::Zero(size, size);
+ MatrixX h3 = MatrixX::Zero(size, size);
+ MatrixX h4 = MatrixX::Zero(size, size);
+ MatrixX h5 = MatrixX::Zero(size, size);
+ MatrixX h6 = MatrixX::Zero(size, size);
+
+ MatrixX a = MatrixX::Zero(size, Dim);
+ MatrixX b = MatrixX::Zero(size, Dim);
+ MatrixX c = MatrixX::Zero(size, Dim);
+ MatrixX d = MatrixX::Zero(size, Dim);
+ MatrixX x = MatrixX::Zero(size, Dim);
+
- private:
- void add_cubic(point_t const& a, point_t const& b, point_t const& c, point_t const &d, const time_t min, const time_t max)
+ In it(wayPointsBegin), next(wayPointsBegin);
+ ++next;
+
+ for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
+ {
+ num_t const dTi((*next).first - (*it).first);
+ num_t const dTi_sqr(dTi * dTi);
+ num_t const dTi_cube(dTi_sqr * dTi);
+ // filling matrices values
+ h3(i,i) = -3 / dTi_sqr;
+ h3(i,i+1) = 3 / dTi_sqr;
+ h4(i,i) = -2 / dTi;
+ h4(i,i+1) = -1 / dTi;
+ h5(i,i) = 2 / dTi_cube;
+ h5(i,i+1) = -2 / dTi_cube;
+ h6(i,i) = 1 / dTi_sqr;
+ h6(i,i+1) = 1 / dTi_sqr;
+ if( i+2 < size)
+ {
+ In it2(next); ++ it2;
+ num_t const dTi_1((*it2).first - (*next).first);
+ num_t const dTi_1sqr(dTi_1 * dTi_1);
+ // this can be optimized but let's focus on clarity as long as not needed
+ h1(i+1, i) = 2 / dTi;
+ h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
+ h1(i+1, i+2) = 2 / dTi_1;
+ h2(i+1, i) = -6 / dTi_sqr;
+ h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
+ h2(i+1, i+2) = 6 / dTi_1sqr;
+ }
+ x.row(i)= (*it).second.transpose();
+ }
+ // adding last x
+ x.row(size-1)= (*it).second.transpose();
+ a= x;
+ PseudoInverse(h1);
+ b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
+ c = h3 * x + h4 * b;
+ d = h5 * x + h6 * b;
+ it= wayPointsBegin, next=wayPointsBegin; ++ next;
+ for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
+ {
+ add_cubic(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first, subSplines);
+ }
+ add_cubic(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1),(*it).first, (*it).first, subSplines);
+ return subSplines;
+ }
+
+ void add_cubic(point_t const& a, point_t const& b, point_t const& c, point_t const &d,
+ const time_t min, const time_t max, t_spline_t& subSplines) const
{
t_point_t coeffs = make_cubic_vector<point_t, t_point_t>(a,b,c,d);
- subSplines_.push_back(new spline_t(coeffs.begin(),coeffs.end(), min, max));
+ subSplines.push_back(spline_t(coeffs.begin(),coeffs.end(), min, max));
}
private:
@@ -153,13 +156,12 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param return : the value x(t)
virtual point_t operator()(time_t t) const
{
-
- if(Safe && (t < subSplines_.front()->t_min_ || t > subSplines_.back()->t_max_)){throw std::out_of_range("TODO");}
- for(CIT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
+ if(Safe && (t < subSplines_.front().t_min_ || t > subSplines_.back().t_max_)){throw std::out_of_range("TODO");}
+ for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
- if(t >= ((*it)->t_min_) && t <= ((*it)->t_max_))
+ if(t >= (it->t_min_) && t <= (it->t_max_))
{
- return (*it)->operator()(t);
+ return it->operator()(t);
}
}
}
@@ -167,13 +169,13 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/
public:
- num_t virtual min() const{return subSplines_.front()->t_min_;}
- num_t virtual max() const{return subSplines_.back()->t_max_;}
+ num_t virtual min() const{return subSplines_.front().t_min_;}
+ num_t virtual max() const{return subSplines_.back().t_max_;}
/*Helpers*/
/*Attributes*/
- private:
- T_cubic subSplines_;
+ public:
+ const t_spline_t subSplines_;
/*Attributes*/
};
}
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index 88b1544ce41ea0fdd383defc1a7c57c66ae7bf05..d3807ae25cceef3d00394057829548bb7f3acafc 100644
--- a/include/spline/spline_curve.h
+++ b/include/spline/spline_curve.h
@@ -98,7 +98,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
, coefficients_(other.coefficients_) {}
private:
- spline_curve& operator=(const spline_curve& other);
+ //spline_curve& operator=(const spline_curve& other);
/* Constructors - destructors */
@@ -130,10 +130,10 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
- const t_point_t coefficients_;
- const time_t t_min_, t_max_;
- const std::size_t dim_;
- const std::size_t order_;
+ t_point_t coefficients_;
+ time_t t_min_, t_max_;
+ std::size_t dim_;
+ std::size_t order_;
/*Attributes*/
private: