Skip to content

GitLab

Commit 6ebf1138 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

no longer using pointers in exact_cubic. had to lose const parameter

parent eb75a2c8
Hide whitespace changes
Inline Side-by-side
include/spline/cubic_zero_vel_acc.h
View file @ 6ebf1138
......@@ -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)
{
......
include/spline/exact_cubic.h
View file @ 6ebf1138
......@@ -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*/
};
}
......
include/spline/spline_curve.h
View file @ 6ebf1138
......@@ -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:
......
Markdown is supported
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