Commit 32b39c08 authored by Steve Tonneau's avatar Steve Tonneau

spline_curve renamed to polynom. operator () replaced with faster horner scheme

parent 4b1c4e8e
......@@ -16,7 +16,7 @@
#include "MathDefs.h"
#include "spline_curve.h"
#include "polynom.h"
#include <stdexcept>
......@@ -35,11 +35,11 @@ T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point
}
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point>
spline_curve<Time,Numeric,Dim,Safe,Point,T_Point> create_cubic(Point const& a, Point const& b, Point const& c, Point const &d,
polynom<Time,Numeric,Dim,Safe,Point,T_Point> create_cubic(Point const& a, Point const& b, Point const& c, Point const &d,
const Time min, const Time max)
{
T_Point coeffs = make_cubic_vector<Point, T_Point>(a,b,c,d);
return spline_curve<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), min, max);
return polynom<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), min, max);
}
}
#endif //_STRUCT_CUBICSPLINE
......
......@@ -37,7 +37,7 @@ 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> >
, typename SplineBase=spline_curve<Time, Numeric, Dim, Safe, Point, T_Point> >
, typename SplineBase=polynom<Time, Numeric, Dim, Safe, Point, T_Point> >
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
......
/**
* \file spline_curve.h
* \file polynom.h
* \brief Definition of a cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the spline_curve struct.
* This file contains definitions for the polynom struct.
* It allows the creation and evaluation of natural
* smooth splines of arbitrary dimension and order
*/
#ifndef _STRUCT_SPLINE
#define _STRUCT_SPLINE
#ifndef _STRUCT_POLYNOM
#define _STRUCT_POLYNOM
#include "MathDefs.h"
......@@ -25,14 +25,14 @@
namespace spline
{
/// \class spline_curve
/// \brief Represents a spline curve of arbitrary order defined on the interval
/// \class polynom
/// \brief Represents a polynomf 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, 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>
struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef Time time_t;
......@@ -49,7 +49,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// by the number of the columns -1.
///\param min: LOWER bound on interval definition of the spline
///\param max: UPPER bound on interval definition of the spline
spline_curve(const coeff_t& coefficients, const time_t min, const time_t max)
polynom(const coeff_t& coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.cols()-1)
{
......@@ -62,7 +62,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// by the size of the coefficients
///\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, const time_t min, const time_t max)
polynom(const T_Point& coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.cols()-1)
......@@ -77,7 +77,7 @@ 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, const time_t min, const time_t max)
polynom(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
:coefficients_(init_coeffs(zeroOrderCoefficient, out)), dim_(Dim), order_(coefficients_.cols()-1),
t_min_(min), t_max_(max)
{
......@@ -85,18 +85,18 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
///\brief Destructor
~spline_curve()
~polynom()
{
// NOTHING
}
spline_curve(const spline_curve& other)
polynom(const polynom& other)
: coefficients_(other.coefficients_), dim_(other.dim_), order_(other.order_),
t_min_(other.t_min_), t_max_(other.t_max_){}
//spline_curve& operator=(const spline_curve& other);
//polynom& operator=(const polynom& other);
private:
void safe_check()
......@@ -114,7 +114,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Operations*/
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 return : the value x(t)
virtual point_t operator()(const time_t t) const
......@@ -126,6 +126,20 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
for(int i = 0; i < order_+1; ++i, cdt*=dt)
currentPoint_ += cdt *coefficients_.col(i);
return currentPoint_;
}*/
/// \brief Evaluation of the cubic spline at time t using horner's scheme.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual point_t operator()(const 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_);
point_t h = coefficients_.col(order_);
for(int i=order_-1; i>=0; i--)
h = dt*h + coefficients_.col(i);
return h;
}
......@@ -183,7 +197,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
res.col(i) = *cit;
return res;
}
}; //class spline_curve
}; //class polynom
}
#endif //_STRUCT_SPLINE
#endif //_STRUCT_POLYNOM
......@@ -16,7 +16,7 @@
#include "MathDefs.h"
#include "spline_curve.h"
#include "polynom.h"
#include <stdexcept>
......@@ -37,11 +37,11 @@ T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c,
}
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point>
spline_curve<Time,Numeric,Dim,Safe,Point,T_Point> create_quintic(Point const& a, Point const& b, Point const& c, Point const &d, Point const &e, Point const &f,
polynom<Time,Numeric,Dim,Safe,Point,T_Point> create_quintic(Point const& a, Point const& b, Point const& c, Point const &d, Point const &e, Point const &f,
const Time min, const Time max)
{
T_Point coeffs = make_quintic_vector<Point, T_Point>(a,b,c,d,e,f);
return spline_curve<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), min, max);
return polynom<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), min, max);
}
}
#endif //_STRUCT_QUINTIC_SPLINE
......
......@@ -40,7 +40,7 @@ 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> >,
typename SplineBase=spline_curve<Time, Numeric, Dim, Safe, Point, T_Point> >
typename SplineBase=polynom<Time, Numeric, Dim, Safe, Point, T_Point> >
struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase>
{
typedef Point point_t;
......@@ -49,7 +49,7 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
typedef Time time_t;
typedef Numeric num_t;
typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
typedef polynom<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
typedef exact_cubic<time_t, Numeric, Dim, Safe, point_t, t_point_t> exact_cubic_t;
typedef typename std::vector<spline_t> t_spline_t;
typedef typename t_spline_t::iterator it_spline_t;
......
#include "spline/bezier_curve.h"
#include "spline/spline_curve.h"
#include "spline/polynom.h"
#include "spline/exact_cubic.h"
#include "spline/spline_deriv_constraint.h"
#include "spline/curve_constraint.h"
......@@ -29,9 +29,9 @@ typedef std::vector<Waypoint6> T_Waypoint6;
typedef spline::bezier_curve <real, real, 3, true, point_t> bezier_t;
typedef spline::bezier_curve <real, real, 6, true, point6_t> bezier6_t;
typedef spline::spline_curve <real, real, 3, true, point_t, t_point_t> spline_curve_t;
typedef spline::polynom <real, real, 3, true, point_t, t_point_t> polynom_t;
typedef spline::exact_cubic <real, real, 3, true, point_t, t_point_t> exact_cubic_t;
typedef spline_curve_t::coeff_t coeff_t;
typedef polynom_t::coeff_t coeff_t;
typedef std::pair<real, point_t> waypoint_t;
typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t;
......@@ -43,7 +43,7 @@ typedef spline::curve_constraints<point6_t> curve_constraints6_t;
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynom_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t)
......@@ -111,9 +111,9 @@ bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, c
}
/*END 6D constructors */
spline_curve_t* wrapSplineConstructor(const coeff_t& array)
polynom_t* wrapSplineConstructor(const coeff_t& array)
{
return new spline_curve_t(array, 0., 1.);
return new polynom_t(array, 0., 1.);
}
......@@ -255,12 +255,12 @@ BOOST_PYTHON_MODULE(spline)
/** BEGIN spline curve function**/
class_<spline_curve_t>("spline", init<const spline_curve_t::coeff_t, const real, const real >())
class_<polynom_t>("polynom", init<const polynom_t::coeff_t, const real, const real >())
.def("__init__", make_constructor(&wrapSplineConstructor))
.def("min", &spline_curve_t::min)
.def("max", &spline_curve_t::max)
.def("__call__", &spline_curve_t::operator())
.def("derivate", &spline_curve_t::derivate)
.def("min", &polynom_t::min)
.def("max", &polynom_t::max)
.def("__call__", &polynom_t::operator())
.def("derivate", &polynom_t::derivate)
;
/** END cubic function**/
......
from spline import bezier, bezier6, spline, exact_cubic, curve_constraints, spline_deriv_constraint
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint
from numpy import matrix
from numpy.linalg import norm
......@@ -46,9 +46,9 @@ assert norm(a.derivate(0,1) - c.init_vel) < 1e-10
assert norm(a.derivate(1,2) - c.end_acc) < 1e-10
#testing spline function
a = spline(waypoints)
a = spline(waypoints, -1., 3.)
#testing polynom function
a = polynom(waypoints)
a = polynom(waypoints, -1., 3.)
a.min()
a.max()
a(0.4)
......
#include "spline/exact_cubic.h"
#include "spline/bezier_curve.h"
#include "spline/spline_curve.h"
#include "spline/polynom.h"
#include "spline/spline_deriv_constraint.h"
#include "spline/helpers/effector_spline.h"
#include "spline/helpers/effector_spline_rotation.h"
......@@ -16,7 +16,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, true, point_t, t_point_t> spline_curve_t;
typedef polynom <double, double, 3, true, point_t, t_point_t> polynom_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
typedef spline_deriv_constraint <double, double, 3, true, point_t> spline_deriv_constraint_t;
typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
......@@ -26,7 +26,7 @@ typedef std::vector<Waypoint> T_Waypoint;
typedef Eigen::Matrix<double,1,1> point_one;
typedef spline_curve<double, double, 1, true, point_one> spline_curve_one;
typedef polynom<double, double, 1, true, point_one> polynom_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;
......@@ -68,28 +68,28 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const
void CubicFunctionTest(bool& error)
{
std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
point_t d(3,6,7);
point_t d(3,6,7);
t_point_t vec;
vec.push_back(a);
vec.push_back(b);
vec.push_back(c);
vec.push_back(d);
spline_curve_t cf(vec.begin(), vec.end(), 0, 1);
point_t res1;
res1 =cf(0);
point_t x0(1,2,3);
polynom_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);
res1 =cf(1);
ComparePoints(x1, res1, errMsg + "(1) ", error);
point_t x2(3.125,5.25,7.125);
res1 =cf(0.5);
point_t x2(3.125,5.25,7.125);
res1 =cf(0.5);
ComparePoints(x2, res1, errMsg + "(0.5) ", error);
vec.clear();
......@@ -97,45 +97,45 @@ void CubicFunctionTest(bool& error)
vec.push_back(b);
vec.push_back(c);
vec.push_back(d);
spline_curve_t cf2(vec, 0.5, 1);
res1 = cf2(0.5);
polynom_t cf2(vec, 0.5, 1);
res1 = cf2(0.5);
ComparePoints(x0, res1, errMsg + "x3 ", error);
error = true;
try
{
cf2(0.4);
}
catch(...)
{
error = false;
}
if(error)
{
std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
}
error = true;
try
{
cf2(1.1);
}
catch(...)
{
error = false;
}
if(error)
{
std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
}
if(cf.max() != 1)
{
error = true;
std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
}
if(cf.min() != 0)
{
error = true;
std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
}
error = true;
try
{
cf2(0.4);
}
catch(...)
{
error = false;
}
if(error)
{
std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
}
error = true;
try
{
cf2(1.1);
}
catch(...)
{
error = false;
}
if(error)
{
std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
}
if(cf.max() != 1)
{
error = true;
std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
}
if(cf.min() != 0)
{
error = true;
std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
}
}
/*bezier_curve Function tests*/
......
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