implemented generic splines. no error

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
 

include/spline/spline_curve.h

+/**
+* \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
+

src/tests/spline_test/Main.cpp

 
-#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
 	{