From ccc4982016af8ac29a15b4a3becdf03c9164e61c Mon Sep 17 00:00:00 2001
From: JasonChmn <jason.chemin@hotmail.fr>
Date: Mon, 23 Sep 2019 17:26:26 +0200
Subject: [PATCH] Add another function to get coefficients in polynomial /
Python bindings OK
---
cmake | 2 +-
include/curves/bezier_curve.h | 10 ++++++++++
include/curves/polynomial.h | 17 ++++++++++++++++-
python/curves_python.cpp | 6 +++++-
python/test/test.py | 14 +++++++++++++-
5 files changed, 45 insertions(+), 4 deletions(-)
diff --git a/cmake b/cmake
index f389aae..cea261e 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit f389aae203c4d92649cd5eb66289fd6a17c03fde
+Subproject commit cea261e3da7d383844530070356bca76d20197a8
diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index d2f02a3..a1b4ba7 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -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
diff --git a/include/curves/polynomial.h b/include/curves/polynomial.h
index 6c5c4c2..a575528 100644
--- a/include/curves/polynomial.h
+++ b/include/curves/polynomial.h
@@ -190,11 +190,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);
}
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 3e92250..9c12986 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -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 */
@@ -261,6 +261,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.")
@@ -285,6 +286,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.")
@@ -336,6 +338,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.")
diff --git a/python/test/test.py b/python/test/test.py
index 1fe23a4..1ae9ed2 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -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)
--
GitLab