Skip to content

GitLab

Commit 005fa588 authored by JasonChmn's avatar JasonChmn Committed by Pierre Fernbach
Browse files

[dynamic dim - bezier curve] Remove dim from template of bezier curve + binding python

parent 5b1a835e
Hide whitespace changes
Inline Side-by-side
include/curves/bezier_curve.h
View file @ 005fa588
......@@ -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
......
include/curves/linear_variable.h
View file @ 005fa588
......@@ -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;
......
python/curves_python.cpp
View file @ 005fa588
......@@ -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
......
python/python_variables.h
View file @ 005fa588
......@@ -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);
......
python/test/test.py
View file @ 005fa588
......@@ -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)
......
tests/Main.cpp
View file @ 005fa588
......@@ -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
......
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