From 83feb504cd8e7e6ab575fa06e94563600edd5158 Mon Sep 17 00:00:00 2001
From: JasonChmn <jason.chemin@hotmail.fr>
Date: Wed, 3 Jul 2019 15:01:55 +0200
Subject: [PATCH] [piecewise_curve and main] Fix all (in)equalities by
comparing to an epsilon
---
include/curves/cubic_hermite_spline.h | 2 +-
include/curves/linear_variable.h | 2 +-
include/curves/piecewise_curve.h | 7 +-
python/test/test.py | 448 +++++++++++++-------------
tests/Main.cpp | 177 +++++-----
5 files changed, 313 insertions(+), 323 deletions(-)
diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
index ecedf4b..1eeda8d 100644
--- a/include/curves/cubic_hermite_spline.h
+++ b/include/curves/cubic_hermite_spline.h
@@ -214,7 +214,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
//
const Time dt = (t1-t0);
const Time alpha = (t - t0)/dt;
- assert(0. <= alpha <= 1. && "alpha must be in [0,1]");
+ assert(0. <= alpha && alpha <= 1. && "alpha must be in [0,1]");
Numeric h00, h10, h01, h11;
evalCoeffs(alpha,h00,h10,h01,h11,order_derivative);
//std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<" h11="<<h11<<std::endl;
diff --git a/include/curves/linear_variable.h b/include/curves/linear_variable.h
index 14ea20e..ff1537c 100644
--- a/include/curves/linear_variable.h
+++ b/include/curves/linear_variable.h
@@ -50,7 +50,7 @@ struct linear_variable{
return *this;
}
- static linear_variable_t Zero(size_t dim=0){
+ static linear_variable_t Zero(){
linear_variable_t w;
w.A_ = matrix_t::Zero();
w.b_ = point_t::Zero();
diff --git a/include/curves/piecewise_curve.h b/include/curves/piecewise_curve.h
index de88eb5..1481e83 100644
--- a/include/curves/piecewise_curve.h
+++ b/include/curves/piecewise_curve.h
@@ -86,9 +86,12 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
void add_curve(const curve_t& cf)
{
// Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
- if (size_!=0 && cf.min()!=T_max_)
+ if (size_!=0)
{
- throw std::invalid_argument("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
+ if (!(fabs(cf.min()-T_max_)<std::numeric_limits<Time>::epsilon()))
+ {
+ throw std::invalid_argument("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
+ }
}
curves_.push_back(cf);
size_ = curves_.size();
diff --git a/python/test/test.py b/python/test/test.py
index 43c5eae..d77db6b 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -17,230 +17,230 @@ from curves import bezier_from_hermite, bezier_from_polynomial
from curves import hermite_from_bezier, hermite_from_polynomial
class TestCurves(unittest.TestCase):
- #def print_str(self, inStr):
- # print inStr
- # return
-
- def test_bezier(self):
- # To test :
- # - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive
- # - Variables : degree, nbWayPoints
- __EPS = 1e-6
- waypoints = matrix([[1., 2., 3.]]).T
- a = bezier3(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()
- waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).transpose()
- time_waypoints = matrix([0., 1.]).transpose()
- # Create bezier6 and bezier3
- a = bezier6(waypoints6)
- a = bezier3(waypoints, 0., 3.)
- # Test : Degree, min, max, derivate
- #self.print_str(("test 1")
- self.assertEqual (a.degree, a.nbWaypoints - 1)
- a.min()
- a.max()
- a(0.4)
- self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
- a.derivate(0.4, 2)
- a = a.compute_derivate(100)
- prim = a.compute_primitive(1)
- # Check primitive and derivate - order 1
- for i in range(10):
- t = float(i) / 10.
- self.assertTrue ((a(t) == prim.derivate(t, 1)).all())
- self.assertTrue ((prim(0) == matrix([0., 0., 0.])).all())
- # Check primitive and derivate - order 2
- prim = a.compute_primitive(2)
- for i in range(10):
- 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
- 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.)
- prim0 = a0.compute_primitive(1)
- prim1 = a1.compute_primitive(1)
- # Check change in argument time_t of bezier3
- for i in range(10):
- t = float(i) / 10.
- self.assertTrue (norm(a0(t) - a1(3 * t)) < __EPS)
- self.assertTrue (norm(a0.derivate(t, 1) - a1.derivate(3 * t, 1) * 3.) < __EPS)
- self.assertTrue (norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
- 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_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)
- self.assertTrue (norm(a.derivate(0, 1) - c.init_vel) < 1e-10)
- self.assertTrue (norm(a.derivate(1, 2) - c.end_acc) < 1e-10)
- return
-
- def test_polynomial(self):
- # To test :
- # - Functions : constructor, min, max, derivate
- 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)
- self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
- a.derivate(0.4, 2)
- return
-
- def test_piecewise_polynomial_curve(self):
- # To test :
- # - Functions : constructor, min, max, derivate, add_curve, is_continuous
- waypoints1 = matrix([[1., 1., 1.]]).transpose()
- waypoints2 = matrix([[1., 1., 1.], [1., 1., 1.]]).transpose()
- a = polynomial(waypoints1, 0.,1.)
- b = polynomial(waypoints2, 1., 3.)
- ppc = piecewise_polynomial_curve(a)
- ppc.add_curve(b)
- ppc.min()
- ppc.max()
- ppc(0.4)
- self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
- ppc.derivate(0.4, 2)
- ppc.is_continuous(0)
- ppc.is_continuous(1)
- return
-
- def test_piecewise_bezier3_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.)
- ppc = piecewise_bezier3_curve(a)
- ppc.add_curve(b)
- ppc.min()
- ppc.max()
- ppc(0.4)
- self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
- ppc.derivate(0.4, 2)
- ppc.is_continuous(0)
- ppc.is_continuous(1)
- return
-
- def test_piecewise_bezier6_curve(self):
- # To test :
- # - Functions : constructor, min, max, derivate, add_curve, is_continuous
- waypoints = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).transpose()
- a = bezier6(waypoints, 0., 1.)
- b = bezier6(waypoints, 1., 2.)
- ppc = piecewise_bezier6_curve(a)
- ppc.add_curve(b)
- ppc.min()
- ppc.max()
- ppc(0.4)
- self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
- ppc.derivate(0.4, 2)
- ppc.is_continuous(0)
- ppc.is_continuous(1)
- return
-
- def test_piecewise_cubic_hermite_curve(self):
- # To test :
- # - Functions : constructor, min, max, derivate, add_curve, is_continuous
- points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
- tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
- time_points0 = matrix([0., 1.]).transpose()
- time_points1 = matrix([1., 2.]).transpose()
- a = cubic_hermite_spline(points, tangents, time_points0)
- b = cubic_hermite_spline(points, tangents, time_points1)
- ppc = piecewise_cubic_hermite_curve(a)
- ppc.add_curve(b)
- ppc.min()
- ppc.max()
- ppc(0.4)
- self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
- ppc.derivate(0.4, 2)
- ppc.is_continuous(0)
- ppc.is_continuous(1)
- return
-
- def test_exact_cubic(self):
- # To test :
- # - Functions : constructor, min, max, derivate, getNumberSplines, getSplineAt
- waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
- time_waypoints = matrix([0., 1.]).transpose()
- a = exact_cubic(waypoints, time_waypoints)
- a.min()
- a.max()
- a(0.4)
- self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
- a.derivate(0.4, 2)
- a.getNumberSplines()
- a.getSplineAt(0)
- return
-
- def test_exact_cubic_constraint(self):
- # To test :
- # - Functions : constructor, min, max, derivate
- waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
- time_waypoints = matrix([0., 1.]).transpose()
- c = curve_constraints()
- c.init_vel
- c.end_vel
- c.init_acc
- c.end_acc
- 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., 1., 1.]).transpose()
- a = exact_cubic(waypoints, time_waypoints)
- a = exact_cubic(waypoints, time_waypoints, c)
- 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()
- time_points = matrix([0., 1.]).transpose()
- a = cubic_hermite_spline(points, tangents, time_points)
- a.min()
- a.max()
- a(0.4)
- self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
- a.derivate(0.4, 2)
- return
-
- def test_conversion_curves(self):
- __EPS = 1e-6
- waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
- a = bezier3(waypoints)
- # converting bezier to polynomial
- a_pol = polynomial_from_bezier(a)
- self.assertTrue (norm(a(0.3) - a_pol(0.3)) < __EPS)
- # converting polynomial to hermite
- a_chs = hermite_from_polynomial(a_pol);
- self.assertTrue (norm(a_chs(0.3) - a_pol(0.3)) < __EPS)
- # converting hermite to bezier
- a_bc = bezier_from_hermite(a_chs);
- self.assertTrue (norm(a_chs(0.3) - a_bc(0.3)) < __EPS)
- self.assertTrue (norm(a(0.3) - a_bc(0.3)) < __EPS)
- # converting bezier to hermite
- a_chs = hermite_from_bezier(a);
- self.assertTrue (norm(a(0.3) - a_chs(0.3)) < __EPS)
- # converting hermite to polynomial
- a_pol = polynomial_from_hermite(a_chs)
- self.assertTrue (norm(a_pol(0.3) - a_chs(0.3)) < __EPS)
- # converting polynomial to bezier
- a_bc = bezier_from_polynomial(a_pol)
- self.assertTrue (norm(a_bc(0.3) - a_pol(0.3)) < __EPS)
- self.assertTrue (norm(a(0.3) - a_bc(0.3)) < __EPS)
- return
+ #def print_str(self, inStr):
+ # print inStr
+ # return
+
+ def test_bezier(self):
+ # To test :
+ # - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive
+ # - Variables : degree, nbWayPoints
+ __EPS = 1e-6
+ waypoints = matrix([[1., 2., 3.]]).T
+ a = bezier3(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()
+ waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).transpose()
+ time_waypoints = matrix([0., 1.]).transpose()
+ # Create bezier6 and bezier3
+ a = bezier6(waypoints6)
+ a = bezier3(waypoints, 0., 3.)
+ # Test : Degree, min, max, derivate
+ #self.print_str(("test 1")
+ self.assertEqual (a.degree, a.nbWaypoints - 1)
+ a.min()
+ a.max()
+ a(0.4)
+ self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
+ a.derivate(0.4, 2)
+ a = a.compute_derivate(100)
+ prim = a.compute_primitive(1)
+ # Check primitive and derivate - order 1
+ for i in range(10):
+ t = float(i) / 10.
+ self.assertTrue ((a(t) == prim.derivate(t, 1)).all())
+ self.assertTrue ((prim(0) == matrix([0., 0., 0.])).all())
+ # Check primitive and derivate - order 2
+ prim = a.compute_primitive(2)
+ for i in range(10):
+ 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
+ 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.)
+ prim0 = a0.compute_primitive(1)
+ prim1 = a1.compute_primitive(1)
+ # Check change in argument time_t of bezier3
+ for i in range(10):
+ t = float(i) / 10.
+ self.assertTrue (norm(a0(t) - a1(3 * t)) < __EPS)
+ self.assertTrue (norm(a0.derivate(t, 1) - a1.derivate(3 * t, 1) * 3.) < __EPS)
+ self.assertTrue (norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
+ 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_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)
+ self.assertTrue (norm(a.derivate(0, 1) - c.init_vel) < 1e-10)
+ self.assertTrue (norm(a.derivate(1, 2) - c.end_acc) < 1e-10)
+ return
+
+ def test_polynomial(self):
+ # To test :
+ # - Functions : constructor, min, max, derivate
+ 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)
+ self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
+ a.derivate(0.4, 2)
+ return
+
+ def test_piecewise_polynomial_curve(self):
+ # To test :
+ # - Functions : constructor, min, max, derivate, add_curve, is_continuous
+ waypoints1 = matrix([[1., 1., 1.]]).transpose()
+ waypoints2 = matrix([[1., 1., 1.], [1., 1., 1.]]).transpose()
+ a = polynomial(waypoints1, 0.,1.)
+ b = polynomial(waypoints2, 1., 3.)
+ ppc = piecewise_polynomial_curve(a)
+ ppc.add_curve(b)
+ ppc.min()
+ ppc.max()
+ ppc(0.4)
+ self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
+ ppc.derivate(0.4, 2)
+ ppc.is_continuous(0)
+ ppc.is_continuous(1)
+ return
+
+ def test_piecewise_bezier3_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.)
+ ppc = piecewise_bezier3_curve(a)
+ ppc.add_curve(b)
+ ppc.min()
+ ppc.max()
+ ppc(0.4)
+ self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
+ ppc.derivate(0.4, 2)
+ ppc.is_continuous(0)
+ ppc.is_continuous(1)
+ return
+
+ def test_piecewise_bezier6_curve(self):
+ # To test :
+ # - Functions : constructor, min, max, derivate, add_curve, is_continuous
+ waypoints = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).transpose()
+ a = bezier6(waypoints, 0., 1.)
+ b = bezier6(waypoints, 1., 2.)
+ ppc = piecewise_bezier6_curve(a)
+ ppc.add_curve(b)
+ ppc.min()
+ ppc.max()
+ ppc(0.4)
+ self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
+ ppc.derivate(0.4, 2)
+ ppc.is_continuous(0)
+ ppc.is_continuous(1)
+ return
+
+ def test_piecewise_cubic_hermite_curve(self):
+ # To test :
+ # - Functions : constructor, min, max, derivate, add_curve, is_continuous
+ points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
+ tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
+ time_points0 = matrix([0., 1.]).transpose()
+ time_points1 = matrix([1., 2.]).transpose()
+ a = cubic_hermite_spline(points, tangents, time_points0)
+ b = cubic_hermite_spline(points, tangents, time_points1)
+ ppc = piecewise_cubic_hermite_curve(a)
+ ppc.add_curve(b)
+ ppc.min()
+ ppc.max()
+ ppc(0.4)
+ self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
+ ppc.derivate(0.4, 2)
+ ppc.is_continuous(0)
+ ppc.is_continuous(1)
+ return
+
+ def test_exact_cubic(self):
+ # To test :
+ # - Functions : constructor, min, max, derivate, getNumberSplines, getSplineAt
+ waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
+ time_waypoints = matrix([0., 1.]).transpose()
+ a = exact_cubic(waypoints, time_waypoints)
+ a.min()
+ a.max()
+ a(0.4)
+ self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
+ a.derivate(0.4, 2)
+ a.getNumberSplines()
+ a.getSplineAt(0)
+ return
+
+ def test_exact_cubic_constraint(self):
+ # To test :
+ # - Functions : constructor, min, max, derivate
+ waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
+ time_waypoints = matrix([0., 1.]).transpose()
+ c = curve_constraints()
+ c.init_vel
+ c.end_vel
+ c.init_acc
+ c.end_acc
+ 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., 1., 1.]).transpose()
+ a = exact_cubic(waypoints, time_waypoints)
+ a = exact_cubic(waypoints, time_waypoints, c)
+ 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()
+ time_points = matrix([0., 1.]).transpose()
+ a = cubic_hermite_spline(points, tangents, time_points)
+ a.min()
+ a.max()
+ a(0.4)
+ self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
+ a.derivate(0.4, 2)
+ return
+
+ def test_conversion_curves(self):
+ __EPS = 1e-6
+ waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
+ a = bezier3(waypoints)
+ # converting bezier to polynomial
+ a_pol = polynomial_from_bezier(a)
+ self.assertTrue (norm(a(0.3) - a_pol(0.3)) < __EPS)
+ # converting polynomial to hermite
+ a_chs = hermite_from_polynomial(a_pol);
+ self.assertTrue (norm(a_chs(0.3) - a_pol(0.3)) < __EPS)
+ # converting hermite to bezier
+ a_bc = bezier_from_hermite(a_chs);
+ self.assertTrue (norm(a_chs(0.3) - a_bc(0.3)) < __EPS)
+ self.assertTrue (norm(a(0.3) - a_bc(0.3)) < __EPS)
+ # converting bezier to hermite
+ a_chs = hermite_from_bezier(a);
+ self.assertTrue (norm(a(0.3) - a_chs(0.3)) < __EPS)
+ # converting hermite to polynomial
+ a_pol = polynomial_from_hermite(a_chs)
+ self.assertTrue (norm(a_pol(0.3) - a_chs(0.3)) < __EPS)
+ # converting polynomial to bezier
+ a_bc = bezier_from_polynomial(a_pol)
+ self.assertTrue (norm(a_bc(0.3) - a_pol(0.3)) < __EPS)
+ self.assertTrue (norm(a(0.3) - a_bc(0.3)) < __EPS)
+ return
if __name__ == '__main__':
unittest.main()
diff --git a/tests/Main.cpp b/tests/Main.cpp
index 536992f..bd1cfbb 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -41,21 +41,13 @@ typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, polynomial
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-9;
-bool QuasiEqual(const double a, const double b, const float margin)
+bool QuasiEqual(const double a, const double b)
{
- if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0))
- {
- return (abs(a-b)) <= margin;
- }
- else
- {
- return abs(a) + abs(b) <= margin;
- }
+ return std::fabs(a-b)<margin;
}
-const double margin = 0.001;
-
} // namespace curves
using namespace curves;
@@ -68,7 +60,7 @@ ostream& operator<<(ostream& os, const point_t& pt)
void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const std::string& errmsg, bool& error, bool notequal = false)
{
- if((pt1-pt2).norm() > margin && !notequal)
+ if(!QuasiEqual((pt1-pt2).norm(), 0.0) && !notequal)
{
error = true;
std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl;
@@ -76,13 +68,13 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const
}
template<typename curve1, typename curve2>
-void compareCurves(curve1 c1, curve2 c2, const std::string& errMsg, bool& error)
+void CompareCurves(curve1 c1, curve2 c2, const std::string& errMsg, bool& error)
{
double T_min = c1.min();
double T_max = c1.max();
- if (T_min!=c2.min() || T_max!=c2.max())
+ if (!QuasiEqual(T_min, c2.min()) || !QuasiEqual(T_max, c2.max()))
{
- std::cout << "compareCurves, time min and max of curves does not match ["<<T_min<<","<<T_max<<"] "
+ std::cout << "CompareCurves, ERROR, time min and max of curves do not match ["<<T_min<<","<<T_max<<"] "
<< " and ["<<c2.min()<<","<<c2.max()<<"] "<<std::endl;
error = true;
}
@@ -161,12 +153,12 @@ void CubicFunctionTest(bool& error)
{
std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
}
- if(cf.max() != 1)
+ if (!QuasiEqual(cf.max(), 1.0))
{
error = true;
std::cout << "Evaluation of cubic cf error, MaxBound should be equal to 1\n";
}
- if(cf.min() != 0)
+ if (!QuasiEqual(cf.min(), 0.0))
{
error = true;
std::cout << "Evaluation of cubic cf error, MinBound should be equal to 1\n";
@@ -257,12 +249,12 @@ void BezierCurveTest(bool& error)
std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
error = true;
}
- if(cf.max() != 1)
+ if (!QuasiEqual(cf.max(),1.0))
{
error = true;
std::cout << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
}
- if(cf.min() != 0)
+ if (!QuasiEqual(cf.min(),0.0))
{
error = true;
std::cout << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
@@ -426,7 +418,7 @@ void BezierDerivativeCurveTimeReparametrizationTest(bool& error)
void BezierDerivativeCurveConstraintTest(bool& error)
{
- std::string errMsg("In test BezierDerivativeCurveConstraintTest, Error While checking value of point on curve : ");
+ std::string errMsg0("In test BezierDerivativeCurveConstraintTest, Error While checking value of point on curve : ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
@@ -446,16 +438,26 @@ void BezierDerivativeCurveConstraintTest(bool& error)
bezier_curve_t::num_t T_max = 3.0;
bezier_curve_t cf(params.begin(), params.end(), constraints, T_min, T_max);
- assert(cf.degree_ == params.size() + 3);
- assert(cf.size_ == params.size() + 4);
-
- ComparePoints(a, cf(T_min), errMsg, error);
- ComparePoints(c, cf(T_max), errMsg, error);
- ComparePoints(constraints.init_vel, cf.derivate(T_min,1), errMsg, error);
- ComparePoints(constraints.end_vel , cf.derivate(T_max,1), errMsg, error);
- ComparePoints(constraints.init_acc, cf.derivate(T_min,2), errMsg, error);
- ComparePoints(constraints.end_vel, cf.derivate(T_max,1), errMsg, error);
- ComparePoints(constraints.end_acc, cf.derivate(T_max,2), errMsg, error);
+ ComparePoints(a, cf(T_min), errMsg0, error);
+ ComparePoints(c, cf(T_max), errMsg0, error);
+ ComparePoints(constraints.init_vel, cf.derivate(T_min,1), errMsg0, error);
+ ComparePoints(constraints.end_vel , cf.derivate(T_max,1), errMsg0, error);
+ ComparePoints(constraints.init_acc, cf.derivate(T_min,2), errMsg0, error);
+ ComparePoints(constraints.end_vel, cf.derivate(T_max,1), errMsg0, error);
+ ComparePoints(constraints.end_acc, cf.derivate(T_max,2), errMsg0, error);
+
+ std::string errMsg1("In test BezierDerivativeCurveConstraintTest, Error While checking checking degree of bezier curve :");
+ std::string errMsg2("In test BezierDerivativeCurveConstraintTest, Error While checking checking size of bezier curve :");
+ if (cf.degree_ != params.size() + 3)
+ {
+ error = true;
+ std::cout << errMsg1 << cf.degree_ << " ; " << params.size()+3 << std::endl;
+ }
+ if (cf.size_ != params.size() + 4)
+ {
+ error = true;
+ std::cout << errMsg2 << cf.size_ << " ; " << params.size()+4 << std::endl;
+ }
}
@@ -488,7 +490,7 @@ void toPolynomialConversionTest(bool& error)
bezier_curve_t::num_t T_max = 3.0;
bezier_curve_t bc(control_points.begin(), control_points.end(),T_min, T_max);
polynomial_t pol = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc);
- compareCurves<polynomial_t, bezier_curve_t>(pol, bc, errMsg, error);
+ CompareCurves<polynomial_t, bezier_curve_t>(pol, bc, errMsg, error);
}
void cubicConversionTest(bool& error)
@@ -518,37 +520,37 @@ void cubicConversionTest(bool& error)
//std::cout<<"======================= \n";
//std::cout<<"hermite to bezier \n";
bezier_curve_t bc0 = bezier_from_curve<bezier_curve_t, cubic_hermite_spline_t>(chs0);
- compareCurves<cubic_hermite_spline_t, bezier_curve_t>(chs0, bc0, errMsg0, error);
+ CompareCurves<cubic_hermite_spline_t, bezier_curve_t>(chs0, bc0, errMsg0, error);
// hermite to pol
//std::cout<<"======================= \n";
//std::cout<<"hermite to polynomial \n";
polynomial_t pol0 = polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>(chs0);
- compareCurves<cubic_hermite_spline_t, polynomial_t>(chs0, pol0, errMsg0, error);
+ CompareCurves<cubic_hermite_spline_t, polynomial_t>(chs0, pol0, errMsg0, error);
// pol to hermite
//std::cout<<"======================= \n";
//std::cout<<"polynomial to hermite \n";
cubic_hermite_spline_t chs1 = hermite_from_curve<cubic_hermite_spline_t, polynomial_t>(pol0);
- compareCurves<polynomial_t, cubic_hermite_spline_t>(pol0,chs1,errMsg2,error);
+ CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol0,chs1,errMsg2,error);
// pol to bezier
//std::cout<<"======================= \n";
//std::cout<<"polynomial to bezier \n";
bezier_curve_t bc1 = bezier_from_curve<bezier_curve_t, polynomial_t>(pol0);
- compareCurves<bezier_curve_t, polynomial_t>(bc1, pol0, errMsg2, error);
+ CompareCurves<bezier_curve_t, polynomial_t>(bc1, pol0, errMsg2, error);
// Bezier to pol
//std::cout<<"======================= \n";
//std::cout<<"bezier to polynomial \n";
polynomial_t pol1 = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc0);
- compareCurves<bezier_curve_t, polynomial_t>(bc0, pol1, errMsg1, error);
+ CompareCurves<bezier_curve_t, polynomial_t>(bc0, pol1, errMsg1, error);
// bezier => hermite
//std::cout<<"======================= \n";
//std::cout<<"bezier to hermite \n";
cubic_hermite_spline_t chs2 = hermite_from_curve<cubic_hermite_spline_t, bezier_curve_t>(bc0);
- compareCurves<bezier_curve_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);
+ CompareCurves<bezier_curve_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);
}
/*Exact Cubic Function tests*/
@@ -564,7 +566,12 @@ void ExactCubicNoErrorTest(bool& error)
// Test number of polynomials in exact cubic
std::size_t numberSegments = exactCubic.getNumberSplines();
- assert(numberSegments == 6);
+ if (numberSegments != 6)
+ {
+ error = true;
+ std::cout << "In ExactCubicNoErrorTest, Error While checking number of splines" <<
+ numberSegments << " ; " << 6 << std::endl;
+ }
// Test getSplineAt function
for (std::size_t i=0; i<numberSegments; i++)
@@ -597,12 +604,12 @@ void ExactCubicNoErrorTest(bool& error)
std::cout << "Evaluation of exactCubic cf error, 3.2 should be an out of range value\n";
}
- if(exactCubic.max() != 3.)
+ if (!QuasiEqual(exactCubic.max(),3.0))
{
error = true;
std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 3\n";
}
- if(exactCubic.min() != 0.)
+ if (!QuasiEqual(exactCubic.min(),0.0))
{
error = true;
std::cout << "Evaluation of exactCubic error, MinBound should be equal to 0\n";
@@ -621,19 +628,24 @@ void ExactCubicTwoPointsTest(bool& error)
exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
point_t res1 = exactCubic(0);
- std::string errmsg("in ExactCubic 2 points Error While checking that given wayPoints are crossed (expected / obtained)");
- ComparePoints(point_t(0,0,0), res1, errmsg, error);
+ std::string errmsg0("in ExactCubicTwoPointsTest, Error While checking that given wayPoints are crossed (expected / obtained)");
+ ComparePoints(point_t(0,0,0), res1, errmsg0, error);
res1 = exactCubic(1);
- ComparePoints(point_t(1,1,1), res1, errmsg, error);
+ ComparePoints(point_t(1,1,1), res1, errmsg0, error);
// Test number of polynomials in exact cubic
std::size_t numberSegments = exactCubic.getNumberSplines();
- assert(numberSegments == 1);
+ if (numberSegments != 1)
+ {
+ error = true;
+ std::cout << "In ExactCubicTwoPointsTest, Error While checking number of splines" <<
+ numberSegments << " ; " << 1 << std::endl;
+ }
// Test getSplineAt
- assert(exactCubic(0.5) == (exactCubic.getSplineAt(0))(0.5));
-
+ std::string errmsg1("in ExactCubicTwoPointsTest, Error While checking value on curve");
+ ComparePoints(exactCubic(0.5), (exactCubic.getSplineAt(0))(0.5), errmsg1, error);
}
void ExactCubicOneDimTest(bool& error)
@@ -879,22 +891,22 @@ void TestReparametrization(bool& error)
{
helpers::rotation_spline s;
const helpers::exact_cubic_constraint_one_dim& sp = s.time_reparam_;
- if(sp.min() != 0)
+ if (!QuasiEqual(sp.min(),0.0))
{
std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
error = true;
}
- if(sp.max() != 1)
+ if (!QuasiEqual(sp.max(),1.0))
{
std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() << std::endl;
error = true;
}
- if(sp(1)[0] != 1.)
+ if (!QuasiEqual(sp(1)[0],1.0))
{
std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] << std::endl;
error = true;
}
- if(sp(0)[0] != 0.)
+ if (!QuasiEqual(sp(0)[0],0.0))
{
std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] << std::endl;
error = true;
@@ -947,13 +959,10 @@ void BezierEvalDeCasteljau(bool& error)
// 3d curve
bezier_curve_t cf(params.begin(), params.end());
+ std::string errmsg("Error in BezierEvalDeCasteljau; while comparing actual bezier evaluation and de Casteljau : ");
for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
{
- if(cf.evalDeCasteljau(*cit) != cf(*cit))
- {
- error = true;
- std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
- }
+ ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
}
params.push_back(d);
@@ -967,11 +976,7 @@ void BezierEvalDeCasteljau(bool& error)
for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
{
- if(cf.evalDeCasteljau(*cit) != cf(*cit))
- {
- error = true;
- std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
- }
+ ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
}
}
@@ -986,6 +991,7 @@ void BezierSplitCurve(bool& error)
size_t n = 5;
double t_min = 0.2;
double t_max = 10;
+ double aux0, aux1;
for(size_t i = 0 ; i < 1 ; ++i)
{
// build a random curve and split it at random time :
@@ -1002,66 +1008,47 @@ void BezierSplitCurve(bool& error)
bezier_curve_t c(wps.begin(), wps.end(),t0, t1);
std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
- //std::cout<<"split curve of duration "<<t<<" at "<<ts<<std::endl;
// test on splitted curves :
if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) ))
{
error = true;
- std::cout<<" Degree of the splitted curve are not the same as the original curve"<<std::endl;
+ std::cout<<"BezierSplitCurve, ERROR Degree of the splitted curve are not the same as the original curve"<<std::endl;
}
-
- if(c.max()-c.min() != (cs.first.max()-cs.first.min() + cs.second.max()-cs.second.min()))
+ 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<<"Duration of the splitted curve doesn't correspond to the original"<<std::endl;
+ std::cout<<"BezierSplitCurve, ERROR duration of the splitted curve doesn't correspond to the original"<<std::endl;
}
-
- if(c(t0) != cs.first(t0))
+ if(!QuasiEqual(cs.first.max(), ts))
{
error = true;
- std::cout<<"initial point of the splitted curve doesn't correspond to the original"<<std::endl;
+ std::cout<<"BezierSplitCurve, ERROR timing of the splitted curve doesn't correspond to the original"<<std::endl;
}
- if(c(t1) != cs.second(cs.second.max()))
- {
- error = true;
- std::cout<<"final point of the splitted curve doesn't correspond to the original"<<std::endl;
- }
- if(cs.first.max() != ts)
- {
- error = true;
- std::cout<<"timing of the splitted curve doesn't correspond to the original"<<std::endl;
- }
-
- if(cs.first(ts) != cs.second(ts))
- {
- error = true;
- std::cout<<"splitting point 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)
{
- if((cs.first(ti) - c(ti)).norm() > 1e-14)
- {
- error = true;
- std::cout<<"first splitted curve and original curve doesn't correspond, error = "<<cs.first(ti) - c(ti) <<std::endl;
- }
+ ComparePoints(cs.first(ti), c(ti), errmsg, error);
ti += 0.01;
}
while(ti <= t1)
{
- if((cs.second(ti) - c(ti)).norm() > 1e-14)
- {
- error = true;
- std::cout<<"second splitted curve and original curve doesn't correspond, error = "<<cs.second(ti-ts) - c(ti)<<std::endl;
- }
+ ComparePoints(cs.second(ti), c(ti), errmsg, error);
ti += 0.01;
}
-
}
}
--
GitLab