Commit 8006bd7c authored by Steve Tonneau's avatar Steve Tonneau

Merge branch 'topic/AddGettersForControlPointsOfCurves' into 'devel'

Topic / add functions to get waypoints of bezier and coeff of polynomial (for python)

See merge request loco-3d/curves!21
parents d0f0841d ccc49820
Subproject commit f389aae203c4d92649cd5eb66289fd6a17c03fde
Subproject commit cea261e3da7d383844530070356bca76d20197a8
......@@ -265,6 +265,16 @@ namespace curves
const t_point_t& waypoints() const {return control_points_;}
const point_t waypointAtIndex(const std::size_t index) const
{
point_t waypoint;
if (index<=control_points_.size())
{
waypoint = control_points_[index];
}
return waypoint;
}
/// \brief Evaluate the curve value at time t using deCasteljau algorithm.
/// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control points
/// of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier
......
......@@ -319,11 +319,26 @@ namespace curves
return deriv.compute_derivate(order-1);
}
Eigen::MatrixXd coeff() const
{
return coefficients_;
}
point_t coeffAtDegree(const std::size_t degree) const
{
point_t res;
if (degree <= degree_)
{
res = coefficients_.col(degree);
}
return res;
}
private:
num_t fact(const std::size_t n, const std::size_t order) const
{
num_t res(1);
for(std::size_t i = 0; i < order; ++i)
for(std::size_t i = 0; i < std::size_t(order); ++i)
{
res *= (num_t)(n-i);
}
......
......@@ -98,7 +98,7 @@ namespace curves
bezier_t* wrapBezierConstructorBoundsConstraints(const pointX_list_t& array, const curve_constraints_t& constraints,
const real T_min, const real T_max)
{
return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(array, constraints,
return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(array, constraints,
T_min, T_max) ;
}
/*END 3D constructors bezier */
......@@ -316,6 +316,7 @@ namespace curves
.def("derivate", &bezier3_t::derivate)
.def("compute_derivate", &bezier3_t::compute_derivate)
.def("compute_primitive", &bezier3_t::compute_primitive)
.def("waypointAtIndex", &bezier3_t::waypointAtIndex)
.def("saveAsText", &bezier3_t::saveAsText<bezier3_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&bezier3_t::loadFromText<bezier3_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&bezier3_t::saveAsXML<bezier3_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
......@@ -340,6 +341,7 @@ namespace curves
.def("derivate", &bezier_t::derivate)
.def("compute_derivate", &bezier_t::compute_derivate)
.def("compute_primitive", &bezier_t::compute_primitive)
.def("waypointAtIndex", &bezier_t::waypointAtIndex)
.def("saveAsText", &bezier_t::saveAsText<bezier_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&bezier_t::loadFromText<bezier_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&bezier_t::saveAsXML<bezier_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
......@@ -407,6 +409,8 @@ namespace curves
.def("min", &polynomial_t::min, "Get the LOWER bound on interval definition of the curve.")
.def("max", &polynomial_t::max,"Get the HIGHER bound on interval definition of the curve.")
.def("dim", &polynomial_t::dim)
.def("coeffAtDegree", &polynomial_t::coeffAtDegree)
.def("coeff", &polynomial_t::coeff)
.def("__call__", &polynomial_t::operator(),"Evaluate the spline at the given time.")
.def("derivate", &polynomial_t::derivate,"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
.def("compute_derivate", &polynomial_t::compute_derivate,"Compute derivative of order N of curve at time t.")
......
......@@ -36,6 +36,14 @@ class TestCurves(unittest.TestCase):
time_waypoints = matrix([0., 1.]).transpose()
# Create bezier6 and bezier
a = bezier(waypoints, 0., 3.)
# Test waypoints
self.assertTrue (a.nbWaypoints == 2)
for i in range(0, a.nbWaypoints):
if i==0:
self.assertTrue ((a.waypointAtIndex(0) == matrix([1., 2., 3.]).transpose()).all())
elif i==1:
self.assertTrue ((a.waypointAtIndex(1) == matrix([4., 5., 6.]).transpose()).all())
#self.assertTrue((a.waypoints == waypoints).all())
# Test : Degree, min, max, derivate
#self.print_str(("test 1")
self.assertEqual(a.degree, a.nbWaypoints - 1)
......@@ -108,13 +116,17 @@ class TestCurves(unittest.TestCase):
print("test_polynomial")
# To test :
# - Functions : constructor, min, max, derivate, serialize, deserialize
waypoints_0 = matrix([[0.,0.,0.], [0.,0.,0.]]).transpose()
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = polynomial(waypoints) # Defined on [0.,1.]
a = polynomial(waypoints, -1., 3.) # Defined on [-1.,3.]
a.min()
a.max()
a(0.4)
# Test get coefficient at degree
self.assertTrue((a.coeff()==waypoints).all())
self.assertTrue((a.coeffAtDegree(0) == matrix([1., 2., 3.]).transpose()).all())
self.assertTrue((a.coeffAtDegree(1) == matrix([4., 5., 6.]).transpose()).all())
# Other tests
self.assertTrue ((a(a.min()) == matrix([1., 2., 3.]).transpose()).all())
self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
......
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