Commit efe1fbc2 authored by Steve Tonneau's avatar Steve Tonneau

spline coeffs in Eigen matrix + cleaning

parent 844e49b7
......@@ -35,13 +35,27 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
typedef typename t_point_t::const_iterator cit_point_t;
typedef Time time_t;
typedef Numeric num_t;
typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
typedef Eigen::Matrix<double, Dim, Eigen::Dynamic> coeff_t;
typedef Eigen::Ref<coeff_t> coeff_t_ref;
/* Constructors - destructors */
public:
///\brief Constructor
///\param coefficients : a container containing all coefficients of the spline, starting
/// with the zero order coefficient, up to the highest order. Spline order is given
/// by the size of the coefficients
///\param min: LOWER bound on interval definition of the spline
///\param max: UPPER bound on interval definition of the spline
spline_curve(const coeff_t_ref& coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.cols()-1)
{
safe_check();
}
///\brief Constructor
///\param coefficients : a container containing all coefficients of the spline, starting
/// with the zero order coefficient, up to the highest order. Spline order is given
......@@ -50,19 +64,10 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param max: UPPER bound on interval definition of the spline
spline_curve(const T_Point& coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.size()+1)
coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.cols()-1)
{
if(Safe)
{
if(t_min_ > t_max_)
{
std::out_of_range("TODO");
}
if(coefficients_.size() != order_+1)
{
std::runtime_error("Spline order and coefficients do not match");
}
}
safe_check();
}
///\brief Constructor
......@@ -73,20 +78,10 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param max: UPPER bound on interval definition of the spline
template<typename In>
spline_curve(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
:coefficients_(init_coeffs(zeroOrderCoefficient, out)), dim_(Dim), order_(coefficients_.size()+1),
:coefficients_(init_coeffs(zeroOrderCoefficient, out)), dim_(Dim), order_(coefficients_.cols()-1),
t_min_(min), t_max_(max)
{
if(Safe)
{
if(t_min_ > t_max_)
{
std::out_of_range("TODO");
}
if(coefficients_.size() != order_+1)
{
std::runtime_error("Spline order and coefficients do not match");
}
}
safe_check();
}
///\brief Destructor
......@@ -97,11 +92,25 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
spline_curve(const spline_curve& other)
: t_min_(other.t_min_), t_max_(other.t_max_)
, coefficients_(other.coefficients_) {}
: coefficients_(other.coefficients_), dim_(other.dim_), order_(other.order_),
t_min_(other.t_min_), t_max_(other.t_max_){}
private:
//spline_curve& operator=(const spline_curve& other);
void safe_check()
{
if(Safe)
{
if(t_min_ > t_max_)
{
std::out_of_range("TODO");
}
if(coefficients_.size() != order_+1)
{
std::runtime_error("Spline order and coefficients do not match");
}
}
}
/* Constructors - destructors */
......@@ -114,11 +123,10 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
time_t const dt (t-t_min_);
time_t cdt(dt);
cit_point_t cit = coefficients_.begin();
point_t currentPoint_ = *cit; ++cit;
for(; cit != coefficients_.end(); ++cit, cdt*=dt)
currentPoint_ += cdt *(*cit);
time_t cdt(1);
point_t currentPoint_ = point_t::Zero();
for(int i = 0; i < order_+1; ++i, cdt*=dt)
currentPoint_ += cdt *coefficients_.col(i);
return currentPoint_;
}
/*Operations*/
......@@ -133,7 +141,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
t_point_t coefficients_;
coeff_t coefficients_;
std::size_t dim_;
std::size_t order_;
......@@ -143,10 +151,14 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
private:
template<typename In>
t_point_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient)
coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient)
{
t_point_t res(std::distance(zeroOrderCoefficient, highestOrderCoefficient));
std::copy(zeroOrderCoefficient, highestOrderCoefficient, res.begin());
std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
coeff_t res = coeff_t(Dim, size); int i = 0;
for(In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; ++cit, ++i)
{
res.col(i) = *cit;
}
return res;
}
}; //class spline_curve
......
#include "spline/exact_cubic.h"
#include "spline/cubic_zero_vel_acc.h"
#include "spline/exact_cubic_vel_acc_cons.h"
#include "spline/bezier_curve.h"
#include "spline/spline_curve.h"
......
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