diff --git a/include/spline/cubic_zero_vel_acc.h b/include/spline/exact_cubic_vel_acc_cons.h
similarity index 100%
rename from include/spline/cubic_zero_vel_acc.h
rename to include/spline/exact_cubic_vel_acc_cons.h
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index da5f481b80bea41a7256efb9a00655eae3b7df90..a01f937d105da432ca32add05a4b7f7b1e1b56fb 100644
--- a/include/spline/spline_curve.h
+++ b/include/spline/spline_curve.h
@@ -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
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 6880949829bd9a06ef7c62669ad9cd44819adf15..64cc78b3bde440f3d8151f162591087147f93f30 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -1,6 +1,6 @@
#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"