Commit d88c3393 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

Merge branch 'packaging'

parents 62165aed 86554a37
......@@ -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:
......
......@@ -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**/
......
......@@ -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)
......
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