Commit 8880d229 authored by Steve Tonneau's avatar Steve Tonneau

generic spline

parent a0527d5d
/**
* \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
......@@ -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*/
......
/**
* \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
/**
* \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*/
......
......@@ -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;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment