diff --git a/include/spline/cubic_function.h b/include/spline/cubic_function.h
index 6fdc5902d66031e63c1fd734241709885ae497ca..7fd5125e8c6eeba0635feeabe949f847257c05cb 100644
--- a/include/spline/cubic_function.h
+++ b/include/spline/cubic_function.h
@@ -31,59 +31,59 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct cubic_function : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
- typedef Point point_t;
+ typedef Point point_t;
typedef Time time_t;
typedef Numeric num_t;
/* Constructors - destructors */
public:
///\brief Constructor
cubic_function(point_t const& a, point_t const& b, point_t const& c, point_t const &d, time_t min, time_t max)
- :a_(a), b_(b), c_(c), d_(d), t_min_(min), t_max_(max)
+ :a_(a), b_(b), c_(c), d_(d), t_min_(min), t_max_(max)
{
- if(t_min_ > t_max_ && Safe)
- {
- std::out_of_range("TODO");
- }
- }
+ if(t_min_ > t_max_ && Safe)
+ {
+ std::out_of_range("TODO");
+ }
+ }
- ///\brief Destructor
- ~cubic_function()
- {
- // NOTHING
- }
+ ///\brief Destructor
+ ~cubic_function()
+ {
+ // NOTHING
+ }
- private:
- cubic_function(const cubic_function&);
- cubic_function& operator=(const cubic_function&);
+ private:
+ cubic_function(const cubic_function&);
+ cubic_function& operator=(const cubic_function&);
/* Constructors - destructors */
/*Operations*/
- public:
- /// \brief Evaluation of the cubic spline at time t.
- /// \param t : the time when to evaluate the spine
- /// \param return : the value x(t)
- virtual point_t operator()(time_t t) const
- {
- if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
- time_t const dt (t-t_min_);
- return a_+ b_ * dt + c_ * dt*dt + d_ * dt*dt*dt;
- }
+ public:
+ /// \brief Evaluation of the cubic spline at time t.
+ /// \param t : the time when to evaluate the spine
+ /// \param return : the value x(t)
+ virtual point_t operator()(time_t t) const
+ {
+ if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
+ time_t const dt (t-t_min_);
+ return a_+ b_ * dt + c_ * dt*dt + d_ * dt*dt*dt;
+ }
/*Operations*/
-
+
/*Helpers*/
- public:
- /// \brief Returns the minimum time for wich curve is defined
- num_t virtual min() const {return t_min_;}
- /// \brief Returns the maximum time for wich curve is defined
- num_t virtual max() const {return t_max_;}
+ public:
+ /// \brief Returns the minimum time for wich curve is defined
+ num_t virtual min() const {return t_min_;}
+ /// \brief Returns the maximum time for wich curve is defined
+ num_t virtual max() const {return t_max_;}
/*Helpers*/
/*Attributes*/
- public:
- const point_t a_, b_, c_ ,d_;
- const time_t t_min_, t_max_;
+ public:
+ const point_t a_, b_, c_ ,d_;
+ const time_t t_min_, t_max_;
/*Attributes*/
- }; //class CubicFunction
+ }; //class CubicFunction
}
#endif //_STRUCT_CUBICFUNCTION
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
new file mode 100644
index 0000000000000000000000000000000000000000..4eb5ac042697cf7447ad291cc127153607c48e0d
--- /dev/null
+++ b/include/spline/spline_curve.h
@@ -0,0 +1,130 @@
+/**
+* \file cubic_function.h
+* \brief Definition of a cubic spline.
+* \author Steve T.
+* \version 0.1
+* \date 06/17/2013
+*
+* This file contains definitions for the CubicFunction struct.
+* It allows the creation and evaluation of natural
+* smooth cubic splines of arbitrary dimension
+*/
+
+
+#ifndef _STRUCT_SPLINE
+#define _STRUCT_SPLINE
+
+#include "MathDefs.h"
+
+#include "curve_abc.h"
+
+#include <iostream>
+#include <algorithm>
+#include <functional>
+#include <stdexcept>
+
+namespace spline
+{
+/// \class spline_curve
+/// \brief Represents a spline curve of arbitrary order defined on the interval
+/// [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,
+ 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>
+{
+ 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;
+/* 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
+ ///\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, time_t min, time_t max)
+ :coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(Order)
+ {
+ if(t_min_ > t_max_ && Safe)
+ {
+ std::out_of_range("TODO");
+ }
+ }
+
+ ///\brief Constructor
+ ///\param zeroOrderCoefficient : an iterator pointing to the first element of a structure containing the coefficients
+ /// it corresponds to the zero degree coefficient
+ ///\param out : an iterator pointing to the last element of a structure ofcoefficients
+ ///\param min: LOWER bound on interval definition of the spline
+ ///\param max: UPPER bound on interval definition of the spline
+ template<typename In>
+ spline_curve(In zeroOrderCoefficient, In out, time_t min, time_t max)
+ :coefficients_(init_coeffs(zeroOrderCoefficient, out)), t_min_(min), t_max_(max), dim_(Dim), order_(Order)
+ {
+ if(t_min_ > t_max_ && Safe)
+ {
+ std::out_of_range("TODO");
+ }
+ }
+
+ ///\brief Destructor
+ ~spline_curve()
+ {
+ // NOTHING
+ }
+
+ private:
+ spline_curve(const spline_curve&);
+ spline_curve& operator=(const spline_curve&);
+/* Constructors - destructors */
+
+/*Operations*/
+ public:
+ /// \brief Evaluation of the cubic spline at time t.
+ /// \param t : the time when to evaluate the spine
+ /// \param return : the value x(t)
+ virtual point_t operator()(time_t t) const
+ {
+ 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);
+ return currentPoint_;
+ }
+/*Operations*/
+
+/*Helpers*/
+ public:
+ /// \brief Returns the minimum time for wich curve is defined
+ num_t virtual min() const {return t_min_;}
+ /// \brief Returns the maximum time for wich curve is defined
+ num_t virtual max() const {return t_max_;}
+/*Helpers*/
+
+/*Attributes*/
+ public:
+ const t_point_t coefficients_;
+ const time_t t_min_, t_max_;
+ const std::size_t dim_;
+ const std::size_t order_;
+/*Attributes*/
+
+ private:
+ template<typename In>
+ t_point_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient)
+ {
+ t_point_t res(Order+1);
+ std::copy(zeroOrderCoefficient, highestOrderCoefficient, res.begin());
+ return res;
+ }
+}; //class spline_curve
+}
+#endif //_STRUCT_SPLINE
+
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 0a120f686d77d9fbee904cb2ad5943bf05abb696..dd4f29bc184b1157c3dc828ea5d7d7fe0687c58f 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -1,7 +1,7 @@
-#include "spline/cubic_function.h"
#include "spline/exact_cubic.h"
#include "spline/bezier_curve.h"
+#include "spline/spline_curve.h"
#include <string>
#include <iostream>
@@ -12,7 +12,8 @@ using namespace std;
namespace spline
{
typedef Eigen::Vector3d point_t;
-typedef cubic_function<double, double, 3, true, point_t> cubic_function_t;
+typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
+typedef spline_curve <double, double, 3, 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;
@@ -20,7 +21,7 @@ typedef std::vector<Waypoint> T_Waypoint;
typedef Eigen::Matrix<double,1,1> point_one;
-typedef cubic_function<double, double, 1, true, point_one> cubic_function_one;
+typedef spline_curve<double, double, 1, 3, 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;
@@ -59,7 +60,7 @@ void ComparePoints(const point_t& pt1, const point_t& pt2, const std::string& er
if(!(pt1 == pt2))
{
error = true;
- std::cout << errmsg << pt1 << " ; " << pt2 << "\n";
+ std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
}
}
@@ -68,7 +69,7 @@ void ComparePoints(const point_one& pt1, const point_one& pt2, const std::string
if(!(pt1 == pt2))
{
error = true;
- std::cout << errmsg << pt1 << " ; " << pt2 << "\n";
+ std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
}
}
@@ -81,25 +82,33 @@ void CubicFunctionTest(bool& error)
point_t b(2,3,4);
point_t c(3,4,5);
point_t d(3,6,7);
-
- cubic_function_t cf(a, b, c, d, 0, 1);
+ t_point_t vec;
+ vec.push_back(a);
+ vec.push_back(b);
+ vec.push_back(c);
+ vec.push_back(d);
+ cubic_function_t cf(vec.begin(), vec.end(), 0, 1);
point_t res1;
res1 =cf(0);
point_t x0(1,2,3);
- ComparePoints(x0, res1, errMsg + "(0) ", error);
-
- point_t x1(9,15,19);
+ ComparePoints(x0, res1, errMsg + "(0) ", error);
- res1 =cf(1);
- ComparePoints(x1, res1, errMsg + "(1) ", error);
+ point_t x1(9,15,19);
+ res1 =cf(1);
+ ComparePoints(x1, res1, errMsg + "(1) ", error);
point_t x2(3.125,5.25,7.125);
res1 =cf(0.5);
- ComparePoints(x2, res1, errMsg + "(0.5) ", error);
-
- cubic_function_t cf2(a, b, c, d, 0.5, 1);
+ ComparePoints(x2, res1, errMsg + "(0.5) ", error);
+
+ vec.clear();
+ vec.push_back(a);
+ vec.push_back(b);
+ vec.push_back(c);
+ vec.push_back(d);
+ cubic_function_t cf2(vec, 0.5, 1);
res1 = cf2(0.5);
- ComparePoints(x0, res1, errMsg + "x3 ", error);
+ ComparePoints(x0, res1, errMsg + "x3 ", error);
error = true;
try
{
@@ -157,29 +166,29 @@ void BezierCurveTest(bool& error)
point_t res1;
res1 = cf(0);
point_t x20 = a ;
- ComparePoints(x20, res1, errMsg + "2(0) ", error);
+ ComparePoints(x20, res1, errMsg + "2(0) ", error);
point_t x21 = b;
res1 = cf(1);
- ComparePoints(x21, res1, errMsg + "2(1) ", error);
+ ComparePoints(x21, res1, errMsg + "2(1) ", error);
//3d curve
params.push_back(c);
bezier_curve_t cf3(params.begin(), params.end());
res1 = cf3(0);
- ComparePoints(a, res1, errMsg + "3(0) ", error);
+ ComparePoints(a, res1, errMsg + "3(0) ", error);
res1 = cf3(1);
- ComparePoints(c, res1, errMsg + "3(1) ", error);
+ ComparePoints(c, res1, errMsg + "3(1) ", error);
//4d curve
params.push_back(d);
bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2);
res1 = cf4(0.4);
- ComparePoints(a, res1, errMsg + "3(0) ", error);
+ ComparePoints(a, res1, errMsg + "3(0) ", error);
res1 = cf4(2);
- ComparePoints(d, res1, errMsg + "3(1) ", error);
+ ComparePoints(d, res1, errMsg + "3(1) ", error);
try
{