diff --git a/Distance_3/include/CGAL/Distance_3/Point_3_Segment_3.h b/Distance_3/include/CGAL/Distance_3/Point_3_Segment_3.h index 1a94bee31f5..aa76b52e66c 100644 --- a/Distance_3/include/CGAL/Distance_3/Point_3_Segment_3.h +++ b/Distance_3/include/CGAL/Distance_3/Point_3_Segment_3.h @@ -127,7 +127,11 @@ compare_squared_distance(const typename K::Point_3& pt, const Vector_3 diff = vector(seg.source(), pt); const Vector_3 segvec = vector(seg.source(), seg.target()); - //Compare first the distance to the line, if larger we can exit early + // If the segment is degen + if(seg.source()==seg.target()) + return csq_dist(pt, seg.source(), d2); + + // Compare first the distance to the line, if larger we can exit early const typename K::Comparison_result res_pl= compare_squared_distance_to_line(segvec, diff, k, d2); if(res_pl==LARGER) return LARGER; diff --git a/Distance_3/include/CGAL/Distance_3/Point_3_Triangle_3.h b/Distance_3/include/CGAL/Distance_3/Point_3_Triangle_3.h index 02002ff2600..5ca5242e428 100644 --- a/Distance_3/include/CGAL/Distance_3/Point_3_Triangle_3.h +++ b/Distance_3/include/CGAL/Distance_3/Point_3_Triangle_3.h @@ -299,85 +299,101 @@ compare_squared_distance_to_triangle(const typename K::Point_3& pt, const typename K::Point_3& t1, const typename K::Point_3& t2, const K& k, - const typename K::Ft &d2, + const typename K::FT &d2, bool& inside) { - // typedef typename K::Vector_3 Vector_3; + typedef typename K::Vector_3 Vector_3; - // typename K::Construct_segment_3 segment = k.construct_segment_3_object(); - // typename K::Construct_vector_3 vector = k.construct_vector_3_object(); - // typename K::Compute_squared_distance_3 sq_dist = k.compute_squared_distance_3_object(); + typename K::Construct_segment_3 segment = k.construct_segment_3_object(); + typename K::Construct_vector_3 vector = k.construct_vector_3_object(); + typename K::Compute_squared_distance_3 sq_dist = k.compute_squared_distance_3_object(); + typename K::Compare_squared_distance_3 csq_dist = k.compare_squared_distance_3_object(); - // const Vector_3 e1 = vector(t0, t1); - // const Vector_3 oe3 = vector(t0, t2); - // const Vector_3 normal = wcross(e1, oe3, k); + const Vector_3 e1 = vector(t0, t1); + const Vector_3 oe3 = vector(t0, t2); + const Vector_3 normal = wcross(e1, oe3, k); + const Vector_3 diff = vector(pt, t0); - // if(normal == NULL_VECTOR) - // { - // // The case normal == NULL_VECTOR covers the case when the triangle - // // is colinear, or even more degenerate. In that case, we can - // // simply take also the distance to the three segments. - // // - // // Note that in the degenerate case, at most 2 edges cover the full triangle, - // // and only two distances could be used, but leaving 3 for the case of - // // inexact constructions as it might improve the accuracy. - // typename K::FT d1 = sq_dist(pt, segment(t2, t0)); - // typename K::FT d2 = sq_dist(pt, segment(t1, t2)); - // typename K::FT d3 = sq_dist(pt, segment(t0, t1)); + if(normal == NULL_VECTOR) + { + // The case normal == NULL_VECTOR covers the case when the triangle + // is colinear, or even more degenerate. In that case, we can + // simply take also the distance to the three segments. + // + // Note that in the degenerate case, at most 2 edges cover the full triangle, + // and only two distances could be used + typename K::Comparison_result res1 = csq_dist(pt, segment(t2, t0), d2); + if(is_certain(res1) && (res1 == SMALLER)) + return SMALLER; + typename K::Comparison_result res2 = csq_dist(pt, segment(t1, t2), d2); + return smaller_of(res1,res2); + } - // return (std::min)((std::min)(d1, d2), d3); - // } + // Compare first the distance to the plane, if larger we can exit early + typename K::Comparison_result res_p_pl = compare(squared_distance_to_plane(normal, vector(t0, pt), k), d2); + if(is_certain(res_p_pl) && res_p_pl==LARGER) + return LARGER; - // if(!on_left_of_triangle_edge(pt, normal, t0, t1, k)) - // { - // if(!on_left_of_triangle_edge(pt, normal, t1, t2, k)) - // { - // // can't be to the right of all three segments - // return (std::min)(sq_dist(pt, segment(t0, t1)), sq_dist(pt, segment(t1, t2))); - // } - // else // on_left_of_triangle_edge(pt, normal, t1, t2, k) - // { - // if(!on_left_of_triangle_edge(pt, normal, t2, t0, k)) - // { - // return (std::min)(sq_dist(pt, segment(t0, t1)), sq_dist(pt, segment(t2, t0))); - // } - // else // on_left_of_triangle_edge(pt, normal, t2, t0, k) - // { - // return sq_dist(pt, segment(t0, t1)); - // } - // } - // } - // else // on_left_of_triangle_edge(pt, normal, t0, t1, k) - // { - // if(!on_left_of_triangle_edge(pt, normal, t1, t2, k)) - // { - // if(!on_left_of_triangle_edge(pt, normal, t2, t0, k)) - // { - // return (std::min)(sq_dist(pt, segment(t1, t2)), sq_dist(pt, segment(t2, t0))); - // } - // else // on_left_of_triangle_edge(pt, normal, t2, t0, k) - // { - // return sq_dist(pt, segment(t1, t2)); - // } - // } - // else // on_left_of_triangle_edge(pt, normal, t1, t2, k) - // { - // if(!on_left_of_triangle_edge(pt, normal, t2, t0, k)) - // { - // return sq_dist(pt, segment(t2, t0)); - // } - // else // on_left_of_triangle_edge(pt, normal, t2, t0, k) - // { - // inside = true; - // return squared_distance_to_plane(normal, vector(t0, pt), k); - // } - // } - // } + //If we are smaller when compare to a segment, we can exit early + if(!on_left_of_triangle_edge(pt, normal, t0, t1, k)) + { + typename K::Comparison_result res_p_s1 = csq_dist(pt, segment(t0, t1), d2); + if(res_p_s1==SMALLER) + return SMALLER; + if(!on_left_of_triangle_edge(pt, normal, t1, t2, k)) + { + // can't be to the right of all three segments + typename K::Comparison_result res_p_s2 = csq_dist(pt, segment(t1, t2), d2); + return smaller_of(res_p_s1, res_p_s2); + } + else // on_left_of_triangle_edge(pt, normal, t1, t2, k) + { + if(!on_left_of_triangle_edge(pt, normal, t2, t0, k)) + { + typename K::Comparison_result res_p_s3 = csq_dist(pt, segment(t2, t0), d2); + return smaller_of(res_p_s1, res_p_s3); + } + else // on_left_of_triangle_edge(pt, normal, t2, t0, k) + { + return res_p_s1; + } + } + } + else // on_left_of_triangle_edge(pt, normal, t0, t1, k) + { + if(!on_left_of_triangle_edge(pt, normal, t1, t2, k)) + { + typename K::Comparison_result res_p_s2 = csq_dist(pt, segment(t1, t2), d2); + if(res_p_s2 == SMALLER) + return SMALLER; + if(!on_left_of_triangle_edge(pt, normal, t2, t0, k)) + { + typename K::Comparison_result res_p_s3 = csq_dist(pt, segment(t2, t0), d2); + return smaller_of(res_p_s2, res_p_s3); + } + else // on_left_of_triangle_edge(pt, normal, t2, t0, k) + { + return res_p_s2; + } + } + else // on_left_of_triangle_edge(pt, normal, t1, t2, k) + { + if(!on_left_of_triangle_edge(pt, normal, t2, t0, k)) + { + return csq_dist(pt, segment(t2, t0), d2); + } + else // on_left_of_triangle_edge(pt, normal, t2, t0, k) + { + inside = true; + return res_p_pl; + } + } + } } template typename K::Comparison_result -compute_squared_distance(const typename K::Point_3& pt, +compare_squared_distance(const typename K::Point_3& pt, const typename K::Triangle_3& t, const K& k, const typename K::FT& d2) @@ -396,55 +412,16 @@ compute_squared_distance(const typename K::Point_3& pt, template typename K::Comparison_result -compute_squared_distance(const typename K::Triangle_3& t, +compare_squared_distance(const typename K::Triangle_3& t, const typename K::Point_3& pt, const K& k, const typename K::FT& d2) { - return compute_squared_distance(pt, t, k, d2); + return compare_squared_distance(pt, t, k, d2); } } // namespace internal -template -inline -typename K::FT -squared_distance(const Point_3& pt, - const Triangle_3& t) -{ - return K().compute_squared_distance_3_object()(pt, t); -} - - -template -inline -typename K::FT -squared_distance(const Triangle_3& t, - const Point_3& pt) -{ - return K().compute_squared_distance_3_object()(t, pt); -} - -template -inline -typename K::Comparison_result -compare_squared_distance(const Point_3& pt, - const Triangle_3& t, - const typename K::FT& d2) -{ - return K().compare_squared_distance_3_object()(pt, t, d2); -} - -template -inline -typename K::Comparison_result -compare_squared_distance(const Triangle_3& t, - const Point_3& pt, - const typename K::FT &d2) -{ - return K().compare_squared_distance_3_object()(t, pt, d2); -} - } // namespace CGAL #endif // CGAL_DISTANCE_3_POINT_3_TRIANGLE_3_H diff --git a/Distance_3/include/CGAL/Distance_3/Segment_3_Segment_3.h b/Distance_3/include/CGAL/Distance_3/Segment_3_Segment_3.h index 5c9562c53bb..310fd80f1e2 100644 --- a/Distance_3/include/CGAL/Distance_3/Segment_3_Segment_3.h +++ b/Distance_3/include/CGAL/Distance_3/Segment_3_Segment_3.h @@ -198,6 +198,7 @@ compare_squared_distance(const typename K::Segment_3& s1, typedef typename K::FT FT; typedef typename K::Point_3 Point_3; typedef typename K::Vector_3 Vector_3; + typedef typename K::Line_3 Line_3; typename K::Construct_vertex_3 vertex = k.construct_vertex_3_object(); typename K::Construct_vector_3 cv = k.construct_vector_3_object(); @@ -262,7 +263,7 @@ compare_squared_distance(const typename K::Segment_3& s1, { // res_y = 1; res_x = boost::algorithm::clamp((e + c) / a, 0, 1); // (e + y*c) / a - return compare(sq_dist(p1+res_x*v1,p2), d2); + return compare(sq_dist(p1+res_x*v1,p2+v2), d2); } else if(res_x==0 || res_x==1) // 0 <= y <= 1 { diff --git a/Distance_3/include/CGAL/Distance_3/internal/squared_distance_utils_3.h b/Distance_3/include/CGAL/Distance_3/internal/squared_distance_utils_3.h index 38844ecab54..43e11bdf4ab 100644 --- a/Distance_3/include/CGAL/Distance_3/internal/squared_distance_utils_3.h +++ b/Distance_3/include/CGAL/Distance_3/internal/squared_distance_utils_3.h @@ -26,6 +26,16 @@ namespace CGAL { namespace internal { +template +K_Comparison_result smaller_of(const K_Comparison_result a, const K_Comparison_result b) +{ + if((a==SMALLER) || (b==SMALLER)) + return SMALLER; + if((a==EQUAL) || (b==EQUAL)) + return EQUAL; + return LARGER; +} + template bool is_null(const typename K::Vector_3 &v, const K&) { @@ -39,7 +49,10 @@ wdot(const typename K::Vector_3 &u, const typename K::Vector_3 &v, const K&) { - return (u.hx()*v.hx() + u.hy()*v.hy() + u.hz()*v.hz()); + if constexpr(std::is_same()) + return std::fma(u.hx(), v.hx(), std::fma(u.hy(), v.hy(), u.hz()*v.hz())); + else + return (u.hx()*v.hx() + u.hy()*v.hy() + u.hz()*v.hz()); } template @@ -84,6 +97,24 @@ wdot(const typename K::Point_3 &p, return wdot_tag(p, q, r, k, tag); } +static double diff_of_products(const double a, const double b, const double c, const double d) +{ +#if 1 + double w = d * c; + double e = std::fma(c, -d, w); + double f = std::fma(a, b, -w); + return f + e; +#else + return std::fma(a, b, -c*d); +#endif +} + +template +static OFT diff_of_products(const OFT& a, const OFT& b, const OFT& c, const OFT& d) +{ + return a*b - c*d; +} + template typename K::Vector_3 wcross(const typename K::Vector_3 &u, @@ -91,10 +122,14 @@ wcross(const typename K::Vector_3 &u, const K&) { typedef typename K::Vector_3 Vector_3; + // return Vector_3( + // u.hy()*v.hz() - u.hz()*v.hy(), + // u.hz()*v.hx() - u.hx()*v.hz(), + // u.hx()*v.hy() - u.hy()*v.hx()); return Vector_3( - u.hy()*v.hz() - u.hz()*v.hy(), - u.hz()*v.hx() - u.hx()*v.hz(), - u.hx()*v.hy() - u.hy()*v.hx()); + diff_of_products(u.hy(),v.hz(),u.hz(),v.hy()), + diff_of_products(u.hz(),v.hx(),u.hx(),v.hz()), + diff_of_products(u.hx(),v.hy(),u.hy(),v.hx())); } template diff --git a/Distance_3/include/CGAL/global_functions_distance_3.h b/Distance_3/include/CGAL/global_functions_distance_3.h index 9dd35ef5c55..4dbac75e2f6 100644 --- a/Distance_3/include/CGAL/global_functions_distance_3.h +++ b/Distance_3/include/CGAL/global_functions_distance_3.h @@ -89,6 +89,7 @@ CGAL_COMPUTE_AND_COMPARE_SQUARED_DISTANCE_FUNCTION_SELF(Plane_3) CGAL_COMPUTE_AND_COMPARE_SQUARED_DISTANCE_FUNCTION(Point_3, Line_3) CGAL_COMPUTE_AND_COMPARE_SQUARED_DISTANCE_FUNCTION(Point_3, Ray_3) CGAL_COMPUTE_AND_COMPARE_SQUARED_DISTANCE_FUNCTION(Point_3, Segment_3) +CGAL_COMPUTE_AND_COMPARE_SQUARED_DISTANCE_FUNCTION(Point_3, Triangle_3) CGAL_COMPUTE_AND_COMPARE_SQUARED_DISTANCE_FUNCTION(Point_3, Plane_3) diff --git a/Distance_3/test/Distance_3/test_compare_distance_3.cpp b/Distance_3/test/Distance_3/test_compare_distance_3.cpp index 29380eaf32a..0e057eb22a5 100644 --- a/Distance_3/test/Distance_3/test_compare_distance_3.cpp +++ b/Distance_3/test/Distance_3/test_compare_distance_3.cpp @@ -7,6 +7,8 @@ #include +#include + #include #include @@ -143,15 +145,35 @@ private: // const FT res_o1o2 = K().compare_squared_distance_3_object()(o1, o2, expected_result); // const FT res_o2o1 = K().compare_squared_distance_3_object()(o2, o1, expected_result); + // std::cout << std::endl << "Test" << std::endl; + + // std::cout << o1 << std::endl << o2 << std::endl; + // std::cout << CGAL::squared_distance(o1, o2) << std::endl; + // std::cout << CGAL::squared_distance( CGAL::Cartesian_converter()(o1) , + // CGAL::Cartesian_converter()(o2)) << std::endl; + + // if constexpr(std::is_same()){ + // if(expected_result != 0){ + // std::cout << CGAL::squared_distance(o1.supporting_line(), o2.supporting_line()) << std::endl; + // std::cout << CGAL::squared_distance( CGAL::Cartesian_converter()(o1.supporting_line()) , + // CGAL::Cartesian_converter()(o2.supporting_line())) << std::endl; + // } + // } + // std::cout << CGAL::SMALLER << CGAL::EQUAL << CGAL::LARGER << std::endl; + const bool res_e_o1o2 = (CGAL::compare_squared_distance(o1, o2, expected_result) == CGAL::EQUAL); const bool res_e_o2o1 = (CGAL::compare_squared_distance(o2, o1, expected_result) == CGAL::EQUAL); - const bool res_s_o1o2 = (CGAL::compare_squared_distance(o1, o2, expected_result+1) == CGAL::SMALLER); - const bool res_s_o2o1 = (CGAL::compare_squared_distance(o2, o1, expected_result+1) == CGAL::SMALLER); - const bool res_l_o1o2 = (CGAL::compare_squared_distance(o1, o2, expected_result-1) == CGAL::LARGER); - const bool res_l_o2o1 = (CGAL::compare_squared_distance(o2, o1, expected_result-1) == CGAL::LARGER); + const bool res_s_o1o2 = (CGAL::compare_squared_distance(o1, o2, expected_result*(1+epsilon)+1) == CGAL::SMALLER); + const bool res_s_o2o1 = (CGAL::compare_squared_distance(o2, o1, expected_result*(1+epsilon)+1) == CGAL::SMALLER); + const bool res_l_o1o2 = (CGAL::compare_squared_distance(o1, o2, expected_result*(1-epsilon)-1) == CGAL::LARGER); + const bool res_l_o2o1 = (CGAL::compare_squared_distance(o2, o1, expected_result*(1-epsilon)-1) == CGAL::LARGER); - assert(res_e_o1o2); - assert(res_e_o2o1); + // The equal result is guaranted only on exact construction kernel + if(epsilon==0) + { + assert(res_e_o1o2); + assert(res_e_o2o1); + } assert(res_s_o1o2); assert(res_s_o2o1); assert(res_l_o1o2); @@ -164,8 +186,10 @@ private: // const FT res_o1o2 = K().compare_squared_distance_3_object()(o1, o2, expected_result); // const FT res_o2o1 = K().compare_squared_distance_3_object()(o2, o1, expected_result); - const bool res_o1o2 = (CGAL::compare_squared_distance(o1, o2, upper_bound) == CGAL::SMALLER); - const bool res_o2o1 = (CGAL::compare_squared_distance(o2, o1, upper_bound) == CGAL::SMALLER); + const FT up = upper_bound * (1 + epsilon) + std::numeric_limits::epsilon(); + + const bool res_o1o2 = (CGAL::compare_squared_distance(o1, o2, up) == CGAL::SMALLER); + const bool res_o2o1 = (CGAL::compare_squared_distance(o2, o1, up) == CGAL::SMALLER); assert(res_o1o2); assert(res_o2o1); @@ -188,61 +212,61 @@ private: check_compare_squared_distance(p(6, 1, 2), S{p( 2, 0, 0), p( 3, 0, 0)}, 14); } -// void P_T() -// { -// std::cout << "Point - Triangle" << std::endl; -// check_compare_squared_distance(p(0, 1, 2), T{p(0, 0, 0), p(2, 0, 0), p(0, 2, 0)}, 4); + void P_T() + { + std::cout << "Point - Triangle" << std::endl; + check_compare_squared_distance(p(0, 1, 2), T{p(0, 0, 0), p(2, 0, 0), p(0, 2, 0)}, 4); -// T t(p(0,0,0), p(3,0,0), p(3,3,0)); -// check_compare_squared_distance (p(-1, -1, 0), t, 2); -// check_compare_squared_distance (p(-1, 0, 0), t, 1); -// check_compare_squared_distance (p(0, 0, 0), t, 0); -// check_compare_squared_distance (p(1, 0, 0), t, 0); -// check_compare_squared_distance (p(4, 0, 0), t, 1); -// check_compare_squared_distance (p(1, -1, 0), t, 1); -// check_compare_squared_distance (p(1, 1, 1), t, 1); -// check_compare_squared_distance (p(1, 0, 1), t, 1); -// check_compare_squared_distance (p(0, 0, 1), t, 1); + T t(p(0,0,0), p(3,0,0), p(3,3,0)); + check_compare_squared_distance (p(-1, -1, 0), t, 2); + check_compare_squared_distance (p(-1, 0, 0), t, 1); + check_compare_squared_distance (p(0, 0, 0), t, 0); + check_compare_squared_distance (p(1, 0, 0), t, 0); + check_compare_squared_distance (p(4, 0, 0), t, 1); + check_compare_squared_distance (p(1, -1, 0), t, 1); + check_compare_squared_distance (p(1, 1, 1), t, 1); + check_compare_squared_distance (p(1, 0, 1), t, 1); + check_compare_squared_distance (p(0, 0, 1), t, 1); -// // Degenerate -// check_compare_squared_distance (p(1, 2, 3), T(p(4,3,2), p(4,3,2), p(4,3,2)), squared_distance(p(1, 2, 3), p(4,3,2))); -// check_compare_squared_distance (p(1, 2, 3), T(p(4,3,2), p(10,12,3), p(4,3,2)), squared_distance(p(1, 2, 3), p(4,3,2))); -// check_compare_squared_distance (p(0, 0, 0), T(p(4,3,2), p(4,-3,-2), p(4,3,2)), squared_distance(p(0, 0, 0), p(4,0,0))); + // // Degenerate + check_compare_squared_distance (p(1, 2, 3), T(p(4,3,2), p(4,3,2), p(4,3,2)), squared_distance(p(1, 2, 3), p(4,3,2))); + check_compare_squared_distance (p(1, 2, 3), T(p(4,3,2), p(10,12,3), p(4,3,2)), squared_distance(p(1, 2, 3), p(4,3,2))); + check_compare_squared_distance (p(0, 0, 0), T(p(4,3,2), p(4,-3,-2), p(4,3,2)), squared_distance(p(0, 0, 0), p(4,0,0))); -// // On the triangle -// check_compare_squared_distance (p(7, 1, -5), T(p(2,9,8), p(-4,-3,-5), p(7, 1, -5)), 0); // vertex -// check_compare_squared_distance (p(7, 1, -5), T(p(14,2,-10), p(-7,-1,5), p(8, 3, -1)), 0); // edge -// check_compare_squared_distance (p(1, 4, -3), T(p(0,-8,-3), p(-5,14,-3), p(10, 1, -3)), 0); // face + // On the triangle + check_compare_squared_distance (p(7, 1, -5), T(p(2,9,8), p(-4,-3,-5), p(7, 1, -5)), 0); // vertex + check_compare_squared_distance (p(7, 1, -5), T(p(14,2,-10), p(-7,-1,5), p(8, 3, -1)), 0); // edge + check_compare_squared_distance (p(1, 4, -3), T(p(0,-8,-3), p(-5,14,-3), p(10, 1, -3)), 0); // face -// // General -// check_compare_squared_distance (p(-15, 1, 0), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), 25); -// check_compare_squared_distance (p(-5, 0, 0), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), squared_distance(p(-5, 0, 0), S(p(-10, 1, 0), p(0,0,0)))); -// check_compare_squared_distance (p(0, -3, 0), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), 9); -// check_compare_squared_distance (p(3, -3, 0), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), squared_distance(p(3, -3, 0), S(p(0,0,0), p(10,0,0)))); -// check_compare_squared_distance (p(16, 1, 1), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), 38); -// check_compare_squared_distance (p(5, 5, 2), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), squared_distance(p(5, 5, 2), S(p(10,0,0), p(-10,1,0)))); + // General + check_compare_squared_distance (p(-15, 1, 0), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), 25); + check_compare_squared_distance (p(-5, 0, 0), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), squared_distance(p(-5, 0, 0), S(p(-10, 1, 0), p(0,0,0)))); + check_compare_squared_distance (p(0, -3, 0), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), 9); + check_compare_squared_distance (p(3, -3, 0), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), squared_distance(p(3, -3, 0), S(p(0,0,0), p(10,0,0)))); + check_compare_squared_distance (p(16, 1, 1), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), 38); + check_compare_squared_distance (p(5, 5, 2), T(p(-10, 1, 0), p(0,0,0), p(10,0,0)), squared_distance(p(5, 5, 2), S(p(10,0,0), p(-10,1,0)))); -// for(int i=0; i