Commit 70c4ecba authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

Merge branch 'topic/AddBezier3PythonBindingOnDevel' into 'devel'

Topic/add bezier3 python binding

See merge request loco-3d/curves!18
parents ea0f5767 3748285e
......@@ -333,9 +333,9 @@ namespace curves
}
t_point_t wps_first(size_),wps_second(size_);
const Numeric u = (t-T_min_)/(T_max_-T_min_);
wps_first[0] = control_points_.front();
wps_second[degree_] = control_points_.back();
t_point_t casteljau_pts = waypoints();
wps_first[0] = casteljau_pts.front();
wps_second[degree_] = casteljau_pts.back();
size_t id = 1;
while(casteljau_pts.size() > 1)
{
......@@ -344,31 +344,37 @@ namespace curves
wps_second[degree_-id] = casteljau_pts.back();
++id;
}
bezier_curve_t c_first(wps_first.begin(), wps_first.end(),T_min_,t,mult_T_);
bezier_curve_t c_second(wps_second.begin(), wps_second.end(),t, T_max_,mult_T_);
return std::make_pair(c_first,c_second);
}
bezier_curve_t extract(const Numeric t1, const Numeric t2){
/// \brief Extract a bezier curve defined between \f$[t_1,t_2]\f$ from the actual bezier curve
/// defined between \f$[T_{min},T_{max}]\f$ with \f$T_{min} \leq t_1 \leq t_2 \leq T_{max}\f$.
/// \param t1 : start time of bezier curve extracted.
/// \param t2 : end time of bezier curve extracted.
/// \return bezier curve extract defined between \f$[t_1,t_2]\f$.
///
bezier_curve_t extract(const Numeric t1, const Numeric t2)
{
if(t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_)
{
throw std::out_of_range("In Extract curve : times out of bounds");
}
if (fabs(t1-T_min_)<MARGIN && fabs(t2-T_max_)<MARGIN)
if (fabs(t1-T_min_)<MARGIN && fabs(t2-T_max_)<MARGIN) // t1=T_min and t2=T_max
{
return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
}
if (fabs(t1-T_min_)<MARGIN)
if (fabs(t1-T_min_)<MARGIN) // t1=T_min
{
return split(t2).first;
}
if (fabs(t2-T_max_)<MARGIN)
if (fabs(t2-T_max_)<MARGIN) // t2=T_max
{
return split(t1).second;
}
std::pair<bezier_curve_t,bezier_curve_t> c_split = this->split(t1);
return c_split.second.split(t2-t1).first;
return c_split.second.split(t2).first;
}
private:
......
......@@ -44,12 +44,16 @@ typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, bezier_
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
typedef curves::exact_cubic <real, real, true, pointX_t, t_pointX_t> exact_cubic_t;
// Bezier 3
typedef curves::bezier_curve <real, real, true, Eigen::Vector3d> bezier3_t;
typedef curves::Bern<double> bernstein_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynomial_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_polynomial_curve_t)
......@@ -244,6 +248,30 @@ namespace curves
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
/** END eigenpy init**/
/** BEGIN bezier3 curve**/
class_<bezier3_t>("bezier3", init<>())
.def("__init__", make_constructor(&wrapBezierConstructor))
.def("__init__", make_constructor(&wrapBezierConstructorBounds))
.def("__init__", make_constructor(&wrapBezierConstructorConstraints))
.def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
.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("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<bezier_t>())
;
/** END bezier3 curve**/
/** BEGIN bezier curve**/
class_<bezier_t>("bezier", init<>())
.def("__init__", make_constructor(&wrapBezierConstructor))
......
......@@ -212,10 +212,10 @@ void BezierCurveTest(bool& error)
std::string errMsg2("In test BezierCurveTest ; Bernstein polynomials do not evaluate as analytical evaluation");
for(double d = 1.; d <2.; d+=0.1)
{
ComparePoints( cf5.evalBernstein(d) , cf5 (d), errMsg2, error);
ComparePoints( cf5.evalHorner(d) , cf5 (d), errMsg2, error);
ComparePoints( cf5.compute_derivate(1).evalBernstein(d) , cf5.compute_derivate(1) (d), errMsg2, error);
ComparePoints( cf5.compute_derivate(1).evalHorner(d) , cf5.compute_derivate(1) (d), errMsg2, error);
ComparePoints( cf5.evalBernstein(d) , cf5 (d), errMsg2, error);
ComparePoints( cf5.evalHorner(d) , cf5 (d), errMsg2, error);
ComparePoints( cf5.compute_derivate(1).evalBernstein(d) , cf5.compute_derivate(1) (d), errMsg2, error);
ComparePoints( cf5.compute_derivate(1).evalHorner(d) , cf5.compute_derivate(1) (d), errMsg2, error);
}
bool error_in(true);
try
......@@ -920,62 +920,82 @@ void BezierSplitCurve(bool& error)
double t_min = 0.2;
double t_max = 10;
double aux0, aux1;
std::string errMsg0("BezierSplitCurve, ERROR initial point of the splitted curve doesn't correspond to the original");
std::string errMsg1("BezierSplitCurve, ERROR splitting point of the splitted curve doesn't correspond to the original");
std::string errMsg2("BezierSplitCurve, ERROR final point of the splitted curve doesn't correspond to the original");
std::string errMsg3("BezierSplitCurve, ERROR while checking value on curve and curves splitted");
std::string errMsg4("BezierSplitCurve, ERROR Degree of the splitted curve are not the same as the original curve");
std::string errMsg5("BezierSplitCurve, ERROR duration of the splitted curve doesn't correspond to the original");
std::string errMsg6("BezierSplitCurve, ERROR while checking value on curve extracted");
for(size_t i = 0 ; i < 1 ; ++i)
{
// build a random curve and split it at random time :
//std::cout<<"build a random curve"<<std::endl;
point_t a;
std::vector<point_t> wps;
for(size_t j = 0 ; j <= n ; ++j)
{
wps.push_back(randomPoint(-10.,10.));
}
double t0 = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
double t1 = (rand()/(double)RAND_MAX )*(t_max-t0) + t0;
double ts = (rand()/(double)RAND_MAX )*(t1-t0)+t0;
bezier_curve_t c(wps.begin(), wps.end(),t0, t1);
std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
// test on splitted curves :
if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) ))
{
error = true;
std::cout<<"BezierSplitCurve, ERROR Degree of the splitted curve are not the same as the original curve"<<std::endl;
}
aux0 = c.max()-c.min();
aux1 = (cs.first.max()-cs.first.min() + cs.second.max()-cs.second.min());
if(!QuasiEqual(aux0, aux1))
{
error = true;
std::cout<<"BezierSplitCurve, ERROR duration of the splitted curve doesn't correspond to the original"<<std::endl;
}
if(!QuasiEqual(cs.first.max(), ts))
{
error = true;
std::cout<<"BezierSplitCurve, ERROR timing of the splitted curve doesn't correspond to the original"<<std::endl;
}
std::string errmsg("BezierSplitCurve, ERROR initial point of the splitted curve doesn't correspond to the original");
ComparePoints(c(t0), cs.first(t0), errmsg, error);
errmsg = "BezierSplitCurve, ERROR splitting point of the splitted curve doesn't correspond to the original";
ComparePoints(cs.first(ts), cs.second(ts), errmsg, error);
errmsg = "BezierSplitCurve, ERROR final point of the splitted curve doesn't correspond to the original";
ComparePoints(c(t1), cs.second(cs.second.max()), errmsg, error);
// check along curve :
double ti = t0;
errmsg = "BezierSplitCurve, ERROR while checking value on curve and curves splitted";
while(ti <= ts)
{
ComparePoints(cs.first(ti), c(ti), errmsg, error);
ti += 0.01;
}
while(ti <= t1)
{
ComparePoints(cs.second(ti), c(ti), errmsg, error);
ti += 0.01;
}
// build a random curve and split it at random time :
//std::cout<<"build a random curve"<<std::endl;
point_t a;
std::vector<point_t> wps;
for(size_t j = 0 ; j <= n ; ++j)
{
wps.push_back(randomPoint(-10.,10.));
}
double t0 = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
double t1 = (rand()/(double)RAND_MAX )*(t_max-t0) + t0;
double ts = (rand()/(double)RAND_MAX )*(t1-t0)+t0;
bezier_curve_t c(wps.begin(), wps.end(),t0, t1);
std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
// test on splitted curves :
if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) ))
{
error = true;
std::cout<<errMsg4<<std::endl;
}
aux0 = c.max()-c.min();
aux1 = (cs.first.max()-cs.first.min() + cs.second.max()-cs.second.min());
if(!QuasiEqual(aux0, aux1))
{
error = true;
std::cout<<errMsg5<<std::endl;
}
if(!QuasiEqual(cs.first.max(), ts))
{
error = true;
std::cout<<errMsg0<<std::endl;
}
ComparePoints(c(t0), cs.first(t0), errMsg0, error);
ComparePoints(cs.first(ts), cs.second(ts), errMsg1, error);
ComparePoints(c(t1), cs.second(cs.second.max()), errMsg2, error);
// check along curve :
double ti = t0;
while(ti <= ts)
{
ComparePoints(cs.first(ti), c(ti), errMsg3, error);
ti += 0.01;
}
while(ti <= t1)
{
ComparePoints(cs.second(ti), c(ti), errMsg3, error);
ti += 0.01;
}
// Test extract function
bezier_curve_t bezier_extracted0 = c.extract(t0+0.01,t1-0.01); // T_min < t0 < t1 < T_max
for(double t=bezier_extracted0.min(); t<bezier_extracted0.max(); t+=0.01)
{
ComparePoints(bezier_extracted0(t),c(t),errMsg6, error);
}
bezier_curve_t bezier_extracted1 = c.extract(t0,t1-0.01); // T_min = t0 < t1 < T_max
for(double t=bezier_extracted1.min(); t<bezier_extracted1.max(); t+=0.01)
{
ComparePoints(bezier_extracted1(t),c(t),errMsg6, error);
}
bezier_curve_t bezier_extracted2 = c.extract(t0+0.01,t1); // T_min < t0 < t1 = T_max
for(double t=bezier_extracted2.min(); t<bezier_extracted2.max(); t+=0.01)
{
ComparePoints(bezier_extracted2(t),c(t),errMsg6, error);
}
bezier_curve_t bezier_extracted3 = c.extract(t0,t1); // T_min = t0 < t1 = T_max
for(double t=bezier_extracted3.min(); t<bezier_extracted3.max(); t+=0.01)
{
ComparePoints(bezier_extracted3(t),c(t),errMsg6, error);
}
}
}
......@@ -983,88 +1003,103 @@ void BezierSplitCurve(bool& error)
/* cubic hermite spline function test */
void CubicHermitePairsPositionDerivativeTest(bool& error)
{
std::string errmsg1("in Cubic Hermite 2 pairs (pos,vel), Error While checking that given wayPoints are crossed (expected / obtained) : ");
std::string errmsg2("in Cubic Hermite 2 points, Error While checking value of point on curve : ");
std::string errmsg3("in Cubic Hermite 2 points, Error While checking value of tangent on curve : ");
std::vector< pair_point_tangent_t > control_points;
point_t res1;
point_t p0(0.,0.,0.);
point_t p1(1.,2.,3.);
point_t p2(4.,4.,4.);
point_t t0(0.5,0.5,0.5);
point_t t1(0.1,0.2,-0.5);
point_t t2(0.1,0.2,0.3);
std::vector< double > time_control_points, time_control_points_test;
// Two pairs
control_points.clear();
control_points.push_back(pair_point_tangent_t(p0,t0));
control_points.push_back(pair_point_tangent_t(p1,t1));
time_control_points.push_back(0.); // Time at P0
time_control_points.push_back(1.); // Time at P1
// Create cubic hermite spline
cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end(), time_control_points);
// Dimension
if (cubic_hermite_spline_1Pair.dim() != 3)
{
error = true;
std::cout << "Cubic hermite spline test, Error : Dimension of curve is wrong\n";
}
//Check
res1 = cubic_hermite_spline_1Pair(0.); // t=0
ComparePoints(p0, res1, errmsg1, error);
res1 = cubic_hermite_spline_1Pair(1.); // t=1
ComparePoints(p1, res1, errmsg1, error);
// Test derivative : two pairs
res1 = cubic_hermite_spline_1Pair.derivate(0.,1);
ComparePoints(t0, res1, errmsg3, error);
res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
ComparePoints(t1, res1, errmsg3, error);
// Three pairs
control_points.push_back(pair_point_tangent_t(p2,t2));
time_control_points.clear();
time_control_points.push_back(0.); // Time at P0
time_control_points.push_back(2.); // Time at P1
time_control_points.push_back(5.); // Time at P2
cubic_hermite_spline_t cubic_hermite_spline_2Pairs(control_points.begin(), control_points.end(), time_control_points);
//Check
res1 = cubic_hermite_spline_2Pairs(0.); // t=0
ComparePoints(p0, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(2.); // t=2
ComparePoints(p1, res1, errmsg2, error);
res1 = cubic_hermite_spline_2Pairs(5.); // t=5
ComparePoints(p2, res1, errmsg1, error);
// Test derivative : three pairs
res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
ComparePoints(t0, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(2.,1);
ComparePoints(t1, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(5.,1);
ComparePoints(t2, res1, errmsg3, error);
// Test time control points by default [0,1] => with N control points :
// Time at P0= 0. | Time at P1= 1.0/(N-1) | Time at P2= 2.0/(N-1) | ... | Time at P_(N-1)= (N-1)/(N-1)= 1.0
time_control_points_test.clear();
time_control_points_test.push_back(0.); // Time at P0
time_control_points_test.push_back(0.5); // Time at P1
time_control_points_test.push_back(1.0); // Time at P2
cubic_hermite_spline_2Pairs.setTime(time_control_points_test);
res1 = cubic_hermite_spline_2Pairs(0.); // t=0
ComparePoints(p0, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
ComparePoints(p1, res1, errmsg2, error);
res1 = cubic_hermite_spline_2Pairs(1.); // t=1
ComparePoints(p2, res1, errmsg1, error);
// Test getTime
try
{
std::string errmsg1("in Cubic Hermite 2 pairs (pos,vel), Error While checking that given wayPoints are crossed (expected / obtained) : ");
std::string errmsg2("in Cubic Hermite 2 points, Error While checking value of point on curve : ");
std::string errmsg3("in Cubic Hermite 2 points, Error While checking value of tangent on curve : ");
std::vector< pair_point_tangent_t > control_points;
point_t res1;
point_t p0(0.,0.,0.);
point_t p1(1.,2.,3.);
point_t p2(4.,4.,4.);
point_t t0(0.5,0.5,0.5);
point_t t1(0.1,0.2,-0.5);
point_t t2(0.1,0.2,0.3);
std::vector< double > time_control_points, time_control_points_test;
// Two pairs
control_points.clear();
control_points.push_back(pair_point_tangent_t(p0,t0));
control_points.push_back(pair_point_tangent_t(p1,t1));
time_control_points.push_back(0.); // Time at P0
time_control_points.push_back(1.); // Time at P1
// Create cubic hermite spline
cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end(), time_control_points);
// Dimension
if (cubic_hermite_spline_1Pair.dim() != 3)
{
error = true;
std::cout << "Cubic hermite spline test, Error : Dimension of curve is wrong\n";
}
//Check
res1 = cubic_hermite_spline_1Pair(0.); // t=0
ComparePoints(p0, res1, errmsg1, error);
res1 = cubic_hermite_spline_1Pair(1.); // t=1
ComparePoints(p1, res1, errmsg1, error);
// Test derivative : two pairs
res1 = cubic_hermite_spline_1Pair.derivate(0.,1);
ComparePoints(t0, res1, errmsg3, error);
res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
ComparePoints(t1, res1, errmsg3, error);
// Three pairs
control_points.push_back(pair_point_tangent_t(p2,t2));
time_control_points.clear();
time_control_points.push_back(0.); // Time at P0
time_control_points.push_back(2.); // Time at P1
time_control_points.push_back(5.); // Time at P2
cubic_hermite_spline_t cubic_hermite_spline_2Pairs(control_points.begin(), control_points.end(), time_control_points);
//Check
res1 = cubic_hermite_spline_2Pairs(0.); // t=0
ComparePoints(p0, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(2.); // t=2
ComparePoints(p1, res1, errmsg2, error);
res1 = cubic_hermite_spline_2Pairs(5.); // t=5
ComparePoints(p2, res1, errmsg1, error);
// Test derivative : three pairs
res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
ComparePoints(t0, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(2.,1);
ComparePoints(t1, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(5.,1);
ComparePoints(t2, res1, errmsg3, error);
// Test time control points by default [0,1] => with N control points :
// Time at P0= 0. | Time at P1= 1.0/(N-1) | Time at P2= 2.0/(N-1) | ... | Time at P_(N-1)= (N-1)/(N-1)= 1.0
time_control_points_test.clear();
time_control_points_test.push_back(0.); // Time at P0
time_control_points_test.push_back(0.5); // Time at P1
time_control_points_test.push_back(1.0); // Time at P2
cubic_hermite_spline_2Pairs.setTime(time_control_points_test);
res1 = cubic_hermite_spline_2Pairs(0.); // t=0
ComparePoints(p0, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
ComparePoints(p1, res1, errmsg2, error);
res1 = cubic_hermite_spline_2Pairs(1.); // t=1
ComparePoints(p2, res1, errmsg1, error);
// Test getTime
try
{
cubic_hermite_spline_2Pairs.getTime();
}
catch(...)
{
}
catch(...)
{
error = false;
}
if(error)
{
std::cout << "Cubic hermite spline test, Error when calling getTime\n";
}
// Test derivative : three pairs, time default
res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
ComparePoints(t0, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(0.5,1);
ComparePoints(t1, res1, errmsg3, error);
res1 = cubic_hermite_spline_2Pairs.derivate(1.,1);
ComparePoints(t2, res1, errmsg3, error);
}
if(error)
catch(...)
{
std::cout << "Cubic hermite spline test, Error when calling getTime\n";
error = true;
std::cout<<"Error in CubicHermitePairsPositionDerivativeTest"<<std::endl;
}
}
......
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