From c499f1ded7e2015b78885fd27e82bf1a5f3594b4 Mon Sep 17 00:00:00 2001
From: JasonChmn <jason.chemin@hotmail.fr>
Date: Fri, 24 May 2019 16:34:11 +0200
Subject: [PATCH] Finish test for conversion curves, binding python added, all
test (conversion and piecewise) ok
---
include/curves/bezier_curve.h | 1 -
include/curves/curve_conversion.h | 14 +---
include/curves/piecewise_polynomial_curve.h | 2 +-
include/curves/polynomial.h | 2 +-
python/curves_python.cpp | 1 +
python/python_variables.h | 2 +-
python/test/test.py | 22 +++++-
tests/Main.cpp | 84 +++++++++++++++------
8 files changed, 87 insertions(+), 41 deletions(-)
diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index 600a225..9ecb0a0 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -359,7 +359,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
t_point_t wps_first(size_),wps_second(size_);
const Numeric u = (t-T_min_)/(T_max_-T_min_);
- std::cout<<T_min_<<" and "<<T_max_<<" t="<<t<<" u="<<u<<std::endl;
wps_first[0] = pts_.front();
wps_second[degree_] = pts_.back();
t_point_t casteljau_pts = waypoints();
diff --git a/include/curves/curve_conversion.h b/include/curves/curve_conversion.h
index d828458..12ec68c 100644
--- a/include/curves/curve_conversion.h
+++ b/include/curves/curve_conversion.h
@@ -23,17 +23,15 @@ Polynomial polynom_from_bezier(const Bezier& curve)
{
typedef typename Polynomial::t_point_t t_point_t;
typedef typename Polynomial::num_t num_t;
- assert (curve.min() == 0.);
- assert (curve.max() == 1.);
t_point_t coefficients;
Bezier current (curve);
- coefficients.push_back(curve(0.));
+ coefficients.push_back(curve(curve.min()));
num_t fact = 1;
for(std::size_t i = 1; i<= curve.degree_; ++i)
{
current = current.compute_derivate(1);
fact *= (num_t)i;
- coefficients.push_back(current(0.)/fact);
+ coefficients.push_back(current(current.min())/fact);
}
return Polynomial(coefficients,curve.min(),curve.max());
}
@@ -43,16 +41,14 @@ Polynomial polynom_from_hermite(const Hermite& curve)
{
typedef typename Polynomial::t_point_t t_point_t;
typedef typename Polynomial::num_t num_t;
- assert (curve.min() == 0.);
- assert (curve.max() == 1.);
t_point_t coefficients;
Hermite current (curve);
- coefficients.push_back(curve(0.));
+ coefficients.push_back(curve(curve.min()));
num_t fact = 1;
for(std::size_t i = 1; i<= 3; ++i)
{
fact *= (num_t)i;
- coefficients.push_back(current.derivate(0.,i)/fact);
+ coefficients.push_back(current.derivate(current.min(),i)/fact);
}
return Polynomial(coefficients,curve.min(),curve.max());
}
@@ -67,8 +63,6 @@ Bezier bezier_from_hermite(const Hermite& curve)
typedef typename Bezier::point_t point_t;
typedef typename Bezier::t_point_t t_point_t;
typedef typename Bezier::num_t num_t;
- assert (curve.min() == 0.);
- assert (curve.max() == 1.);
Hermite current (curve);
assert(current.control_points_.size() >= 2);
diff --git a/include/curves/piecewise_polynomial_curve.h b/include/curves/piecewise_polynomial_curve.h
index 44d694c..4b01b33 100644
--- a/include/curves/piecewise_polynomial_curve.h
+++ b/include/curves/piecewise_polynomial_curve.h
@@ -99,7 +99,7 @@ struct piecewise_polynomial_curve : public curve_abc<Time, Numeric, Dim, Safe, P
else
{
value_end = current.derivate(current.max(),order);
- value_start = next.derivate(current.min(),order);
+ value_start = next.derivate(next.min(),order);
}
if ((value_end-value_start).norm() > margin)
diff --git a/include/curves/polynomial.h b/include/curves/polynomial.h
index 257fb20..b6ef4c3 100644
--- a/include/curves/polynomial.h
+++ b/include/curves/polynomial.h
@@ -162,7 +162,7 @@ struct polynomial : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if((t < t_min_ || t > t_max_) && Safe)
{
- throw std::out_of_range("TODO");
+ throw std::out_of_range("error in polynomial : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
}
time_t const dt (t-t_min_);
time_t cdt(1);
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 7144186..959a01a 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -374,6 +374,7 @@ BOOST_PYTHON_MODULE(curves)
/** BEGIN curves conversion**/
def("polynom_from_bezier", polynom_from_bezier<bezier3_t,polynomial_t>);
def("bezier_from_hermite", bezier_from_hermite<cubic_hermite_spline_t,bezier3_t>);
+ def("polynom_from_hermite", polynom_from_hermite<cubic_hermite_spline_t,polynomial_t>);
/** END curves conversion**/
diff --git a/python/python_variables.h b/python/python_variables.h
index 3932842..d369c20 100644
--- a/python/python_variables.h
+++ b/python/python_variables.h
@@ -21,7 +21,7 @@ typedef curves::bezier_curve <real, real, dim, true, variables_3_t> bezier_line
bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors);
bezier_linear_variable_t* wrapBezierLinearConstructorBounds
- (const point_list_t& matrices, const point_list_t& vectors, const real ub);
+ (const point_list_t& matrices, const point_list_t& vectors, const real T_min, const real T_max);
typedef std::pair<Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>,
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> > linear_points_t;
diff --git a/python/test/test.py b/python/test/test.py
index fffafd4..50cd361 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -9,7 +9,9 @@ from numpy.linalg import norm
import unittest
-from curves import bezier3, bezier6, curve_constraints, exact_cubic, cubic_hermite_spline, polynom_from_bezier, bezier_from_hermite, polynomial
+from curves import bezier3
+from curves import bezier6
+from curves import curve_constraints, polynomial, exact_cubic, cubic_hermite_spline, polynom_from_bezier, bezier_from_hermite, polynom_from_hermite
class TestCurves(unittest.TestCase):
@@ -23,7 +25,7 @@ class TestCurves(unittest.TestCase):
# - Variables : degree, nbWayPoints
__EPS = 1e-6
waypoints = matrix([[1., 2., 3.]]).T
- a = bezier3(waypoints,2.)
+ a = bezier3(waypoints,0.,2.)
t = 0.
while t < 2.:
self.assertTrue (norm(a(t) - matrix([1., 2., 3.]).T) < __EPS)
@@ -33,7 +35,7 @@ class TestCurves(unittest.TestCase):
time_waypoints = matrix([0., 1.]).transpose()
# Create bezier6 and bezier3
a = bezier6(waypoints6)
- a = bezier3(waypoints, 3.)
+ a = bezier3(waypoints, 0., 3.)
# Test : Degree, min, max, derivate
#self.print_str(("test 1")
self.assertEqual (a.degree, a.nbWaypoints - 1)
@@ -58,7 +60,7 @@ class TestCurves(unittest.TestCase):
# Create new bezier3 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, 3.)
+ a1 = bezier3(waypoints, 0., 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)
# Check change in argument time_t of bezier3
@@ -147,6 +149,18 @@ class TestCurves(unittest.TestCase):
self.assertTrue (norm(a_hermite(0.3) - a_bezier(0.3)) < __EPS)
return
+ def test_polynom_from_cubic_hermite(self):
+ # converting cubic hermite to polynom
+ __EPS = 1e-6
+ points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
+ tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
+ time_points = matrix([0., 1.]).transpose()
+ a_hermite = cubic_hermite_spline(points, tangents, time_points)
+
+ a_pol = polynom_from_hermite(a_hermite)
+ self.assertTrue (norm(a_hermite(0.3) - a_pol(0.3)) < __EPS)
+ return
+
def test_cubic_hermite_spline(self):
points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
diff --git a/tests/Main.cpp b/tests/Main.cpp
index fbf6631..a3fe18a 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -452,9 +452,11 @@ void BezierToPolynomialConversionTest(bool& error)
control_points.push_back(h);
control_points.push_back(i);
- bezier_curve_t cf(control_points.begin(), control_points.end());
+ double T_min = 1.0;
+ double T_max = 2.0;
+ bezier_curve_t cf(control_points.begin(), control_points.end(),T_min, T_max);
polynomial_t pol =polynom_from_bezier<bezier_curve_t, polynomial_t>(cf);
- for(double i =0.; i<1.; i+=0.01)
+ for(double i =T_min; i<T_max; i+=0.01)
{
ComparePoints(cf(i),pol(i),errMsg, error, true);
ComparePoints(cf(i),pol(i),errMsg, error, false);
@@ -1010,13 +1012,10 @@ void BezierSplitCurve(bool& error)
std::cout<<"splitting point of the splitted curve doesn't correspond to the original"<<std::endl;
}
- std::cout<<"4"<<std::endl;
-
// check along curve :
double ti = t0;
while(ti <= ts)
{
- std::cout<<"a "<<ti<<std::endl;
if((cs.first(ti) - c(ti)).norm() > 1e-14)
{
error = true;
@@ -1024,8 +1023,8 @@ void BezierSplitCurve(bool& error)
}
ti += 0.01;
}
- while(ti <= t1){
- std::cout<<"b "<<ti<<std::endl;
+ while(ti <= t1)
+ {
if((cs.second(ti) - c(ti)).norm() > 1e-14)
{
error = true;
@@ -1034,7 +1033,6 @@ void BezierSplitCurve(bool& error)
ti += 0.01;
}
- std::cout<<"5"<<std::endl;
}
}
@@ -1176,8 +1174,6 @@ void piecewisePolynomialCurveTest(bool& error)
res = ppc_C0(1.5);
ComparePoints(point_t(1.5,1.5,1.5),res,errmsg1,error);
- std::cout<<"here"<<std::endl;
-
// piecewise curve C1 from Hermite
point_t p0(0.,0.,0.);
point_t p1(1.,2.,3.);
@@ -1191,35 +1187,81 @@ void piecewisePolynomialCurveTest(bool& error)
std::vector< pair_point_tangent_t > control_points_1;
control_points_1.push_back(pair_point_tangent_t(p1,t1));
control_points_1.push_back(pair_point_tangent_t(p2,t2)); // control_points_1 = 2nd piece of curve
- std::vector< double > time_control_points;
- time_control_points.push_back(0.);
- time_control_points.push_back(1.); // For both curves, time will be between [0,1]
- cubic_hermite_spline_t chs0(control_points_0.begin(), control_points_0.end(), time_control_points);
- cubic_hermite_spline_t chs1(control_points_1.begin(), control_points_1.end(), time_control_points);
+ std::vector< double > time_control_points0, time_control_points1;
+ time_control_points0.push_back(0.);
+ time_control_points0.push_back(1.); // hermite 0 between [0,1]
+ time_control_points1.push_back(1.);
+ time_control_points1.push_back(3.); // hermite 1 between [1,3]
+ cubic_hermite_spline_t chs0(control_points_0.begin(), control_points_0.end(), time_control_points0);
+ cubic_hermite_spline_t chs1(control_points_1.begin(), control_points_1.end(), time_control_points1);
// Convert to polynomial
polynomial_t pol_chs0 = polynom_from_hermite<cubic_hermite_spline_t, polynomial_t>(chs0);
polynomial_t pol_chs1 = polynom_from_hermite<cubic_hermite_spline_t, polynomial_t>(chs1);
piecewise_polynomial_curve_t ppc_C1(pol_chs0);
ppc_C1.add_polynomial_curve(pol_chs1);
+ // piecewise curve C2
+ point_t a0(0,0,0);
+ point_t b0(1,1,1);
+ t_point_t veca, vecb;
+ // in [0,1[
+ veca.push_back(a0);
+ veca.push_back(b0); // x=t, y=t, z=t
+ // in [1,2]
+ vecb.push_back(b0);
+ vecb.push_back(b0); // x=(t-1)+1, y=(t-1)+1, z=(t-1)+1
+ polynomial_t pola(veca.begin(), veca.end(), 0, 1);
+ polynomial_t polb(vecb.begin(), vecb.end(), 1, 2);
+ piecewise_polynomial_curve_t ppc_C2(pola);
+ ppc_C2.add_polynomial_curve(polb);
+
// check C0 continuity
- std::string errmsg2("in piecewise polynomial curve test, Error while checking continuity C0 ");
- // Test 1 : not C0
+ std::string errmsg2("in piecewise polynomial curve test, Error while checking continuity C0 on ");
+ std::string errmsg3("in piecewise polynomial curve test, Error while checking continuity C1 on ");
+ std::string errmsg4("in piecewise polynomial curve test, Error while checking continuity C2 on ");
+ // not C0
bool isC0 = ppc.isContinuous(0);
if (isC0)
{
std::cout << errmsg2 << " ppc " << std::endl;
error = true;
}
- // Test 1 : C0
+ // C0
isC0 = ppc_C0.isContinuous(0);
if (not isC0)
{
std::cout << errmsg2 << " ppc_C0 " << std::endl;
error = true;
}
-
+ // not C1
+ bool isC1 = ppc_C0.isContinuous(1);
+ if (isC1)
+ {
+ std::cout << errmsg3 << " ppc_C0 " << std::endl;
+ error = true;
+ }
+ // C1
+ isC1 = ppc_C1.isContinuous(1);
+ if (not isC1)
+ {
+ std::cout << errmsg3 << " ppc_C1 " << std::endl;
+ error = true;
+ }
+ // not C2
+ bool isC2 = ppc_C1.isContinuous(2);
+ if (isC2)
+ {
+ std::cout << errmsg4 << " ppc_C1 " << std::endl;
+ error = true;
+ }
+ // C2
+ isC2 = ppc_C2.isContinuous(2);
+ if (not isC2)
+ {
+ std::cout << errmsg4 << " ppc_C2 " << std::endl;
+ error = true;
+ }
}
@@ -1244,13 +1286,9 @@ int main(int /*argc*/, char** /*argv[]*/)
BezierDerivativeCurveConstraintTest(error);
BezierCurveTestCompareHornerAndBernstein(error);
BezierDerivativeCurveTimeReparametrizationTest(error);
- std::cout<<"here 1"<<std::endl;
BezierEvalDeCasteljau(error);
- std::cout<<"here 2"<<std::endl;
BezierSplitCurve(error);
- std::cout<<"here 3"<<std::endl;
CubicHermitePairsPositionDerivativeTest(error);
- std::cout<<"here 4"<<std::endl;
BezierToPolynomialConversionTest(error);
CubicHermiteToCubicBezierConversionTest(error);
--
GitLab