Skip to content

GitLab

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

library now just header only (templated)

parent 20b7bd84
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 4c1d45e3
......@@ -3,12 +3,10 @@ project(spline)
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/")
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
add_subdirectory (src/spline)
add_subdirectory (src/tests/spline_test)
src/spline/API/BezierCurve.h
View file @ 4c1d45e3
/**
* \file BezierCurve.h
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
......@@ -15,51 +15,104 @@
#include "Exports.h"
#include "MathDefs.h"
#include <memory>
#include <vector>
namespace spline
{
/// \class BezierCurve
/// \brief Represents a curve
///
class BezierCurve : public Curve_ABC
{
/// \class BezierCurve
/// \brief Represents a curve
///
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 */
public:
///\brief Constructor
///\param points: the points parametering the Bezier curve
///\TODO : sor far size above 3 is ignored
SPLINE_API BezierCurve(const T_Vector& /*points*/, const Real minBound=0, const Real maxBound=1);
///\brief Constructor
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///\TODO : so far size above 3 is ignored
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
SPLINE_API ~BezierCurve();
///\brief Destructor
SPLINE_API ~bezier_curve()
{
// NOTHING
}
private:
BezierCurve(const BezierCurve&);
BezierCurve& operator=(const BezierCurve&);
bezier_curve(const bezier_curve&);
bezier_curve& operator=(const bezier_curve&);
/* Constructors - destructors */
/*Operations*/
public:
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t)
/// \param return : true if evaluation is successful, false if t is out of range
SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
/// \param return : the value x(t)
SPLINE_API virtual point_t operator()(time_t t) 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*/
SPLINE_API virtual Real MinBound() const;
SPLINE_API virtual Real MaxBound() const;
/*Helpers*/
SPLINE_API virtual time_t MinBound() const{return minBound_;}
SPLINE_API virtual time_t MaxBound() const{return minBound_;}
/*Helpers*/
public:
const int size_;
const Real minBound_, maxBound_;
const int size_;
const time_t minBound_, maxBound_;
private:
const T_Vector pts_;
};
typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Vector;
T_Vector pts_;
};
}
#endif //_CLASS_BEZIERCURVE
src/spline/API/CubicFunction.h
View file @ 4c1d45e3
......@@ -5,63 +5,84 @@
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the CubicFunction class.
* It allows the creation and evaluation of natural 3D
* smooth cubic splines
* This file contains definitions for the CubicFunction struct.
* It allows the creation and evaluation of natural
* smooth cubic splines of arbitrary dimension
*/
#ifndef _CLASS_CUBICFUNCTIONIMP
#define _CLASS_CUBICFUNCTIONIMP
#ifndef _STRUCT_CUBICFUNCTION
#define _STRUCT_CUBICFUNCTION
#include "Exports.h"
#include "MathDefs.h"
#include "Curve_ABC.h"
#include <stdexcept>
namespace spline
{
class SplineVisitor;
/// \class CubicFunction
/// \brief Represents a cubic spline defined on the interval
/// [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 */
public:
///\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
SPLINE_API ~CubicFunction();
SPLINE_API ~cubic_function()
{
// NOTHING
}
private:
CubicFunction(const CubicFunction&);
CubicFunction& operator=(const CubicFunction&);
//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 result : a reference to the Point set to the x(t)
SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
/// \param return : the value x(t)
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*/
/*Helpers*/
public:
SPLINE_API Real virtual MinBound() const;
SPLINE_API Real virtual MaxBound() const;
SPLINE_API num_t virtual MinBound() const {return t_min_;}
SPLINE_API num_t virtual MaxBound() const {return t_max_;}
/*Helpers*/
/*Attributes*/
private:
const Vector3 a_, b_, c_ ,d_;
const Real tBegin_, tEnd_;
public:
const point_t a_, b_, c_ ,d_;
const time_t t_min_, t_max_;
/*Attributes*/
}; //class CubicFunction
}
#endif //_CLASS_CUBICFUNCTIONIMP
#endif //_STRUCT_CUBICFUNCTION
src/spline/API/Curve_ABC.h
View file @ 4c1d45e3
/**
* \file Curve_ABC.h
* \brief class allowing to create an Exact cubic spline.
* \file curve_abc.h
* \brief interface for a Curve of arbitrary dimension.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
......@@ -9,67 +9,49 @@
*/
#ifndef _CLASS_CURVEABC
#define _CLASS_CURVEABC
#include "SplineVisitor.h"
#ifndef _STRUCT_CURVE_ABC
#define _STRUCT_CURVE_ABC
#include "Exports.h"
#include "MathDefs.h"
#include <memory>
#include <vector>
#include <functional>
namespace spline
{
/// \class Curve_ABC
/// \brief Represents a curve
///
class Curve_ABC
{
/// \struct curve_abc
/// \brief Represents a curve of dimension Dim
/// is Safe is false, no verification is made on the evaluation of the curve.
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 */
public:
///\brief Constructor
SPLINE_API Curve_ABC(){};
///\brief Destructor
SPLINE_API ~Curve_ABC(){};
///\brief Constructor
SPLINE_API curve_abc(){};
private:
Curve_ABC(const Curve_ABC&);
Curve_ABC& operator=(const Curve_ABC&);
///\brief Destructor
SPLINE_API ~curve_abc(){};
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t)
/// \param return : true if evaluation is successful, false if t is out of range
SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const = 0;
/// \param return : the value x(t)
SPLINE_API virtual point_t operator()(time_t t) const = 0;
/*Operations*/
/*Helpers*/
public:
/// \brief Given a timestep dt, returns a set of values for the exact spline
/// 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;
SPLINE_API virtual time_t MinBound() const = 0;
SPLINE_API virtual time_t MaxBound() const = 0;
/*Helpers*/
};
}
#endif //_CLASS_EXACTCUBIC
#endif //_STRUCT_CURVE_ABC
src/spline/API/ExactCubic.h
View file @ 4c1d45e3
/**
* \file ExactCubic.h
* \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
......@@ -21,56 +21,157 @@
#define _CLASS_EXACTCUBIC
#include "Curve_ABC.h"
#include "CubicFunction.h"
#include "Exports.h"
#include "MathDefs.h"
#include <memory>
#include <functional>
#include <vector>
#include <Eigen/StdVector>
#include <iostream>
namespace spline
{
struct CubicPImpl; // private implementation
/// \class ExactCubic
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization
///
struct ExactCubic : public Curve_ABC
{
/* Constructors - destructors */
/// \class ExactCubic
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization
///
template<typename Time= double, typename Numeric=Time, int Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
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:
///\brief Constructor
///\param waypoints : a list comprising at least 2 waypoints in ascending time order
SPLINE_API ExactCubic(const T_Waypoint& /*waypoints*/);
///\brief Constructor
///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
///\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
SPLINE_API ~ExactCubic();
MatrixX a = MatrixX::Zero(size, Dim);
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:
ExactCubic(const ExactCubic&);
ExactCubic& operator=(const ExactCubic&);
/* Constructors - destructors */
exact_cubic(const exact_cubic&);
exact_cubic& operator=(const exact_cubic&);
/* Constructors - destructors */
/*Operations*/
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t)
/// \return : true if evaluation is successful, false if t is out of range
SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
/*Operations*/
/// \param return : the value x(t)
SPLINE_API 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");}
for(CIT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
if(t >= ((*it)->t_min_) && t <= ((*it)->t_max_))
{
return (*it)->operator()(t);
}
}
}
/*Operations*/
/*Helpers*/
/*Helpers*/
public:
SPLINE_API Real virtual MinBound() const;
SPLINE_API Real virtual MaxBound() const;
/*Helpers*/
SPLINE_API num_t virtual MinBound() const{return subSplines_.front()->t_min_;}
SPLINE_API num_t virtual MaxBound() const{return subSplines_.back()->t_max_;}
/*Helpers*/
/*Attributes*/
/*Attributes*/
private:
std::auto_ptr<CubicPImpl> pImpl_;
/*Attributes*/
};
T_cubic subSplines_;
/*Attributes*/
};
}
#endif //_CLASS_EXACTCUBIC
src/spline/API/MathDefs.h
View file @ 4c1d45e3
......@@ -24,35 +24,17 @@
#include <utility>
namespace spline{
#if (USEFLOAT)
typedef float Real;
typedef Eigen::Vector3f Vector3;
typedef Eigen::Vector2f Vector2;
typedef Eigen::VectorXf VectorX;
typedef Eigen::MatrixXf MatrixX;
typedef Eigen::Matrix4f Matrix4;
typedef Eigen::Matrix3f Matrix3;
#else
typedef double Real;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Vector2d Vector2;
typedef Eigen::VectorXd VectorX;
typedef Eigen::MatrixXd MatrixX;
typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix3d Matrix3;
#endif
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template<typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat)
{
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
VectorX m_sigma = svd.singularValues();
_Matrix_Type_ m_sigma = svd.singularValues();
Real pinvtoler= 1.e-6; // choose your tolerance widely!
double pinvtoler= 1.e-6; // choose your tolerance widely!
MatrixX m_sigma_inv = MatrixX::Zero(pinvmat.cols(),pinvmat.rows());
_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows());
for (long i=0; i<m_sigma.rows(); ++i)
{
if (m_sigma(i) > pinvtoler)
......@@ -61,14 +43,6 @@ void PseudoInverse(_Matrix_Type_& pinvmat)
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
}
typedef std::vector<Vector3,Eigen::aligned_allocator<Vector3> > T_Vector;
/** Definition for a waypoint */
typedef std::pair<Real, Vector3> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef T_Waypoint::iterator IT_Waypoint;
typedef T_Waypoint::const_iterator CIT_Waypoint;
} // namespace spline
#endif //_SPLINEMATH
src/spline/API/SplineVisitor.h deleted 100644 → 0
View file @ 20b7bd84
/**
* \file SplineVisitor.h
* \brief Visitor for the Spline classes
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/