Commit 4c1d45e3 authored by stonneau's avatar stonneau
Browse files

library now just header only (templated)

parent 20b7bd84
...@@ -3,12 +3,10 @@ project(spline) ...@@ -3,12 +3,10 @@ project(spline)
set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/build/") set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/build/")
set(LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib/")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_SOURCE_DIR}/bin/") set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_SOURCE_DIR}/bin/")
find_package(Eigen3 REQUIRED) find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR}) include_directories(${EIGEN3_INCLUDE_DIR})
add_subdirectory (src/spline)
add_subdirectory (src/tests/spline_test) add_subdirectory (src/tests/spline_test)
/** /**
* \file BezierCurve.h * \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3. * \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T. * \author Steve T.
* \version 0.1 * \version 0.1
...@@ -15,51 +15,104 @@ ...@@ -15,51 +15,104 @@
#include "Exports.h" #include "Exports.h"
#include "MathDefs.h" #include "MathDefs.h"
#include <memory>
#include <vector> #include <vector>
namespace spline namespace spline
{ {
/// \class BezierCurve /// \class BezierCurve
/// \brief Represents a curve /// \brief Represents a curve
/// ///
class BezierCurve : public Curve_ABC template<typename Time= double, typename Numeric=Time, int Dim=3, bool Safe=false
{ , typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef Time time_t;
typedef Numeric num_t;
/* Constructors - destructors */ /* Constructors - destructors */
public: public:
///\brief Constructor ///\brief Constructor
///\param points: the points parametering the Bezier curve ///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///\TODO : sor far size above 3 is ignored ///\TODO : so far size above 3 is ignored
SPLINE_API BezierCurve(const T_Vector& /*points*/, const Real minBound=0, const Real maxBound=1); template<typename In>
SPLINE_API bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound=0, const time_t maxBound=1)
: minBound_(minBound)
, maxBound_(maxBound)
, size_(std::distance(PointsBegin, PointsEnd))
{
In it(PointsBegin);
if(Safe && (size_<=1 || minBound == maxBound))
{
throw; // TODO
}
for(; it != PointsEnd; ++it)
{
pts_.push_back(*it);
}
}
///\brief Destructor ///\brief Destructor
SPLINE_API ~BezierCurve(); SPLINE_API ~bezier_curve()
{
// NOTHING
}
private: private:
BezierCurve(const BezierCurve&); bezier_curve(const bezier_curve&);
BezierCurve& operator=(const BezierCurve&); bezier_curve& operator=(const bezier_curve&);
/* Constructors - destructors */ /* Constructors - destructors */
/*Operations*/ /*Operations*/
public:
public: public:
/// \brief Evaluation of the cubic spline at time t. /// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine /// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t) /// \param return : the value x(t)
/// \param return : true if evaluation is successful, false if t is out of range SPLINE_API virtual point_t operator()(time_t t) const
SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const; {
num_t nT = (t - minBound_) / (maxBound_ - minBound_);
if(Safe &! (0 <= nT && nT <= 1))
{
throw; // TODO
}
else
{
num_t dt = (1 - nT);
switch(size_)
{
case 2 :
return pts_[0] * dt + nT * pts_[1];
break;
case 3 :
return pts_[0] * dt * dt
+ 2 * pts_[1] * nT * dt
+ pts_[2] * nT * nT;
break;
default :
return pts_[0] * dt * dt * dt
+ 3 * pts_[1] * nT * dt * dt
+ 3 * pts_[2] * nT * nT * dt
+ pts_[3] * nT * nT *nT;
break;
}
}
}
/*Operations*/ /*Operations*/
SPLINE_API virtual Real MinBound() const; /*Helpers*/
SPLINE_API virtual Real MaxBound() const; SPLINE_API virtual time_t MinBound() const{return minBound_;}
SPLINE_API virtual time_t MaxBound() const{return minBound_;}
/*Helpers*/ /*Helpers*/
public: public:
const int size_; const int size_;
const Real minBound_, maxBound_; const time_t minBound_, maxBound_;
private: private:
const T_Vector pts_; typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Vector;
}; T_Vector pts_;
};
} }
#endif //_CLASS_BEZIERCURVE #endif //_CLASS_BEZIERCURVE
...@@ -5,63 +5,84 @@ ...@@ -5,63 +5,84 @@
* \version 0.1 * \version 0.1
* \date 06/17/2013 * \date 06/17/2013
* *
* This file contains definitions for the CubicFunction class. * This file contains definitions for the CubicFunction struct.
* It allows the creation and evaluation of natural 3D * It allows the creation and evaluation of natural
* smooth cubic splines * smooth cubic splines of arbitrary dimension
*/ */
#ifndef _CLASS_CUBICFUNCTIONIMP #ifndef _STRUCT_CUBICFUNCTION
#define _CLASS_CUBICFUNCTIONIMP #define _STRUCT_CUBICFUNCTION
#include "Exports.h" #include "Exports.h"
#include "MathDefs.h" #include "MathDefs.h"
#include "Curve_ABC.h" #include "Curve_ABC.h"
#include <stdexcept>
namespace spline namespace spline
{ {
class SplineVisitor;
/// \class CubicFunction /// \class CubicFunction
/// \brief Represents a cubic spline defined on the interval /// \brief Represents a cubic spline defined on the interval
/// [tBegin, tEnd]. It follows the equation /// [tBegin, tEnd]. It follows the equation
/// x(t) = a + b(t - tBegin) + c(t - tBegin)^2 + d(t - tBegin)^3 /// x(t) = a + b(t - t_min_) + c(t - t_min_)^2 + d(t - t_min_)^3
/// ///
class CubicFunction : public Curve_ABC template<typename Time= double, typename Numeric=Time, int Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct cubic_function : public curve_abc<Time, Numeric, Dim, Safe, Point>
{ {
typedef Point point_t;
typedef Time time_t;
typedef Numeric num_t;
/* Constructors - destructors */ /* Constructors - destructors */
public: public:
///\brief Constructor ///\brief Constructor
SPLINE_API CubicFunction(const Vector3& /*a*/, const Vector3& /*b*/, const Vector3& /*c*/, const Vector3& /*d*/, const Real /*tBegin*/, const Real /*tEnd*/); SPLINE_API 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)
{
if(t_min_ >= t_max_ & Safe)
{
std::out_of_range("TODO");
}
}
///\brief Destructor ///\brief Destructor
SPLINE_API ~CubicFunction(); SPLINE_API ~cubic_function()
{
// NOTHING
}
private: private:
CubicFunction(const CubicFunction&); //cubic_function(const cubic_function&);
CubicFunction& operator=(const CubicFunction&); //cubic_function& operator=(const cubic_function&);
/* Constructors - destructors */ /* Constructors - destructors */
/*Operations*/ /*Operations*/
public: public:
/// \brief Evaluation of the cubic spline at time t. /// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine /// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t) /// \param return : the value x(t)
SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const; SPLINE_API 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*/ /*Operations*/
/*Helpers*/ /*Helpers*/
public: public:
SPLINE_API Real virtual MinBound() const; SPLINE_API num_t virtual MinBound() const {return t_min_;}
SPLINE_API Real virtual MaxBound() const; SPLINE_API num_t virtual MaxBound() const {return t_max_;}
/*Helpers*/ /*Helpers*/
/*Attributes*/ /*Attributes*/
private: public:
const Vector3 a_, b_, c_ ,d_; const point_t a_, b_, c_ ,d_;
const Real tBegin_, tEnd_; const time_t t_min_, t_max_;
/*Attributes*/ /*Attributes*/
}; //class CubicFunction }; //class CubicFunction
} }
#endif //_CLASS_CUBICFUNCTIONIMP #endif //_STRUCT_CUBICFUNCTION
/** /**
* \file Curve_ABC.h * \file curve_abc.h
* \brief class allowing to create an Exact cubic spline. * \brief interface for a Curve of arbitrary dimension.
* \author Steve T. * \author Steve T.
* \version 0.1 * \version 0.1
* \date 06/17/2013 * \date 06/17/2013
...@@ -9,67 +9,49 @@ ...@@ -9,67 +9,49 @@
*/ */
#ifndef _CLASS_CURVEABC #ifndef _STRUCT_CURVE_ABC
#define _CLASS_CURVEABC #define _STRUCT_CURVE_ABC
#include "SplineVisitor.h"
#include "Exports.h" #include "Exports.h"
#include "MathDefs.h" #include "MathDefs.h"
#include <memory> #include <functional>
#include <vector>
namespace spline namespace spline
{ {
/// \class Curve_ABC /// \struct curve_abc
/// \brief Represents a curve /// \brief Represents a curve of dimension Dim
/// /// is Safe is false, no verification is made on the evaluation of the curve.
class Curve_ABC template<typename Time= double, typename Numeric=Time, int Dim=3, bool Safe=false
{ , typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct curve_abc : std::unary_function<Time, Point>
{
typedef Point point_t;
typedef Time time_t;
/* Constructors - destructors */ /* Constructors - destructors */
public: public:
///\brief Constructor ///\brief Constructor
SPLINE_API Curve_ABC(){}; SPLINE_API curve_abc(){};
///\brief Destructor
SPLINE_API ~Curve_ABC(){};
private: ///\brief Destructor
Curve_ABC(const Curve_ABC&); SPLINE_API ~curve_abc(){};
Curve_ABC& operator=(const Curve_ABC&);
/* Constructors - destructors */ /* Constructors - destructors */
/*Operations*/ /*Operations*/
public: public:
/// \brief Evaluation of the cubic spline at time t. /// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine /// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t) /// \param return : the value x(t)
/// \param return : true if evaluation is successful, false if t is out of range SPLINE_API virtual point_t operator()(time_t t) const = 0;
SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const = 0;
/*Operations*/ /*Operations*/
/*Helpers*/ /*Helpers*/
public: SPLINE_API virtual time_t MinBound() const = 0;
/// \brief Given a timestep dt, returns a set of values for the exact spline SPLINE_API virtual time_t MaxBound() const = 0;
/// separated by this timestep
/// \param visitor : an object called for each timestep in the spline interval.
/// \param dt : the timestep
/// \param result : a reference to the Point set to the x(t)
SPLINE_API void Accept(SplineVisitor& visitor, Real dt) const
{
for(Real ti = MinBound(); ti <= MaxBound(); ti = ti + dt)
{
Vector3 res; Evaluate(ti, res);
visitor.Visit(ti, res);
}
}
SPLINE_API virtual Real MinBound() const = 0;
SPLINE_API virtual Real MaxBound() const = 0;
/*Helpers*/ /*Helpers*/
}; };
} }
#endif //_CLASS_EXACTCUBIC #endif //_STRUCT_CURVE_ABC
/** /**
* \file ExactCubic.h * \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline. * \brief class allowing to create an Exact cubic spline.
* \author Steve T. * \author Steve T.
* \version 0.1 * \version 0.1
...@@ -21,56 +21,157 @@ ...@@ -21,56 +21,157 @@
#define _CLASS_EXACTCUBIC #define _CLASS_EXACTCUBIC
#include "Curve_ABC.h" #include "Curve_ABC.h"
#include "CubicFunction.h"
#include "Exports.h" #include "Exports.h"
#include "MathDefs.h" #include "MathDefs.h"
#include <memory> #include <functional>
#include <vector> #include <vector>
#include <Eigen/StdVector>
#include <iostream>
namespace spline namespace spline
{ {
struct CubicPImpl; // private implementation /// \class ExactCubic
/// \class ExactCubic /// \brief Represents a set of cubic splines defining a continuous function
/// \brief Represents a set of cubic splines defining a continuous function /// crossing each of the waypoint given in its initialization
/// crossing each of the waypoint given in its initialization ///
/// template<typename Time= double, typename Numeric=Time, int Dim=3, bool Safe=false
struct ExactCubic : public Curve_ABC , typename Point= Eigen::Matrix<Numeric, Dim, 1> >
{ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/* Constructors - destructors */ {
typedef Point point_t;
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 typename T_cubic::iterator IT_cubic;
typedef typename T_cubic::const_iterator CIT_cubic;
/* Constructors - destructors */
public: public:
///\brief Constructor ///\brief Constructor
///\param waypoints : a list comprising at least 2 waypoints in ascending time order ///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
SPLINE_API ExactCubic(const T_Waypoint& /*waypoints*/); ///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template<typename In>
SPLINE_API exact_cubic(In wayPointsBegin, In wayPointsEnd)
{
std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
if(Safe && size < 1)
{
throw; // TODO
}
// refer to the paper to understand all this.
MatrixX h1 = MatrixX::Zero(size, size);
MatrixX h2 = MatrixX::Zero(size, size);
MatrixX h3 = MatrixX::Zero(size, size);
MatrixX h4 = MatrixX::Zero(size, size);
MatrixX h5 = MatrixX::Zero(size, size);
MatrixX h6 = MatrixX::Zero(size, size);
///\brief Destructor MatrixX a = MatrixX::Zero(size, Dim);
SPLINE_API ~ExactCubic(); MatrixX b = MatrixX::Zero(size, Dim);
MatrixX c = MatrixX::Zero(size, Dim);
MatrixX d = MatrixX::Zero(size, Dim);
MatrixX x = MatrixX::Zero(size, Dim);
In it(wayPointsBegin), next(wayPointsBegin);
++next;
Numeric t_previous((*it).first);
for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
{
num_t const dTi((*next).first - (*it).first);
num_t const dTi_sqr(dTi * dTi);
num_t const dTi_cube(dTi_sqr * dTi);
// filling matrices values
h3(i,i) = -3 / dTi_sqr;
h3(i,i+1) = 3 / dTi_sqr;
h4(i,i) = -2 / dTi;
h4(i,i+1) = -1 / dTi;
h5(i,i) = 2 / dTi_cube;
h5(i,i+1) = -2 / dTi_cube;
h6(i,i) = 1 / dTi_sqr;
h6(i,i+1) = 1 / dTi_sqr;
if( i+2 < size)
{
In it2(next); ++ it2;
num_t const dTi_1(1/((*it2).first - (*next).first));
num_t const dTi_1sqr(dTi_1 * dTi_1);
// this can be optimized but let's focus on clarity as long as not needed
h1(i+1, i) = 2 / dTi;
h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
h1(i+1, i+2) = 2 / dTi_1;
h2(i+1, i) = -6 / dTi_sqr;
h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
h2(i+1, i+2) = 6 / dTi_1sqr;
}
x.row(i)= (*it).second.transpose();
}
// adding last x
x.row(size-1)= (*it).second.transpose();
a= x;
PseudoInverse(h1);
b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
c = h3 * x + h4 * b;
d = h5 * x + h6 * b;
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_function_t(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
}
///\brief Destructor
SPLINE_API ~exact_cubic()
{
for(IT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
delete(*it);
}
}
private: private:
ExactCubic(const ExactCubic&); exact_cubic(const exact_cubic&);
ExactCubic& operator=(const ExactCubic&); exact_cubic& operator=(const exact_cubic&);
/* Constructors - destructors */ /* Constructors - destructors */
/*Operations*/ /*Operations*/
public: public:
/// \brief Evaluation of the cubic spline at time t. /// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine /// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t) /// \param return : the value x(t)
/// \return : true if evaluation is successful, false if t is out of range SPLINE_API virtual point_t operator()(time_t t) const
SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const; {
/*Operations*/
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_))
{
return (*it)->operator()(t);
}
}
}
/*Operations*/