From 005fa5880dd6a47254fa5653537a1e301f8bea70 Mon Sep 17 00:00:00 2001
From: JasonChmn <jason.chemin@hotmail.fr>
Date: Fri, 23 Aug 2019 10:21:00 +0200
Subject: [PATCH] [dynamic dim - bezier curve] Remove dim from template of
bezier curve + binding python
---
include/curves/bezier_curve.h | 48 ++++++----
include/curves/linear_variable.h | 6 ++
python/curves_python.cpp | 152 +++++++++++++++++++------------
python/python_variables.h | 2 +-
python/test/test.py | 41 ++++++---
tests/Main.cpp | 16 ++--
6 files changed, 166 insertions(+), 99 deletions(-)
diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index 8f98930..e146c9c 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -28,7 +28,7 @@ namespace curves
/// For degree lesser than 4, the evaluation is analitycal. Otherwise
/// the bernstein polynoms are used to evaluate the spline at a given location.
///
- template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
+ template<typename Time= double, typename Numeric=Time, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>
{
@@ -38,14 +38,14 @@ namespace curves
typedef curve_constraints<point_t> curve_constraints_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef typename t_point_t::const_iterator cit_point_t;
- typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
+ typedef bezier_curve<Time, Numeric, Safe, Point > bezier_curve_t;
/* Constructors - destructors */
public:
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
bezier_curve()
- : T_min_(0), T_max_(0)
+ : dim_(0), T_min_(0), T_max_(0)
{}
/// \brief Constructor.
@@ -74,6 +74,11 @@ namespace curves
{
control_points_.push_back(*it);
}
+ // set dim
+ if (control_points_.size()!=0)
+ {
+ dim_ = PointsBegin->size();
+ }
}
/// \brief Constructor
@@ -102,10 +107,15 @@ namespace curves
{
control_points_.push_back(*cit);
}
+ // set dim
+ if (control_points_.size()!=0)
+ {
+ dim_ = PointsBegin->size();
+ }
}
bezier_curve(const bezier_curve& other)
- : T_min_(other.T_min_), T_max_(other.T_max_),
+ : dim_(other.dim_), T_min_(other.T_min_), T_max_(other.T_max_),
mult_T_(other.mult_T_), size_(other.size_),
degree_(other.degree_), bernstein_(other.bernstein_),
control_points_(other.control_points_)
@@ -123,7 +133,7 @@ namespace curves
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const
{
- check_if_not_empty();
+ check_conditions();
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
@@ -144,7 +154,7 @@ namespace curves
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
bezier_curve_t compute_derivate(const std::size_t order) const
{
- check_if_not_empty();
+ check_conditions();
if(order == 0)
{
return *this;
@@ -156,7 +166,7 @@ namespace curves
}
if(derived_wp.empty())
{
- derived_wp.push_back(point_t::Zero(Dim));
+ derived_wp.push_back(point_t::Zero(dim_));
}
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_min_, T_max_, mult_T_ * (1./(T_max_-T_min_)) );
return deriv.compute_derivate(order-1);
@@ -169,14 +179,14 @@ namespace curves
/// \return primitive at order N of x(t).
bezier_curve_t compute_primitive(const std::size_t order) const
{
- check_if_not_empty();
+ check_conditions();
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(Dim);
+ point_t current_sum = point_t::Zero(dim_);
// 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);
@@ -213,7 +223,7 @@ namespace curves
point_t evalBernstein(const Numeric t) const
{
const Numeric u = (t-T_min_)/(T_max_-T_min_);
- point_t res = point_t::Zero(Dim);
+ point_t res = point_t::Zero(dim_);
typename t_point_t::const_iterator control_points_it = control_points_.begin();
for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
cit !=bernstein_.end(); ++cit, ++control_points_it)
@@ -316,7 +326,7 @@ namespace curves
///
std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t)
{
- check_if_not_empty();
+ check_conditions();
if (fabs(t-T_max_)<MARGIN)
{
throw std::runtime_error("can't split curve, interval range is equal to original curve");
@@ -388,11 +398,15 @@ namespace curves
return res;
}
- void check_if_not_empty() const
+ void check_conditions() const
{
if (control_points_.size() == 0)
{
- throw std::runtime_error("Error in beziercurve : there is no control points set / did you use empty constructor ?");
+ throw std::runtime_error("Error in bezier curve : there is no control points set / did you use empty constructor ?");
+ }
+ else if(dim_ == 0)
+ {
+ throw std::runtime_error("Error in bezier curve : Dimension of points is zero / did you use empty constructor ?");
}
}
/*Operations*/
@@ -425,10 +439,10 @@ namespace curves
static const double MARGIN;
/* Attributes */
- static bezier_curve_t zero(const time_t T=1.)
+ static bezier_curve_t zero(const std::size_t dim, const time_t T=1.)
{
std::vector<point_t> ts;
- ts.push_back(point_t::Zero(Dim));
+ ts.push_back(point_t::Zero(dim));
return bezier_curve_t(ts.begin(), ts.end(),0.,T);
}
@@ -451,8 +465,8 @@ namespace curves
}
}; // End struct bezier_curve
- template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
- const double bezier_curve<Time, Numeric, Dim, Safe, Point>::MARGIN(0.001);
+ template<typename Time, typename Numeric, bool Safe, typename Point>
+ const double bezier_curve<Time, Numeric, Safe, Point>::MARGIN(0.001);
} // namespace curve
#endif //_CLASS_BEZIERCURVE
diff --git a/include/curves/linear_variable.h b/include/curves/linear_variable.h
index c8a087b..2ac2175 100644
--- a/include/curves/linear_variable.h
+++ b/include/curves/linear_variable.h
@@ -116,6 +116,12 @@ namespace curves
return *this;
}
+ std::size_t size()
+ {
+ variables_t w;
+ return w.size();
+ }
+
static variables_t Zero(size_t /*dim*/){
variables_t w;
return w;
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 03d90ea..04d115a 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -30,6 +30,7 @@ typedef std::pair<point3_t, point3_t> pair_point3_tangent_t;
typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point3_list_t;
typedef std::vector<point3_t,Eigen::aligned_allocator<point3_t> > t_point3_t;
typedef std::vector<pair_point3_tangent_t,Eigen::aligned_allocator<pair_point3_tangent_t> > t_pair_point3_tangent_t;
+typedef curves::curve_constraints<point3_t> curve_constraints3_t;
// XD
typedef Eigen::VectorXd pointX_t;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1, 0, Eigen::Dynamic, 1> ret_pointX_t;
@@ -37,6 +38,7 @@ typedef std::pair<pointX_t, pointX_t> pair_pointX_tangent_t;
typedef Eigen::MatrixXd pointX_list_t;
typedef std::vector<pointX_t,Eigen::aligned_allocator<point3_t> > t_pointX_t;
typedef std::vector<pair_pointX_tangent_t,Eigen::aligned_allocator<pair_pointX_tangent_t> > t_pair_pointX_tangent_t;
+typedef curves::curve_constraints<pointX_t> curve_constraints_t;
// Else
typedef std::pair<real, point3_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
@@ -44,9 +46,9 @@ typedef std::vector<Waypoint6> T_Waypoint6;
// Dynamic dim
typedef curves::cubic_hermite_spline <real, real, true, pointX_t> cubic_hermite_spline_t;
+typedef curves::bezier_curve <real, real, true, pointX_t> bezier_t;
// 3D
-typedef curves::bezier_curve <real, real, 3, true, point3_t> bezier3_t;
typedef curves::polynomial <real, real, 3, true, point3_t, t_point3_t> polynomial3_t;
typedef curves::exact_cubic <real, real, 3, true, point3_t, t_point3_t> exact_cubic3_t;
typedef polynomial3_t::coeff_t coeff_t;
@@ -54,20 +56,20 @@ typedef std::pair<real, point3_t> waypoint3_t;
typedef std::vector<waypoint3_t, Eigen::aligned_allocator<point3_t> > t_waypoint3_t;
typedef curves::piecewise_curve <real, real, 3, true, point3_t, t_point3_t, polynomial3_t> piecewise_polynomial3_curve_t;
-typedef curves::piecewise_curve <real, real, 3, true, point3_t, t_point3_t, bezier3_t> piecewise_bezier3_curve_t;
+typedef curves::piecewise_curve <real, real, 3, true, point3_t, t_point3_t, bezier_t> piecewise_bezier3_curve_t;
typedef curves::piecewise_curve <real, real, 3, true, point3_t, t_point3_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
typedef curves::Bern<double> bernstein_t;
-
-typedef curves::curve_constraints<point3_t> curve_constraints3_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier3_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(cubic_hermite_spline_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
+
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynomial3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic3_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_polynomial3_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_bezier3_curve_t)
@@ -95,23 +97,23 @@ namespace curves
/* End Template constructor bezier */
/*3D constructors bezier */
- bezier3_t* wrapBezierConstructor3(const point3_list_t& array)
+ bezier_t* wrapBezierConstructor(const pointX_list_t& array)
{
- return wrapBezierConstructorTemplate<bezier3_t, point3_list_t, t_point3_t>(array) ;
+ return wrapBezierConstructorTemplate<bezier_t, pointX_list_t, t_pointX_t>(array) ;
}
- bezier3_t* wrapBezierConstructorBounds3(const point3_list_t& array, const real T_min, const real T_max)
+ bezier_t* wrapBezierConstructorBounds(const pointX_list_t& array, const real T_min, const real T_max)
{
- return wrapBezierConstructorTemplate<bezier3_t, point3_list_t, t_point3_t>(array, T_min, T_max) ;
+ return wrapBezierConstructorTemplate<bezier_t, pointX_list_t, t_pointX_t>(array, T_min, T_max) ;
}
- bezier3_t* wrapBezierConstructor3Constraints(const point3_list_t& array, const curve_constraints3_t& constraints)
+ bezier_t* wrapBezierConstructorConstraints(const pointX_list_t& array, const curve_constraints_t& constraints)
{
- return wrapBezierConstructorConstraintsTemplate<bezier3_t, point3_list_t, t_point3_t, curve_constraints3_t>(array, constraints) ;
+ return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(array, constraints) ;
}
- bezier3_t* wrapBezierConstructorBounds3Constraints(const point3_list_t& array, const curve_constraints3_t& constraints,
- const real T_min, const real T_max)
+ bezier_t* wrapBezierConstructorBoundsConstraints(const pointX_list_t& array, const curve_constraints_t& constraints,
+ const real T_min, const real T_max)
{
- return wrapBezierConstructorConstraintsTemplate<bezier3_t, point3_list_t, t_point3_t, curve_constraints3_t>(array, constraints,
- T_min, T_max) ;
+ return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(array, constraints,
+ T_min, T_max) ;
}
/*END 3D constructors bezier */
@@ -163,7 +165,7 @@ namespace curves
{
return new piecewise_polynomial3_curve_t();
}
- piecewise_bezier3_curve_t* wrapPiecewiseBezier3CurveConstructor(const bezier3_t& bc)
+ piecewise_bezier3_curve_t* wrapPiecewiseBezier3CurveConstructor(const bezier_t& bc)
{
return new piecewise_bezier3_curve_t(bc);
}
@@ -192,21 +194,6 @@ namespace curves
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_cubic3_t* wrapExactCubic3Constructor(const coeff_t& array, const time_waypoints_t& time_wp)
{
t_waypoint3_t wps = getWayPoints(array, time_wp);
@@ -220,6 +207,7 @@ namespace curves
return new exact_cubic3_t(wps.begin(), wps.end(), constraints);
}
+ /// For constraints 3D
point3_t get_init_vel(const curve_constraints3_t& c)
{
return c.init_vel;
@@ -259,6 +247,46 @@ namespace curves
{
c.end_acc = val;
}
+ /// For constraints XD
+ point3_t get_init_velX(const curve_constraints_t& c)
+ {
+ return c.init_vel;
+ }
+
+ point3_t get_init_accX(const curve_constraints_t& c)
+ {
+ return c.init_acc;
+ }
+
+ point3_t get_end_velX(const curve_constraints_t& c)
+ {
+ return c.end_vel;
+ }
+
+ point3_t get_end_accX(const curve_constraints_t& c)
+ {
+ return c.end_acc;
+ }
+
+ void set_init_velX(curve_constraints_t& c, const point_t& val)
+ {
+ c.init_vel = val;
+ }
+
+ void set_init_accX(curve_constraints_t& c, const point_t& val)
+ {
+ c.init_acc = val;
+ }
+
+ void set_end_velX(curve_constraints_t& c, const point_t& val)
+ {
+ c.end_vel = val;
+ }
+
+ void set_end_accX(curve_constraints_t& c, const point_t& val)
+ {
+ c.end_acc = val;
+ }
/* End wrap exact cubic spline */
@@ -277,28 +305,27 @@ namespace curves
eigenpy::exposeQuaternion();*/
/** END eigenpy init**/
/** BEGIN bezier curve**/
- class_<bezier3_t>("bezier3", init<>())
- .def("__init__", make_constructor(&wrapBezierConstructor3))
- .def("__init__", make_constructor(&wrapBezierConstructorBounds3))
- .def("__init__", make_constructor(&wrapBezierConstructor3Constraints))
- .def("__init__", make_constructor(&wrapBezierConstructorBounds3Constraints))
- .def("min", &bezier3_t::min)
- .def("max", &bezier3_t::max)
- .def("dim", &bezier3_t::dim)
- .def("__call__", &bezier3_t::operator())
- .def("derivate", &bezier3_t::derivate)
- .def("compute_derivate", &bezier3_t::compute_derivate)
- .def("compute_primitive", &bezier3_t::compute_primitive)
- .def("waypoints", &wayPointsToList<bezier3_t,3>)
- .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.")
- .def("loadFromXML",&bezier3_t::loadFromXML<bezier3_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&bezier3_t::saveAsBinary<bezier3_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&bezier3_t::loadFromBinary<bezier3_t>,bp::args("filename"),"Loads *this from a binary file.")
- .def_readonly("degree", &bezier3_t::degree_)
- .def_readonly("nbWaypoints", &bezier3_t::size_)
- //.def(SerializableVisitor<bezier3_t>())
+ class_<bezier_t>("bezier", init<>())
+ .def("__init__", make_constructor(&wrapBezierConstructor))
+ .def("__init__", make_constructor(&wrapBezierConstructorBounds))
+ .def("__init__", make_constructor(&wrapBezierConstructorConstraints))
+ .def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
+ .def("min", &bezier_t::min)
+ .def("max", &bezier_t::max)
+ .def("dim", &bezier_t::dim)
+ .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("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.")
+ .def("loadFromXML",&bezier_t::loadFromXML<bezier_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
+ .def("saveAsBinary",&bezier_t::saveAsBinary<bezier_t>,bp::args("filename"),"Saves *this inside a binary file.")
+ .def("loadFromBinary",&bezier_t::loadFromBinary<bezier_t>,bp::args("filename"),"Loads *this from a binary file.")
+ .def_readonly("degree", &bezier_t::degree_)
+ .def_readonly("nbWaypoints", &bezier_t::size_)
+ //.def(SerializableVisitor<bezier_t>())
;
/** END bezier curve**/
/** BEGIN variable points bezier curve**/
@@ -461,6 +488,15 @@ namespace curves
.add_property("end_acc", &get_end_acc, &set_end_acc)
;
/** END curve constraints**/
+ /** BEGIN curve constraints**/
+ class_<curve_constraints_t>
+ ("curve_constraints", init<>())
+ .add_property("init_vel", &get_init_velX, &set_init_velX)
+ .add_property("init_acc", &get_init_accX, &set_init_accX)
+ .add_property("end_vel", &get_end_velX, &set_end_velX)
+ .add_property("end_acc", &get_end_accX, &set_end_accX)
+ ;
+ /** END curve constraints**/
/** BEGIN bernstein polynomial**/
class_<bernstein_t>
("bernstein", init<const unsigned int, const unsigned int>())
@@ -468,11 +504,11 @@ namespace curves
;
/** END bernstein polynomial**/
/** BEGIN curves conversion**/
- def("polynomial_from_bezier", polynomial_from_curve<polynomial3_t,bezier3_t>);
+ def("polynomial_from_bezier", polynomial_from_curve<polynomial3_t,bezier_t>);
def("polynomial_from_hermite", polynomial_from_curve<polynomial3_t,cubic_hermite_spline_t>);
- def("bezier_from_hermite", bezier_from_curve<bezier3_t,cubic_hermite_spline_t>);
- def("bezier_from_polynomial", bezier_from_curve<bezier3_t,polynomial3_t>);
- def("hermite_from_bezier", hermite_from_curve<cubic_hermite_spline_t, bezier3_t>);
+ def("bezier_from_hermite", bezier_from_curve<bezier_t,cubic_hermite_spline_t>);
+ def("bezier_from_polynomial", bezier_from_curve<bezier_t,polynomial3_t>);
+ def("hermite_from_bezier", hermite_from_curve<cubic_hermite_spline_t, bezier_t>);
def("hermite_from_polynomial", hermite_from_curve<cubic_hermite_spline_t, polynomial3_t>);
/** END curves conversion**/
} // End BOOST_PYTHON_MODULE
diff --git a/python/python_variables.h b/python/python_variables.h
index 91c6520..00ce47e 100644
--- a/python/python_variables.h
+++ b/python/python_variables.h
@@ -14,7 +14,7 @@ namespace curves
static const int dim = 3;
typedef curves::linear_variable<dim, real> linear_variable_3_t;
typedef curves::variables<linear_variable_3_t> variables_3_t;
- typedef curves::bezier_curve <real, real, dim, true, variables_3_t> bezier_linear_variable_t;
+ typedef curves::bezier_curve <real, real, true, variables_3_t> bezier_linear_variable_t;
/*linear variable control points*/
bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors);
diff --git a/python/test/test.py b/python/test/test.py
index 4360315..1431a1c 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -8,7 +8,7 @@ from numpy.linalg import norm
from curves import (bezier_from_hermite, bezier_from_polynomial, hermite_from_polynomial,
hermite_from_bezier, polynomial_from_hermite, polynomial_from_bezier,
- cubic_hermite_spline3, curve_constraints3, exact_cubic3, bezier3,
+ cubic_hermite_spline3, curve_constraints3,curve_constraints, exact_cubic3, bezier,
piecewise_bezier3_curve, piecewise_cubic_hermite3_curve,
piecewise_polynomial3_curve, polynomial3
)
@@ -26,15 +26,15 @@ class TestCurves(unittest.TestCase):
# - Variables : degree, nbWayPoints
__EPS = 1e-6
waypoints = matrix([[1., 2., 3.]]).T
- a = bezier3(waypoints, 0., 2.)
+ a = bezier(waypoints, 0., 2.)
t = 0.
while t < 2.:
self.assertTrue(norm(a(t) - matrix([1., 2., 3.]).T) < __EPS)
t += 0.1
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_waypoints = matrix([0., 1.]).transpose()
- # Create bezier6 and bezier3
- a = bezier3(waypoints, 0., 3.)
+ # Create bezier6 and bezier
+ a = bezier(waypoints, 0., 3.)
# Test : Degree, min, max, derivate
#self.print_str(("test 1")
self.assertEqual(a.degree, a.nbWaypoints - 1)
@@ -56,13 +56,13 @@ class TestCurves(unittest.TestCase):
t = float(i) / 10.
self.assertTrue((a(t) == prim.derivate(t, 2)).all())
self.assertTrue((prim(0) == matrix([0., 0., 0.])).all())
- # Create new bezier3 curve
+ # Create new bezier curve
waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose()
- a0 = bezier3(waypoints)
- a1 = bezier3(waypoints, 0., 3.)
+ a0 = bezier(waypoints)
+ a1 = bezier(waypoints, 0., 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)
- # Check change in argument time_t of bezier3
+ # Check change in argument time_t of bezier
for i in range(10):
t = float(i) / 10.
self.assertTrue(norm(a0(t) - a1(3 * t)) < __EPS)
@@ -71,21 +71,32 @@ class TestCurves(unittest.TestCase):
self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
self.assertTrue((prim(0) == matrix([0., 0., 0.])).all())
# testing bezier with constraints
- c = curve_constraints3()
+ c = curve_constraints()
c.init_vel = matrix([0., 1., 1.]).transpose()
c.end_vel = matrix([0., 1., 1.]).transpose()
c.init_acc = matrix([0., 1., -1.]).transpose()
c.end_acc = matrix([0., 100., 1.]).transpose()
#Check derivate with constraints
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
- a = bezier3(waypoints, c)
+ a = bezier(waypoints, c)
self.assertTrue(norm(a.derivate(0, 1) - c.init_vel) < 1e-10)
self.assertTrue(norm(a.derivate(1, 2) - c.end_acc) < 1e-10)
# Test serialization : bezier 3
a.saveAsText("serialization_curve.test")
#waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose()
- b = bezier3()
+ b = bezier()
+ b.loadFromText("serialization_curve.test")
+ self.assertTrue((a(0.4) == b(0.4)).all())
+ os.remove("serialization_curve.test")
+
+ # Bezier dim 4
+ waypoints = matrix([[1., 2., 3., 4.]]).T
+ a = bezier(waypoints, 0., 2.)
+ # Test serialization : bezier of dim 4
+ a.saveAsText("serialization_curve.test")
+ #waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose()
+ b = bezier()
b.loadFromText("serialization_curve.test")
self.assertTrue((a(0.4) == b(0.4)).all())
os.remove("serialization_curve.test")
@@ -173,12 +184,12 @@ class TestCurves(unittest.TestCase):
os.remove("serialization_pc.test")
return
- def test_piecewise_bezier3_curve(self):
+ def test_piecewise_bezier_curve(self):
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
- a = bezier3(waypoints, 0., 1.)
- b = bezier3(waypoints, 1., 2.)
+ a = bezier(waypoints, 0., 1.)
+ b = bezier(waypoints, 1., 2.)
pc = piecewise_bezier3_curve(a)
pc.add_curve(b)
pc.min()
@@ -264,7 +275,7 @@ class TestCurves(unittest.TestCase):
def test_conversion_curves(self):
__EPS = 1e-6
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
- a = bezier3(waypoints)
+ a = bezier(waypoints)
# converting bezier to polynomial
a_pol = polynomial_from_bezier(a)
self.assertTrue(norm(a(0.3) - a_pol(0.3)) < __EPS)
diff --git a/tests/Main.cpp b/tests/Main.cpp
index 6023f46..9cfe2b5 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -23,21 +23,21 @@ namespace curves
typedef curve_abc <double, double, true, point_t> curve_abc_t;
typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
- typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
+ typedef bezier_curve <double, double, true, point_t> bezier_curve_t;
+ typedef cubic_hermite_spline <double, double, true, point_t> cubic_hermite_spline_t;
+ typedef exact_cubic <double, double, 1, true, Eigen::Matrix<double,1,1> > exact_cubic_one;
+ typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, polynomial_t> piecewise_polynomial_curve_t;
+ typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
+ typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef std::pair<double, point_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef Eigen::Matrix<double,1,1> point_one;
- typedef polynomial<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;
- typedef cubic_hermite_spline <double, double, true, point_t> cubic_hermite_spline_t;
typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
typedef std::vector<pair_point_tangent_t,Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
- typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, polynomial_t> piecewise_polynomial_curve_t;
- typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
- typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
+
const double margin = 1e-3;
bool QuasiEqual(const double a, const double b)
{
@@ -1245,7 +1245,7 @@ void curveAbcDimDynamicTest(bool& error)
typedef polynomial <double, double, 3, true> polynomial_test_t;
typedef exact_cubic <double, double, 3, true> exact_cubic_test_t;
typedef exact_cubic_test_t::spline_constraints spline_constraints_test_t;
- typedef bezier_curve <double, double, 3, true> bezier_curve_test_t;
+ typedef bezier_curve <double, double, true> bezier_curve_test_t;
typedef cubic_hermite_spline <double, double, true> cubic_hermite_spline_test_t;
curve_abc_test_t * pt_curve_abc;
// POLYNOMIAL
--
GitLab