diff --git a/include/spline/cubic_spline.h b/include/spline/cubic_spline.h
index a4ebb794691beb0671238b08d70092b7255f0d91..0e866a62339bcfd2052138b231eba22ab9a9d4fd 100644
--- a/include/spline/cubic_spline.h
+++ b/include/spline/cubic_spline.h
@@ -22,58 +22,17 @@
namespace spline
{
-/// \class CubicFunction
-/// \brief Represents a cubic spline defined on the interval
+/// \brief Creates coefficient vector of a cubic spline defined on the interval
/// [tBegin, tEnd]. It follows the equation
-/// x(t) = a + b(t - t_min_) + c(t - t_min_)^2 + d(t - t_min_)^3
+/// x(t) = a + b(t - t_min_) + c(t - t_min_)^2 + d(t - t_min_)^3
///
-template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
-, typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
-struct cubic_spline : public spline_curve<Time, Numeric, Dim,3, Safe, Point, T_Point>
+template<typename Point, typename T_Point>
+T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const &d)
{
- typedef Point point_t;
- typedef T_Point t_point_t;
- typedef Time time_t;
- typedef Numeric num_t;
- typedef spline_curve<Time, Numeric, Dim,3, Safe, Point, T_Point> spline_curve_t;
-/* Constructors - destructors */
- public:
- ///\brief Constructor
- cubic_spline(point_t const& a, point_t const& b, point_t const& c, point_t const &d, time_t min, time_t max)
- :spline_curve_t(makeVector(a,b,c,d), min, max) {}
-
-
- ///\brief Constructor
- cubic_spline(const T_Point& coefficients, time_t min, time_t max)
- :spline_curve_t(coefficients, min, max) {}
-
-
- ///\brief Constructor
- template<typename In>
- cubic_spline(In zeroOrderCoefficient, In out, time_t min, time_t max)
- :spline_curve_t(zeroOrderCoefficient, out, min, max) {}
-
- ///\brief Destructor
- ~cubic_spline()
- {
- // NOTHING
- }
-
- private:
- //cubic_spline(const cubic_spline&);
- //cubic_spline& operator=(const cubic_spline&);
-/* Constructors - destructors */
-
- /*Operations*/
- T_Point makeVector(point_t const& a, point_t const& b, point_t const& c, point_t const &d)
- {
- T_Point res;
- res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d);
- return res;
- }
-
- /*Operations*/
- }; //class cubic_spline
+ T_Point res;
+ res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d);
+ return res;
+}
}
#endif //_STRUCT_CUBICSPLINE
diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index 9d8701f32e9955927dbc8af9290a92e7c40eab1f..0376292991d246490a70b0e1634d85f6276992ec 100644
--- a/include/spline/exact_cubic.h
+++ b/include/spline/exact_cubic.h
@@ -22,6 +22,7 @@
#include "curve_abc.h"
#include "cubic_spline.h"
+#include "quintic_spline.h"
#include "MathDefs.h"
@@ -35,15 +36,16 @@ namespace spline
/// crossing each of the waypoint given in its initialization
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
-, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
+, typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
+ typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Time time_t;
typedef Numeric num_t;
- typedef cubic_spline<time_t, Numeric, Dim, Safe, Point> cubic_spline_t;
- typedef typename std::vector<cubic_spline_t*> T_cubic;
+ 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;
@@ -77,8 +79,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)
{
@@ -119,9 +120,9 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
it= wayPointsBegin, next=wayPointsBegin; ++ next;
for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
{
- subSplines_.push_back(new cubic_spline_t(a.row(i), b.row(i), c.row(i), d.row(i), (*it).first, (*next).first));
+ add_cubic(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first);
}
- subSplines_.push_back(new cubic_spline_t(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
+ add_cubic(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1),(*it).first, (*it).first);
}
///\brief Destructor
@@ -133,6 +134,13 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
}
+ 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)
+ {
+ 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));
+ }
+
private:
exact_cubic(const exact_cubic&);
exact_cubic& operator=(const exact_cubic&);
diff --git a/include/spline/quintic_spline.h b/include/spline/quintic_spline.h
index cf2b22280c545254c2526c928ef8ec1a75fb56e1..c80a5e760edae8d85f79223d3e435439f84b23b6 100644
--- a/include/spline/quintic_spline.h
+++ b/include/spline/quintic_spline.h
@@ -22,60 +22,19 @@
namespace spline
{
-/// \class quintic_spline
-/// \brief Represents a quintic spline defined on the interval
+/// \brief Creates coefficient vector of a quintic spline defined on the interval
/// [tBegin, tEnd]. It follows the equation
/// x(t) = a + b(t - t_min_) + c(t - t_min_)^2 + d(t - t_min_)^3 + e(t - t_min_)^4 + f(t - t_min_)^5
///
-template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
-, typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
-struct quintic_spline : public spline_curve<Time, Numeric, Dim,5, Safe, Point, T_Point>
+template<typename Point, typename T_Point>
+T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c,
+ Point const &d, Point const& e, Point const& f)
{
- typedef Point point_t;
- typedef T_Point t_point_t;
- typedef Time time_t;
- typedef Numeric num_t;
- typedef spline_curve<Time, Numeric, Dim,5, Safe, Point, T_Point> spline_curve_t;
-/* Constructors - destructors */
- public:
- ///\brief Constructor
- quintic_spline(point_t const& a, point_t const& b, point_t const& c, point_t const &d, point_t const &e, point_t const &f, time_t min, time_t max)
- :spline_curve_t(makeVector(a,b,c,d,e,f), min, max) {}
-
-
- ///\brief Constructor
- quintic_spline(const T_Point& coefficients, time_t min, time_t max)
- :spline_curve_t(coefficients, min, max) {}
-
-
- ///\brief Constructor
- template<typename In>
- quintic_spline(In zeroOrderCoefficient, In out, time_t min, time_t max)
- :spline_curve_t(zeroOrderCoefficient, out, min, max) {}
-
- ///\brief Destructor
- ~quintic_spline()
- {
- // NOTHING
- }
-
- private:
- //quintic_spline(const quintic_spline&);
- quintic_spline& operator=(const quintic_spline&);
-/* Constructors - destructors */
-
- /*Operations*/
- T_Point makeVector(point_t const& a, point_t const& b, point_t const& c,
- point_t const &d, point_t const& e, point_t const& f)
- {
- T_Point res;
- res.push_back(a);res.push_back(b);res.push_back(c);
- res.push_back(d);res.push_back(e);res.push_back(f);
- return res;
- }
-
- /*Operations*/
- }; //class quintic_spline
+ T_Point res;
+ res.push_back(a);res.push_back(b);res.push_back(c);
+ res.push_back(d);res.push_back(e);res.push_back(f);
+ return res;
+}
}
#endif //_STRUCT_QUINTIC_SPLINE
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index a4a6ef38e5df11064cffb51df114e6d53982a11b..38fe60fda57bbf7c334b56eac957f47021db73a4 100644
--- a/include/spline/spline_curve.h
+++ b/include/spline/spline_curve.h
@@ -30,7 +30,7 @@ namespace spline
/// [tBegin, tEnd]. It follows the equation
/// x(t) = a + b(t - t_min_) + ... + d(t - t_min_)^N, where N is the order
///
-template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, std::size_t Order=3, bool Safe=false,
+template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
@@ -43,11 +43,12 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
public:
///\brief Constructor
///\param coefficients : a container containing all coefficients of the spline, starting
- /// with the zero order coefficient, up to the highest order
+ /// 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 T_Point& coefficients, const time_t min, const time_t max)
- :coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(Order)
+ :coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.size()+1)
{
if(Safe)
{
@@ -70,7 +71,7 @@ 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)), t_min_(min), t_max_(max), dim_(Dim), order_(Order)
+ :coefficients_(init_coeffs(zeroOrderCoefficient, out)), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.size()+1)
{
if(Safe)
{
@@ -145,7 +146,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
template<typename In>
t_point_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient)
{
- t_point_t res(Order+1);
+ t_point_t res(std::distance(zeroOrderCoefficient, highestOrderCoefficient));
std::copy(zeroOrderCoefficient, highestOrderCoefficient, res.begin());
return res;
}
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 4263938a8c0ae21a754c7ddc0d051691ad83e2fd..276b03b77fe742e60565e068cb512e1f68cd69ca 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -2,8 +2,6 @@
#include "spline/exact_cubic.h"
#include "spline/bezier_curve.h"
#include "spline/spline_curve.h"
-#include "spline/cubic_spline.h"
-#include "spline/quintic_spline.h"
#include <string>
#include <iostream>
@@ -15,7 +13,7 @@ namespace spline
{
typedef Eigen::Vector3d point_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
-typedef cubic_spline <double, double, 3, true, point_t, t_point_t> cubic_function_t;
+typedef spline_curve <double, double, 3, true, point_t, t_point_t> cubic_function_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
typedef std::pair<double, point_t> Waypoint;
@@ -23,7 +21,7 @@ typedef std::vector<Waypoint> T_Waypoint;
typedef Eigen::Matrix<double,1,1> point_one;
-typedef spline_curve<double, double, 1, 3, true, point_one> cubic_function_one;
+typedef spline_curve<double, double, 1, true, point_one> cubic_function_one;
typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one;
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;