Commit d1757d2a authored by Steve Tonneau's avatar Steve Tonneau Committed by Steve Tonneau

implemented generic splines. no error

parent 3b8434c8
......@@ -31,59 +31,59 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct cubic_function : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef Point point_t;
typedef Time time_t;
typedef Numeric num_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)
:a_(a), b_(b), c_(c), d_(d), t_min_(min), t_max_(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");
}
}
if(t_min_ > t_max_ && Safe)
{
std::out_of_range("TODO");
}
}
///\brief Destructor
~cubic_function()
{
// NOTHING
}
///\brief Destructor
~cubic_function()
{
// NOTHING
}
private:
cubic_function(const cubic_function&);
cubic_function& operator=(const cubic_function&);
private:
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 return : the value x(t)
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;
}
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
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:
/// \brief Returns the minimum time for wich curve is defined
num_t virtual min() const {return t_min_;}
/// \brief Returns the maximum time for wich curve is defined
num_t virtual max() const {return t_max_;}
public:
/// \brief Returns the minimum time for wich curve is defined
num_t virtual min() const {return t_min_;}
/// \brief Returns the maximum time for wich curve is defined
num_t virtual max() const {return t_max_;}
/*Helpers*/
/*Attributes*/
public:
const point_t a_, b_, c_ ,d_;
const time_t t_min_, t_max_;
public:
const point_t a_, b_, c_ ,d_;
const time_t t_min_, t_max_;
/*Attributes*/
}; //class CubicFunction
}; //class CubicFunction
}
#endif //_STRUCT_CUBICFUNCTION
/**
* \file cubic_function.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_SPLINE
#define _STRUCT_SPLINE
#include "MathDefs.h"
#include "curve_abc.h"
#include <iostream>
#include <algorithm>
#include <functional>
#include <stdexcept>
namespace spline
{
/// \class spline_curve
/// \brief Represents a spline curve of arbitrary order defined on the interval
/// [tBegin, tEnd]. It follows the equation
/// x(t) = a + b(t - t_min_) + ... + d(t - t_min_)^N, where N is the order
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, std::size_t Order=3, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
typedef typename t_point_t::const_iterator cit_point_t;
typedef Time time_t;
typedef Numeric num_t;
/* Constructors - destructors */
public:
///\brief Constructor
///\param coefficients : a container containing all coefficients of the spline, starting
/// 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)
:coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(Order)
{
if(t_min_ > t_max_ && Safe)
{
std::out_of_range("TODO");
}
}
///\brief Constructor
///\param zeroOrderCoefficient : an iterator pointing to the first element of a structure containing the coefficients
/// it corresponds to the zero degree coefficient
///\param out : an iterator pointing to the last element of a structure ofcoefficients
///\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)
:coefficients_(init_coeffs(zeroOrderCoefficient, out)), t_min_(min), t_max_(max), dim_(Dim), order_(Order)
{
if(t_min_ > t_max_ && Safe)
{
std::out_of_range("TODO");
}
}
///\brief Destructor
~spline_curve()
{
// NOTHING
}
private:
spline_curve(const spline_curve&);
spline_curve& operator=(const spline_curve&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
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_);
time_t cdt(dt);
cit_point_t cit = coefficients_.begin();
point_t currentPoint_ = *cit; ++cit;
for(; cit != coefficients_.end(); ++cit, cdt*=dt)
currentPoint_ += cdt *(*cit);
return currentPoint_;
}
/*Operations*/
/*Helpers*/
public:
/// \brief Returns the minimum time for wich curve is defined
num_t virtual min() const {return t_min_;}
/// \brief Returns the maximum time for wich curve is defined
num_t virtual max() const {return t_max_;}
/*Helpers*/
/*Attributes*/
public:
const t_point_t coefficients_;
const time_t t_min_, t_max_;
const std::size_t dim_;
const std::size_t order_;
/*Attributes*/
private:
template<typename In>
t_point_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient)
{
t_point_t res(Order+1);
std::copy(zeroOrderCoefficient, highestOrderCoefficient, res.begin());
return res;
}
}; //class spline_curve
}
#endif //_STRUCT_SPLINE
#include "spline/cubic_function.h"
#include "spline/exact_cubic.h"
#include "spline/bezier_curve.h"
#include "spline/spline_curve.h"
#include <string>
#include <iostream>
......@@ -12,7 +12,8 @@ using namespace std;
namespace spline
{
typedef Eigen::Vector3d point_t;
typedef cubic_function<double, double, 3, true, point_t> cubic_function_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 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;
......@@ -20,7 +21,7 @@ typedef std::vector<Waypoint> T_Waypoint;
typedef Eigen::Matrix<double,1,1> point_one;
typedef cubic_function<double, double, 1, true, point_one> cubic_function_one;
typedef spline_curve<double, double, 1, 3, true, point_one> cubic_function_one;
typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one;
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
......@@ -59,7 +60,7 @@ void ComparePoints(const point_t& pt1, const point_t& pt2, const std::string& er
if(!(pt1 == pt2))
{
error = true;
std::cout << errmsg << pt1 << " ; " << pt2 << "\n";
std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
}
}
......@@ -68,7 +69,7 @@ void ComparePoints(const point_one& pt1, const point_one& pt2, const std::string
if(!(pt1 == pt2))
{
error = true;
std::cout << errmsg << pt1 << " ; " << pt2 << "\n";
std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
}
}
......@@ -81,25 +82,33 @@ void CubicFunctionTest(bool& error)
point_t b(2,3,4);
point_t c(3,4,5);
point_t d(3,6,7);
cubic_function_t cf(a, b, c, d, 0, 1);
t_point_t vec;
vec.push_back(a);
vec.push_back(b);
vec.push_back(c);
vec.push_back(d);
cubic_function_t cf(vec.begin(), vec.end(), 0, 1);
point_t res1;
res1 =cf(0);
point_t x0(1,2,3);
ComparePoints(x0, res1, errMsg + "(0) ", error);
point_t x1(9,15,19);
ComparePoints(x0, res1, errMsg + "(0) ", error);
res1 =cf(1);
ComparePoints(x1, res1, errMsg + "(1) ", error);
point_t x1(9,15,19);
res1 =cf(1);
ComparePoints(x1, res1, errMsg + "(1) ", error);
point_t x2(3.125,5.25,7.125);
res1 =cf(0.5);
ComparePoints(x2, res1, errMsg + "(0.5) ", error);
cubic_function_t cf2(a, b, c, d, 0.5, 1);
ComparePoints(x2, res1, errMsg + "(0.5) ", error);
vec.clear();
vec.push_back(a);
vec.push_back(b);
vec.push_back(c);
vec.push_back(d);
cubic_function_t cf2(vec, 0.5, 1);
res1 = cf2(0.5);
ComparePoints(x0, res1, errMsg + "x3 ", error);
ComparePoints(x0, res1, errMsg + "x3 ", error);
error = true;
try
{
......@@ -157,29 +166,29 @@ void BezierCurveTest(bool& error)
point_t res1;
res1 = cf(0);
point_t x20 = a ;
ComparePoints(x20, res1, errMsg + "2(0) ", error);
ComparePoints(x20, res1, errMsg + "2(0) ", error);
point_t x21 = b;
res1 = cf(1);
ComparePoints(x21, res1, errMsg + "2(1) ", error);
ComparePoints(x21, res1, errMsg + "2(1) ", error);
//3d curve
params.push_back(c);
bezier_curve_t cf3(params.begin(), params.end());
res1 = cf3(0);
ComparePoints(a, res1, errMsg + "3(0) ", error);
ComparePoints(a, res1, errMsg + "3(0) ", error);
res1 = cf3(1);
ComparePoints(c, res1, errMsg + "3(1) ", error);
ComparePoints(c, res1, errMsg + "3(1) ", error);
//4d curve
params.push_back(d);
bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2);
res1 = cf4(0.4);
ComparePoints(a, res1, errMsg + "3(0) ", error);
ComparePoints(a, res1, errMsg + "3(0) ", error);
res1 = cf4(2);
ComparePoints(d, res1, errMsg + "3(1) ", error);
ComparePoints(d, res1, errMsg + "3(1) ", error);
try
{
......
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