Commit 83feb504 authored by JasonChmn's avatar JasonChmn
Browse files

[piecewise_curve and main] Fix all (in)equalities by comparing to an epsilon

parent 4dd18a3a
...@@ -214,7 +214,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -214,7 +214,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
// //
const Time dt = (t1-t0); const Time dt = (t1-t0);
const Time alpha = (t - t0)/dt; 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; Numeric h00, h10, h01, h11;
evalCoeffs(alpha,h00,h10,h01,h11,order_derivative); 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; //std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<" h11="<<h11<<std::endl;
......
...@@ -50,7 +50,7 @@ struct linear_variable{ ...@@ -50,7 +50,7 @@ struct linear_variable{
return *this; return *this;
} }
static linear_variable_t Zero(size_t dim=0){ static linear_variable_t Zero(){
linear_variable_t w; linear_variable_t w;
w.A_ = matrix_t::Zero(); w.A_ = matrix_t::Zero();
w.b_ = point_t::Zero(); w.b_ = point_t::Zero();
......
...@@ -86,9 +86,12 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -86,9 +86,12 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
void add_curve(const curve_t& cf) void add_curve(const curve_t& cf)
{ {
// Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve. // 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); curves_.push_back(cf);
size_ = curves_.size(); size_ = curves_.size();
......
...@@ -17,230 +17,230 @@ from curves import bezier_from_hermite, bezier_from_polynomial ...@@ -17,230 +17,230 @@ from curves import bezier_from_hermite, bezier_from_polynomial
from curves import hermite_from_bezier, hermite_from_polynomial from curves import hermite_from_bezier, hermite_from_polynomial
class TestCurves(unittest.TestCase): class TestCurves(unittest.TestCase):
#def print_str(self, inStr): #def print_str(self, inStr):
# print inStr # print inStr
# return # return
def test_bezier(self): def test_bezier(self):
# To test : # To test :
# - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive # - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive
# - Variables : degree, nbWayPoints # - Variables : degree, nbWayPoints
__EPS = 1e-6 __EPS = 1e-6
waypoints = matrix([[1., 2., 3.]]).T waypoints = matrix([[1., 2., 3.]]).T
a = bezier3(waypoints,0.,2.) a = bezier3(waypoints,0.,2.)
t = 0. t = 0.
while t < 2.: while t < 2.:
self.assertTrue (norm(a(t) - matrix([1., 2., 3.]).T) < __EPS) self.assertTrue (norm(a(t) - matrix([1., 2., 3.]).T) < __EPS)
t += 0.1 t += 0.1
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 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() time_waypoints = matrix([0., 1.]).transpose()
# Create bezier6 and bezier3 # Create bezier6 and bezier3
a = bezier6(waypoints6) a = bezier6(waypoints6)
a = bezier3(waypoints, 0., 3.) a = bezier3(waypoints, 0., 3.)
# Test : Degree, min, max, derivate # Test : Degree, min, max, derivate
#self.print_str(("test 1") #self.print_str(("test 1")
self.assertEqual (a.degree, a.nbWaypoints - 1) self.assertEqual (a.degree, a.nbWaypoints - 1)
a.min() a.min()
a.max() a.max()
a(0.4) a(0.4)
self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all()) self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2) a.derivate(0.4, 2)
a = a.compute_derivate(100) a = a.compute_derivate(100)
prim = a.compute_primitive(1) prim = a.compute_primitive(1)
# Check primitive and derivate - order 1 # Check primitive and derivate - order 1
for i in range(10): for i in range(10):
t = float(i) / 10. t = float(i) / 10.
self.assertTrue ((a(t) == prim.derivate(t, 1)).all()) self.assertTrue ((a(t) == prim.derivate(t, 1)).all())
self.assertTrue ((prim(0) == matrix([0., 0., 0.])).all()) self.assertTrue ((prim(0) == matrix([0., 0., 0.])).all())
# Check primitive and derivate - order 2 # Check primitive and derivate - order 2
prim = a.compute_primitive(2) prim = a.compute_primitive(2)
for i in range(10): for i in range(10):
t = float(i) / 10. t = float(i) / 10.
self.assertTrue ((a(t) == prim.derivate(t, 2)).all()) self.assertTrue ((a(t) == prim.derivate(t, 2)).all())
self.assertTrue ((prim(0) == matrix([0., 0., 0.])).all()) self.assertTrue ((prim(0) == matrix([0., 0., 0.])).all())
# Create new bezier3 curve # Create new bezier3 curve
waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose() waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose()
a0 = bezier3(waypoints) a0 = bezier3(waypoints)
a1 = bezier3(waypoints, 0., 3.) a1 = bezier3(waypoints, 0., 3.)
prim0 = a0.compute_primitive(1) prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1) prim1 = a1.compute_primitive(1)
# Check change in argument time_t of bezier3 # Check change in argument time_t of bezier3
for i in range(10): for i in range(10):
t = float(i) / 10. t = float(i) / 10.
self.assertTrue (norm(a0(t) - a1(3 * t)) < __EPS) 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, 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(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
self.assertTrue (norm(prim0(t) - prim1(t * 3) / 3.) < __EPS) self.assertTrue (norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
self.assertTrue ((prim(0) == matrix([0., 0., 0.])).all()) self.assertTrue ((prim(0) == matrix([0., 0., 0.])).all())
# testing bezier with constraints # testing bezier with constraints
c = curve_constraints() c = curve_constraints()
c.init_vel = matrix([0., 1., 1.]).transpose() c.init_vel = matrix([0., 1., 1.]).transpose()
c.end_vel = matrix([0., 1., 1.]).transpose() c.end_vel = matrix([0., 1., 1.]).transpose()
c.init_acc = matrix([0., 1., -1.]).transpose() c.init_acc = matrix([0., 1., -1.]).transpose()
c.end_acc = matrix([0., 100., 1.]).transpose() c.end_acc = matrix([0., 100., 1.]).transpose()
#Check derivate with constraints #Check derivate with constraints
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier3(waypoints, c) a = bezier3(waypoints, c)
self.assertTrue (norm(a.derivate(0, 1) - c.init_vel) < 1e-10) self.assertTrue (norm(a.derivate(0, 1) - c.init_vel) < 1e-10)
self.assertTrue (norm(a.derivate(1, 2) - c.end_acc) < 1e-10) self.assertTrue (norm(a.derivate(1, 2) - c.end_acc) < 1e-10)
return return
def test_polynomial(self): def test_polynomial(self):
# To test : # To test :
# - Functions : constructor, min, max, derivate # - Functions : constructor, min, max, derivate
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = polynomial(waypoints) # Defined on [0.,1.] a = polynomial(waypoints) # Defined on [0.,1.]
a = polynomial(waypoints, -1., 3.) # Defined on [-1.,3.] a = polynomial(waypoints, -1., 3.) # Defined on [-1.,3.]
a.min() a.min()
a.max() a.max()
a(0.4) a(0.4)
self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all()) self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2) a.derivate(0.4, 2)
return return
def test_piecewise_polynomial_curve(self): def test_piecewise_polynomial_curve(self):
# To test : # To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous # - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints1 = matrix([[1., 1., 1.]]).transpose() waypoints1 = matrix([[1., 1., 1.]]).transpose()
waypoints2 = matrix([[1., 1., 1.], [1., 1., 1.]]).transpose() waypoints2 = matrix([[1., 1., 1.], [1., 1., 1.]]).transpose()
a = polynomial(waypoints1, 0.,1.) a = polynomial(waypoints1, 0.,1.)
b = polynomial(waypoints2, 1., 3.) b = polynomial(waypoints2, 1., 3.)
ppc = piecewise_polynomial_curve(a) ppc = piecewise_polynomial_curve(a)
ppc.add_curve(b) ppc.add_curve(b)
ppc.min() ppc.min()
ppc.max() ppc.max()
ppc(0.4) ppc(0.4)
self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all()) self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
ppc.derivate(0.4, 2) ppc.derivate(0.4, 2)
ppc.is_continuous(0) ppc.is_continuous(0)
ppc.is_continuous(1) ppc.is_continuous(1)
return return
def test_piecewise_bezier3_curve(self): def test_piecewise_bezier3_curve(self):
# To test : # To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous # - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier3(waypoints, 0., 1.) a = bezier3(waypoints, 0., 1.)
b = bezier3(waypoints, 1., 2.) b = bezier3(waypoints, 1., 2.)
ppc = piecewise_bezier3_curve(a) ppc = piecewise_bezier3_curve(a)
ppc.add_curve(b) ppc.add_curve(b)
ppc.min() ppc.min()
ppc.max() ppc.max()
ppc(0.4) ppc(0.4)
self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all()) self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
ppc.derivate(0.4, 2) ppc.derivate(0.4, 2)
ppc.is_continuous(0) ppc.is_continuous(0)
ppc.is_continuous(1) ppc.is_continuous(1)
return return
def test_piecewise_bezier6_curve(self): def test_piecewise_bezier6_curve(self):
# To test : # To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous # - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).transpose() waypoints = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).transpose()
a = bezier6(waypoints, 0., 1.) a = bezier6(waypoints, 0., 1.)
b = bezier6(waypoints, 1., 2.) b = bezier6(waypoints, 1., 2.)
ppc = piecewise_bezier6_curve(a) ppc = piecewise_bezier6_curve(a)
ppc.add_curve(b) ppc.add_curve(b)
ppc.min() ppc.min()
ppc.max() ppc.max()
ppc(0.4) ppc(0.4)
self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all()) self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
ppc.derivate(0.4, 2) ppc.derivate(0.4, 2)
ppc.is_continuous(0) ppc.is_continuous(0)
ppc.is_continuous(1) ppc.is_continuous(1)
return return
def test_piecewise_cubic_hermite_curve(self): def test_piecewise_cubic_hermite_curve(self):
# To test : # To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous # - Functions : constructor, min, max, derivate, add_curve, is_continuous
points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_points0 = matrix([0., 1.]).transpose() time_points0 = matrix([0., 1.]).transpose()
time_points1 = matrix([1., 2.]).transpose() time_points1 = matrix([1., 2.]).transpose()
a = cubic_hermite_spline(points, tangents, time_points0) a = cubic_hermite_spline(points, tangents, time_points0)
b = cubic_hermite_spline(points, tangents, time_points1) b = cubic_hermite_spline(points, tangents, time_points1)
ppc = piecewise_cubic_hermite_curve(a) ppc = piecewise_cubic_hermite_curve(a)
ppc.add_curve(b) ppc.add_curve(b)
ppc.min() ppc.min()
ppc.max() ppc.max()
ppc(0.4) ppc(0.4)
self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all()) self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
ppc.derivate(0.4, 2) ppc.derivate(0.4, 2)
ppc.is_continuous(0) ppc.is_continuous(0)
ppc.is_continuous(1) ppc.is_continuous(1)
return return
def test_exact_cubic(self): def test_exact_cubic(self):
# To test : # To test :
# - Functions : constructor, min, max, derivate, getNumberSplines, getSplineAt # - Functions : constructor, min, max, derivate, getNumberSplines, getSplineAt
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_waypoints = matrix([0., 1.]).transpose() time_waypoints = matrix([0., 1.]).transpose()
a = exact_cubic(waypoints, time_waypoints) a = exact_cubic(waypoints, time_waypoints)
a.min() a.min()
a.max() a.max()
a(0.4) a(0.4)
self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all()) self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2) a.derivate(0.4, 2)
a.getNumberSplines() a.getNumberSplines()
a.getSplineAt(0) a.getSplineAt(0)
return return
def test_exact_cubic_constraint(self): def test_exact_cubic_constraint(self):
# To test : # To test :
# - Functions : constructor, min, max, derivate # - Functions : constructor, min, max, derivate
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_waypoints = matrix([0., 1.]).transpose() time_waypoints = matrix([0., 1.]).transpose()
c = curve_constraints() c = curve_constraints()
c.init_vel c.init_vel
c.end_vel c.end_vel
c.init_acc c.init_acc
c.end_acc c.end_acc
c.init_vel = matrix([0., 1., 1.]).transpose() c.init_vel = matrix([0., 1., 1.]).transpose()
c.end_vel = matrix([0., 1., 1.]).transpose() c.end_vel = matrix([0., 1., 1.]).transpose()
c.init_acc = matrix([0., 1., 1.]).transpose() c.init_acc = matrix([0., 1., 1.]).transpose()
c.end_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)
a = exact_cubic(waypoints, time_waypoints, c) a = exact_cubic(waypoints, time_waypoints, c)
return return
def test_cubic_hermite_spline(self): def test_cubic_hermite_spline(self):
points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_points = matrix([0., 1.]).transpose() time_points = matrix([0., 1.]).transpose()
a = cubic_hermite_spline(points, tangents, time_points) a = cubic_hermite_spline(points, tangents, time_points)
a.min() a.min()
a.max() a.max()
a(0.4) a(0.4)
self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all()) self.assertTrue ((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2) a.derivate(0.4, 2)
return return
def test_conversion_curves(self): def test_conversion_curves(self):
__EPS = 1e-6 __EPS = 1e-6
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier3(waypoints) a = bezier3(waypoints)
# converting bezier to polynomial # converting bezier to polynomial
a_pol = polynomial_from_bezier(a) a_pol = polynomial_from_bezier(a)
self.assertTrue (norm(a(0.3) - a_pol(0.3)) < __EPS) self.assertTrue (norm(a(0.3) - a_pol(0.3)) < __EPS)
# converting polynomial to hermite # converting polynomial to hermite
a_chs = hermite_from_polynomial(a_pol); a_chs = hermite_from_polynomial(a_pol);
self.assertTrue (norm(a_chs(0.3) - a_pol(0.3)) < __EPS) self.assertTrue (norm(a_chs(0.3) - a_pol(0.3)) < __EPS)
# converting hermite to bezier # converting hermite to bezier
a_bc = bezier_from_hermite(a_chs); a_bc = bezier_from_hermite(a_chs);
self.assertTrue (norm(a_chs(0.3) - a_bc(0.3)) < __EPS) self.assertTrue (norm(a_chs(0.3) - a_bc(0.3)) < __EPS)
self.assertTrue (norm(a(0.3) - a_bc(0.3)) < __EPS) self.assertTrue (norm(a(0.3) - a_bc(0.3)) < __EPS)
# converting bezier to hermite # converting bezier to hermite
a_chs = hermite_from_bezier(a); a_chs = hermite_from_bezier(a);
self.assertTrue (norm(a(0.3) - a_chs(0.3)) < __EPS) self.assertTrue (norm(a(0.3) - a_chs(0.3)) < __EPS)
# converting hermite to polynomial # converting hermite to polynomial
a_pol = polynomial_from_hermite(a_chs) a_pol = polynomial_from_hermite(a_chs)
self.assertTrue (norm(a_pol(0.3) - a_chs(0.3)) < __EPS) self.assertTrue (norm(a_pol(0.3) - a_chs(0.3)) < __EPS)
# converting polynomial to bezier # converting polynomial to bezier
a_bc = bezier_from_polynomial(a_pol) a_bc = bezier_from_polynomial(a_pol)
self.assertTrue (norm(a_bc(0.3) - a_pol(0.3)) < __EPS) self.assertTrue (norm(a_bc(0.3) - a_pol(0.3)) < __EPS)
self.assertTrue (norm(a(0.3) - a_bc(0.3)) < __EPS) self.assertTrue (norm(a(0.3) - a_bc(0.3)) < __EPS)
return return
if __name__ == '__main__': if __name__ == '__main__':
unittest.main() unittest.main()
...@@ -41,21 +41,13 @@ typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, polynomial ...@@ -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, 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 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 std::fabs(a-b)<margin;
{
return (abs(a-b)) <= margin;
}
else
{
return abs(a) + abs(b) <= margin;
}
} }
const double margin = 0.001;
} // namespace curves } // namespace curves
using namespace curves; using namespace curves;
...@@ -68,7 +60,7 @@ ostream& operator<<(ostream& os, const point_t& pt) ...@@ -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) 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; error = true;
std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl; std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl;
...@@ -76,13 +68,13 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const ...@@ -76,13 +68,13 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const
} }
template<typename curve1, typename curve2> 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_min = c1.min();
double T_max = c1.max(); 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; << " and ["<<c2.min()<<","<<c2.max()<<"] "<<std::endl;
error = true; error = true;
} }
...@@ -161,12 +153,12 @@ void CubicFunctionTest(bool& error) ...@@ -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"; 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; error = true;
std::cout << "Evaluation of cubic cf error, MaxBound should be equal to 1\n"; 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; error = true;
std::cout << "Evaluation of cubic cf error, MinBound should be equal to 1\n"; std::cout << "Evaluation of cubic cf error, MinBound should be equal to 1\n";
...@@ -257,12 +249,12 @@ void BezierCurveTest(bool& error) ...@@ -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"; std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
error = true; error = true;
} }
if(cf.max() != 1) if (!QuasiEqual(cf.max(),1.0))
{ {
error = true;