diff --git a/include/spline/cubic_function.h b/include/spline/cubic_spline.h
similarity index 68%
rename from include/spline/cubic_function.h
rename to include/spline/cubic_spline.h
index cee59c3678dd3bcf8a30f685262e26faceab2dd8..a4ebb794691beb0671238b08d70092b7255f0d91 100644
--- a/include/spline/cubic_function.h
+++ b/include/spline/cubic_spline.h
@@ -1,5 +1,5 @@
/**
-* \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
diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index 8c42502e3ae072f3f825033dc05198bcb4bfb75f..9d8701f32e9955927dbc8af9290a92e7c40eab1f 100644
--- a/include/spline/exact_cubic.h
+++ b/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*/
diff --git a/include/spline/quintic_spline.h b/include/spline/quintic_spline.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf2b22280c545254c2526c928ef8ec1a75fb56e1
--- /dev/null
+++ b/include/spline/quintic_spline.h
@@ -0,0 +1,81 @@
+/**
+* \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
+
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index 4eb5ac042697cf7447ad291cc127153607c48e0d..a4a6ef38e5df11064cffb51df114e6d53982a11b 100644
--- a/include/spline/spline_curve.h
+++ b/include/spline/spline_curve.h
@@ -1,13 +1,13 @@
/**
-* \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*/
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index dd4f29bc184b1157c3dc828ea5d7d7fe0687c58f..4263938a8c0ae21a754c7ddc0d051691ad83e2fd 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/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;