generic spline

include/spline/cubic_function.h → include/spline/cubic_spline.h

 /**
-* \file cubic_function.h
+* \file cubic_spline.h
 * \brief Definition of a cubic spline.
 * \author Steve T.
 * \version 0.1
@@ -11,8 +11,8 @@
 */
 
 
-#ifndef _STRUCT_CUBICFUNCTION
-#define _STRUCT_CUBICFUNCTION
+#ifndef _STRUCT_CUBICSPLINE
+#define _STRUCT_CUBICSPLINE
 
 #include "MathDefs.h"
 
@@ -29,39 +29,39 @@ namespace spline
 ///
 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_function : public spline_curve<Time, Numeric, Dim,3, Safe, Point, T_Point>
+struct cubic_spline : public spline_curve<Time, Numeric, Dim,3, Safe, Point, T_Point>
 {
     typedef Point 	point_t;
     typedef T_Point t_point_t;
-  	typedef Time 	time_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_function(point_t const& a, point_t const& b, point_t const& c, point_t const &d, time_t min, time_t max)
+    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_function(const T_Point& coefficients, time_t min, time_t max)
+    cubic_spline(const T_Point& coefficients, time_t min, time_t max)
         :spline_curve_t(coefficients, min, max) {}
 
 
     ///\brief Constructor
     template<typename In>
-    cubic_function(In zeroOrderCoefficient, In out, time_t min, time_t max)
+    cubic_spline(In zeroOrderCoefficient, In out, time_t min, time_t max)
         :spline_curve_t(zeroOrderCoefficient, out, min, max) {}
 
     ///\brief Destructor
-    ~cubic_function()
+    ~cubic_spline()
     {
         // NOTHING
     }
 
     private:
-    cubic_function(const cubic_function&);
-    cubic_function& operator=(const cubic_function&);
+    //cubic_spline(const cubic_spline&);
+    //cubic_spline& operator=(const cubic_spline&);
 /* Constructors - destructors */
 
     /*Operations*/
@@ -73,7 +73,7 @@ struct cubic_function : public spline_curve<Time, Numeric, Dim,3, Safe, Point, T
     }
 
     /*Operations*/
-    }; //class CubicFunction
+    }; //class cubic_spline
 }
-#endif //_STRUCT_CUBICFUNCTION
+#endif //_STRUCT_CUBICSPLINE
 

include/spline/exact_cubic.h

@@ -21,7 +21,7 @@
 #define _CLASS_EXACTCUBIC
 
 #include "curve_abc.h"
-#include "cubic_function.h"
+#include "cubic_spline.h"
 
 #include "MathDefs.h"
 
@@ -42,8 +42,8 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
 	typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
 	typedef Time 	time_t;
 	typedef Numeric	num_t;
-	typedef cubic_function<time_t, Numeric, Dim, Safe, Point> cubic_function_t;
-	typedef typename std::vector<cubic_function_t*> T_cubic;
+	typedef cubic_spline<time_t, Numeric, Dim, Safe, Point> cubic_spline_t;
+    typedef typename std::vector<cubic_spline_t*> T_cubic;
 	typedef typename T_cubic::iterator IT_cubic;
 	typedef typename T_cubic::const_iterator CIT_cubic;
 
@@ -119,18 +119,18 @@ 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_function_t(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(i), b.row(i), c.row(i), d.row(i), (*it).first, (*next).first));
 		}
-		subSplines_.push_back(new cubic_function_t(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).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));
 	}
 
 	///\brief Destructor
 	~exact_cubic()
-	{
-		for(IT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
-		{
-			delete(*it);
-		}
+    {
+        for(IT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
+        {
+            delete(*it);
+        }
 	}
 
 	private:
@@ -146,12 +146,12 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
 	virtual point_t operator()(time_t t) const
 	{
 
-    	if(Safe && (t < subSplines_.front()->t_min_ || t > subSplines_.back()->t_max_)){throw std::out_of_range("TODO");}
+        if(Safe && (t < subSplines_.front()->t_min_ || t > subSplines_.back()->t_max_)){throw std::out_of_range("TODO");}
 		for(CIT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
 		{
-			if(t >= ((*it)->t_min_) && t <= ((*it)->t_max_))
+            if(t >= ((*it)->t_min_) && t <= ((*it)->t_max_))
 			{
-				return (*it)->operator()(t);
+                return (*it)->operator()(t);
 			}
 		}
 	}
@@ -159,8 +159,8 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
 
 	/*Helpers*/
 	public:
-	num_t virtual min() const{return subSplines_.front()->t_min_;}
-	num_t virtual max() const{return subSplines_.back()->t_max_;}
+    num_t virtual min() const{return subSplines_.front()->t_min_;}
+    num_t virtual max() const{return subSplines_.back()->t_max_;}
 	/*Helpers*/
 
 	/*Attributes*/

include/spline/quintic_spline.h

+/**
+* \file cubic_spline.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_QUINTIC_SPLINE
+#define _STRUCT_QUINTIC_SPLINE
+
+#include "MathDefs.h"
+
+#include "spline_curve.h"
+
+#include <stdexcept>
+
+namespace spline
+{
+/// \class quintic_spline
+/// \brief Represents 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>
+{
+    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
+}
+#endif //_STRUCT_QUINTIC_SPLINE
+

include/spline/spline_curve.h

 /**
-* \file cubic_function.h
+* \file spline_curve.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.
+* This file contains definitions for the spline_curve struct.
 * It allows the creation and evaluation of natural
-* smooth cubic splines of arbitrary dimension
+* smooth splines of arbitrary dimension and order
 */
 
 
@@ -46,12 +46,19 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
     /// 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)
+    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)
     {
-        if(t_min_ > t_max_ && Safe)
+        if(Safe)
         {
-            std::out_of_range("TODO");
+            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");
+            }
         }
     }
 
@@ -62,12 +69,19 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
     ///\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)
+    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)
     {
-        if(t_min_ > t_max_ && Safe)
+        if(Safe)
         {
-            std::out_of_range("TODO");
+            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");
+            }
         }
     }
 
@@ -77,9 +91,20 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
         // NOTHING
     }
 
-    private:
-    spline_curve(const spline_curve&);
-    spline_curve& operator=(const spline_curve&);
+
+    spline_curve(const spline_curve& other)
+        : t_min_(other.t_min_), t_max_(other.t_max_)
+        , coefficients_(other.coefficients_) {}
+
+    //private:
+    spline_curve& operator=(const spline_curve& other)
+    {
+        t_min_ = other.t_min_;
+        t_max_ = other.t_max_;
+        coefficients_ = other.coefficients_;
+        return *this;
+    }
+
 /* Constructors - destructors */
 
 /*Operations*/

src/tests/spline_test/Main.cpp

@@ -2,6 +2,8 @@
 #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>
@@ -13,7 +15,7 @@ namespace spline
 {
 typedef Eigen::Vector3d point_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 cubic_spline  <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;
@@ -66,7 +68,7 @@ void ComparePoints(const point_t& pt1, const point_t& pt2, const std::string& er
 
 void ComparePoints(const point_one& pt1, const point_one& pt2, const std::string& errmsg, bool& error)
 {
-	if(!(pt1 == pt2))
+    if(!(pt1 == pt2))
 	{
 		error = true;
         std::cout << errmsg << pt1 << " ; " << pt2 <<  std::endl;