Skip to content
Snippets Groups Projects
Commit d88c3393 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

Merge branch 'packaging'

parents 62165aed 86554a37
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/spline/bezier_curve.h
+ 29
− 2
View file @ d88c3393
......@@ -18,6 +18,8 @@
#include <vector>
#include <stdexcept>
#include <iostream>
namespace spline
{
/// \class BezierCurve
......@@ -45,7 +47,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
: minBound_(minBound)
, maxBound_(maxBound)
, size_(std::distance(PointsBegin, PointsEnd))
, bernstein_(spline::makeBernstein<num_t>(size_-1))
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
......@@ -117,13 +120,34 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
if(order == 0) return *this;
t_point_t derived_wp;
for(typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end()-1; ++pit)
derived_wp.push_back(*(pit+1) - (*pit));
derived_wp.push_back(degree_ * (*(pit+1) - (*pit)));
if(derived_wp.empty())
derived_wp.push_back(point_t::Zero());
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),minBound_,maxBound_);
return deriv.compute_derivate(order-1);
}
/// \brief Computes the primitive of the curve at order N.
/// \param constant : value of the primitive at t = 0
/// \param return : the curve x_1(t) such that d/dt(x_1(t)) = x_1(t)
bezier_curve_t compute_primitive(const std::size_t order) const
{
if(order == 0) return *this;
num_t new_degree = (num_t)(degree_+1);
t_point_t n_wp;
point_t current_sum = point_t::Zero();
// recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
// then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
n_wp.push_back(current_sum);
for(typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end(); ++pit)
{
current_sum += *pit;
n_wp.push_back(current_sum / new_degree);
}
bezier_curve_t integ(n_wp.begin(), n_wp.end(),minBound_,maxBound_);
return integ.compute_primitive(order-1);
}
/// \brief Evaluates the derivative at order N of the curve.
/// If the derivative is to be evaluated several times, it is
/// rather recommended to compute the derivative curve using compute_derivate
......@@ -151,6 +175,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
return res;
}
const t_point_t& waypoints() const {return pts_;}
/*Operations*/
/*Helpers*/
......@@ -161,6 +187,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
public:
const time_t minBound_, maxBound_;
const std::size_t size_;
const std::size_t degree_;
const std::vector<Bern<Numeric> > bernstein_;
private:
......
python/spline_python.cpp
+ 63
− 5
View file @ d88c3393
......@@ -13,14 +13,21 @@
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
typedef double real;
typedef Eigen::Vector3d point_t;
typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> point6_t;
typedef Eigen::Matrix<double, 3, 1, 0, 3, 1> ret_point_t;
typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> ret_point6_t;
typedef Eigen::VectorXd time_waypoints_t;
typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
typedef Eigen::Matrix<real, 6, Eigen::Dynamic> point_list6_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef std::vector<point6_t,Eigen::aligned_allocator<point6_t> > t_point6_t;
typedef std::pair<real, point_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef std::pair<real, point6_t> Waypoint6;
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::exact_cubic <real, real, 3, true, point_t, t_point_t> exact_cubic_t;
typedef spline_curve_t::coeff_t coeff_t;
......@@ -33,6 +40,7 @@ typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
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(exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_constraints_t)
......@@ -41,10 +49,10 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t)
namespace spline
{
using namespace boost::python;
t_point_t vectorFromEigenArray(const point_list_t& array)
template <typename PointList, typename T_Point>
T_Point vectorFromEigenArray(const PointList& array)
{
t_point_t res;
T_Point res;
for(int i =0;i<array.cols();++i)
res.push_back(array.col(i));
return res;
......@@ -52,17 +60,30 @@ t_point_t vectorFromEigenArray(const point_list_t& array)
bezier_t* wrapBezierConstructor(const point_list_t& array)
{
t_point_t asVector = vectorFromEigenArray(array);
t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
return new bezier_t(asVector.begin(), asVector.end());
}
bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real lb, const real ub)
{
t_point_t asVector = vectorFromEigenArray(array);
t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
return new bezier_t(asVector.begin(), asVector.end(), lb, ub);
}
bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
{
t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
return new bezier6_t(asVector.begin(), asVector.end());
}
bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real lb, const real ub)
{
t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
return new bezier6_t(asVector.begin(), asVector.end(), lb, ub);
}
spline_curve_t* wrapSplineConstructor(const coeff_t& array)
{
return new spline_curve_t(array, 0., 1.);
......@@ -77,6 +98,19 @@ t_waypoint_t getWayPoints(const coeff_t& array, const time_waypoints_t& time_wp)
return res;
}
template <typename BezierType, int dim>
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> wayPointsToList(const BezierType& self)
{
typedef typename BezierType::t_point_t t_point;
typedef typename BezierType::t_point_t::const_iterator cit_point;
const t_point& wps = self.waypoints();
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> res (dim, wps.size());
int col = 0;
for(cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++col)
res.block<dim,1>(0,col) = *cit;
return res;
}
exact_cubic_t* wrapExactCubicConstructor(const coeff_t& array, const time_waypoints_t& time_wp)
{
t_waypoint_t wps = getWayPoints(array, time_wp);
......@@ -146,11 +180,31 @@ BOOST_PYTHON_MODULE(spline)
eigenpy::enableEigenPySpecific<point_t,point_t>();
eigenpy::enableEigenPySpecific<ret_point_t,ret_point_t>();
eigenpy::enableEigenPySpecific<point_list_t,point_list_t>();
eigenpy::enableEigenPySpecific<point6_t,point6_t>();
eigenpy::enableEigenPySpecific<ret_point6_t,ret_point6_t>();
eigenpy::enableEigenPySpecific<point_list6_t,point_list6_t>();
eigenpy::enableEigenPySpecific<coeff_t,coeff_t>();
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
/** END eigenpy init**/
/** BEGIN bezier curve 6**/
class_<bezier6_t>
("bezier6", no_init)
.def("__init__", make_constructor(&wrapBezierConstructor6))
.def("__init__", make_constructor(&wrapBezierConstructorBounds6))
.def("min", &bezier6_t::min)
.def("max", &bezier6_t::max)
.def("__call__", &bezier6_t::operator())
.def("derivate", &bezier6_t::derivate)
.def("compute_derivate", &bezier6_t::compute_derivate)
.def("compute_primitive", &bezier6_t::compute_primitive)
.def("waypoints", &wayPointsToList<bezier6_t,6>)
.def_readonly("degree", &bezier6_t::degree_)
.def_readonly("nbWaypoints", &bezier6_t::size_)
;
/** END bezier curve**/
/** BEGIN bezier curve**/
class_<bezier_t>
("bezier", no_init)
......@@ -161,6 +215,10 @@ BOOST_PYTHON_MODULE(spline)
.def("__call__", &bezier_t::operator())
.def("derivate", &bezier_t::derivate)
.def("compute_derivate", &bezier_t::compute_derivate)
.def("compute_primitive", &bezier_t::compute_primitive)
.def("waypoints", &wayPointsToList<bezier_t,3>)
.def_readonly("degree", &bezier_t::degree_)
.def_readonly("nbWaypoints", &bezier_t::size_)
;
/** END bezier curve**/
......
python/test/test.py
+ 17
− 2
View file @ d88c3393
......@@ -6,16 +6,31 @@ waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
time_waypoints = matrix([0.,1.])
#testing bezier curve
a = bezier(waypoints)
a = bezier(waypoints, -1., 3.)
a = bezier(waypoints)
assert(a.degree == a.nbWaypoints -1)
a.min()
a.max()
a(0.4)
assert((a.derivate(0.4,0) == a(0.4)).all())
a.derivate(0.4,2)
a = a.compute_derivate(100)
prim = a.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert(a(t) == prim.derivate(t,1)).all()
assert(prim(0) == matrix([0.,0.,0.])).all()
prim = a.compute_primitive(2)
for i in range(10):
t = float(i) / 10.
assert(a(t) == prim.derivate(t,2)).all()
assert(prim(0) == matrix([0.,0.,0.])).all()
#testing spline function
a = spline(waypoints)
......
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