diff --git a/Distance_3/include/CGAL/Distance_3/Point_3_Tetrahedron_3.h b/Distance_3/include/CGAL/Distance_3/Point_3_Tetrahedron_3.h index 04d148818a3..aff5010f755 100644 --- a/Distance_3/include/CGAL/Distance_3/Point_3_Tetrahedron_3.h +++ b/Distance_3/include/CGAL/Distance_3/Point_3_Tetrahedron_3.h @@ -43,10 +43,28 @@ squared_distance(const typename K::Point_3& pt, const Point_3& t2 = vertex(tet, 2); const Point_3& t3 = vertex(tet, 3); - bool dmin_initialized = false; + Orientation ori_tet = orientation(t0,t1,t2,t3); + + if(ori_tet==COPLANAR){ + //Degen Tetrahedron + //Get the minimum of three triangles (no need to test the fourth) + bool inside = false; + typename K::FT dmin = squared_distance_to_triangle(pt, t0, t1, t2, k, inside); + if(inside) + return dmin; + typename K::FT d = squared_distance_to_triangle(pt, t0, t1, t3, k, inside); + if(inside) + return d; + dmin=(std::min)(d,dmin); + d = squared_distance_to_triangle(pt, t0, t2, t3, k, inside); + return (std::min)(d,dmin); + } + + bool dmin_initialized = false; //dmin_initialized and !on_bounded_side have always the samed value typename K::FT dmin; bool inside = false; - if(orientation(t0,t1,t2, pt) == NEGATIVE) + + if(orientation(pt, t0,t1,t2) == ori_tet) { on_bounded_side = false; dmin = squared_distance_to_triangle(pt, t0, t1, t2, k, inside); @@ -55,7 +73,7 @@ squared_distance(const typename K::Point_3& pt, return dmin; } - if(orientation(t0,t3,t1, pt) == NEGATIVE) + if(orientation(pt, t0,t3,t1) == ori_tet) { on_bounded_side = false; const typename K::FT d = squared_distance_to_triangle(pt, t0, t3, t1, k, inside); @@ -73,7 +91,7 @@ squared_distance(const typename K::Point_3& pt, } } - if(orientation(t1,t3,t2, pt) == NEGATIVE) + if(orientation(pt, t1,t3,t2) == ori_tet) { on_bounded_side = false; const typename K::FT d = squared_distance_to_triangle(pt, t1, t3, t2, k, inside); @@ -91,7 +109,7 @@ squared_distance(const typename K::Point_3& pt, } } - if(orientation(t2,t3,t0, pt) == NEGATIVE) + if(orientation(pt, t2,t3,t0) == ori_tet) { on_bounded_side = false; const typename K::FT d = squared_distance_to_triangle(pt, t2, t3, t0, k, inside); @@ -125,24 +143,86 @@ squared_distance(const typename K::Tetrahedron_3& tet, return squared_distance(pt, tet, k); } +template +inline +typename K::Comparison_result +compare_squared_distance(const typename K::Point_3& pt, + const typename K::Tetrahedron_3& tet, + const K& k, + const typename K::FT& d2) +{ + typedef typename K::Point_3 Point_3; + + typename K::Construct_vertex_3 vertex = k.construct_vertex_3_object(); + typename K::Orientation_3 orientation = k.orientation_3_object(); + + bool on_bounded_side = true; + bool inside_or_far_to_the_plane = false; + const Point_3& t0 = vertex(tet, 0); + const Point_3& t1 = vertex(tet, 1); + const Point_3& t2 = vertex(tet, 2); + const Point_3& t3 = vertex(tet, 3); + + Orientation ori_tet = orientation(t0,t1,t2,t3); + + if(ori_tet==COPLANAR){ + //Degen Tetrahedron + //Get the minimum of three triangles (no need to test the fourth) + typename K::Comparison_result res = compare_squared_distance_to_triangle(pt, t0, t1, t2, k, d2, inside_or_far_to_the_plane); + if(inside_or_far_to_the_plane) + return res; + typename K::Comparison_result temp_res = compare_squared_distance_to_triangle(pt, t0, t1, t3, k, d2, inside_or_far_to_the_plane); + if(inside_or_far_to_the_plane) + return temp_res; + res=smaller_of(res,temp_res); + temp_res = compare_squared_distance_to_triangle(pt, t0, t2, t3, k, d2, inside_or_far_to_the_plane); + return smaller_of(res,temp_res); + } + + typename K::Comparison_result res=LARGER; + if(orientation(pt, t0,t1,t2) == ori_tet) + { + on_bounded_side = false; + res = compare_squared_distance_to_triangle(pt, t0, t1, t2, k, d2, inside_or_far_to_the_plane); + if(inside_or_far_to_the_plane || res==SMALLER) + return res; + } + + if(orientation(pt, t0,t3,t1) == ori_tet) + { + on_bounded_side = false; + const typename K::Comparison_result temp_res = compare_squared_distance_to_triangle(pt, t0, t3, t1, k, d2, inside_or_far_to_the_plane); + if(inside_or_far_to_the_plane || res==SMALLER) + return temp_res; + res = smaller_of(res, temp_res); + } + + if(orientation(pt, t1,t3,t2) == ori_tet) + { + on_bounded_side = false; + const typename K::Comparison_result temp_res = compare_squared_distance_to_triangle(pt, t1, t3, t2, k, d2, inside_or_far_to_the_plane); + if(inside_or_far_to_the_plane || res==SMALLER) + return temp_res; + res = smaller_of(res, temp_res); + } + + if(orientation(pt, t2,t3,t0) == ori_tet) + { + on_bounded_side = false; + const typename K::Comparison_result temp_res = compare_squared_distance_to_triangle(pt, t2, t3, t0, k, d2, inside_or_far_to_the_plane); + if(inside_or_far_to_the_plane || res==SMALLER) + return temp_res; + res = smaller_of(res, temp_res); + } + + if(on_bounded_side) + return compare(typename K::FT(0),d2); + + return res; +} + } // namespace internal -template -typename K::FT -squared_distance(const Tetrahedron_3& tet, - const Point_3& pt) -{ - return K().compute_squared_distance_3_object()(tet, pt); -} - -template -typename K::FT -squared_distance(const Point_3& pt, - const Tetrahedron_3& tet) -{ - return K().compute_squared_distance_3_object()(pt, tet); -} - } // namespace CGAL #endif // CGAL_DISTANCE_3_POINT_3_TETRAHEDRON_3_H 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 5ca5242e428..759062de81f 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 @@ -295,12 +295,12 @@ squared_distance(const typename K::Triangle_3& t, template typename K::Comparison_result compare_squared_distance_to_triangle(const typename K::Point_3& pt, - const typename K::Point_3& t0, - const typename K::Point_3& t1, - const typename K::Point_3& t2, - const K& k, - const typename K::FT &d2, - bool& inside) + const typename K::Point_3& t0, + const typename K::Point_3& t1, + const typename K::Point_3& t2, + const K& k, + const typename K::FT &d2, + bool& inside_or_far_to_the_plane) { typedef typename K::Vector_3 Vector_3; @@ -332,7 +332,10 @@ compare_squared_distance_to_triangle(const typename K::Point_3& pt, // 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) + { + inside_or_far_to_the_plane=true; return LARGER; + } //If we are smaller when compare to a segment, we can exit early if(!on_left_of_triangle_edge(pt, normal, t0, t1, k)) @@ -384,7 +387,7 @@ compare_squared_distance_to_triangle(const typename K::Point_3& pt, } else // on_left_of_triangle_edge(pt, normal, t2, t0, k) { - inside = true; + inside_or_far_to_the_plane = true; return res_p_pl; } } @@ -400,14 +403,14 @@ compare_squared_distance(const typename K::Point_3& pt, { typename K::Construct_vertex_3 vertex = k.construct_vertex_3_object(); - bool unused_inside = false; + bool unused_inside_or_far_to_the_plane = false; return compare_squared_distance_to_triangle(pt, vertex(t, 0), vertex(t, 1), vertex(t, 2), k, d2, - unused_inside); + unused_inside_or_far_to_the_plane); } template diff --git a/Distance_3/include/CGAL/Distance_3/Segment_3_Ray_3.h b/Distance_3/include/CGAL/Distance_3/Segment_3_Ray_3.h index 5943630d76f..f787fc11056 100644 --- a/Distance_3/include/CGAL/Distance_3/Segment_3_Ray_3.h +++ b/Distance_3/include/CGAL/Distance_3/Segment_3_Ray_3.h @@ -73,6 +73,119 @@ squared_distance(const typename K::Segment_3& seg, const Point_3& ss = seg.source(); const Point_3& se = seg.target(); + //TODO Me semble vachement compliqué pour un truc si simple, est-ce que y'a pas moyen de s'inspirer de Lumelsky pour segment segment + + if(ss == se) + return sq_dist(ss, ray); + + const Vector_3 raydir = ray.direction().vector(); + const Vector_3 segdir = seg.direction().vector(); + const Vector_3 normal = wcross(segdir, raydir, k); + + if(is_null(normal, k)) + return squared_distance_parallel(seg, ray, k); + + bool crossing1, crossing2; + + const Vector_3 perpend2seg = wcross(segdir, normal, k); + const Vector_3 perpend2ray = wcross(raydir, normal, k); + const Vector_3 ss_min_rs = vector(ray.source(), ss); + const Vector_3 se_min_rs = vector(ray.source(), se); + const RT sdm_ss2r = wdot(perpend2ray, ss_min_rs, k); + const RT sdm_se2r = wdot(perpend2ray, se_min_rs, k); + + if(sdm_ss2r < RT(0)) + { + crossing1 = (sdm_se2r >= RT(0)); + } + else + { + if(sdm_se2r <= RT(0)) + crossing1 = true; + else + crossing1 = (sdm_ss2r == RT(0)); + } + + const RT sdm_rs2s = - wdot(perpend2seg, ss_min_rs, k); + const RT sdm_re2s = wdot(perpend2seg, raydir, k); + if(sdm_rs2s < RT(0)) + { + crossing2 = (sdm_re2s >= RT(0)); + } else + { + if(sdm_re2s <= RT(0)) + crossing2 = true; + else + crossing2 = (sdm_rs2s == RT(0)); + } + + if(crossing1) + { + if(crossing2) + return squared_distance_to_plane(normal, ss_min_rs, k); + + return sq_dist(ray.source(), seg); + } + else + { + if(crossing2) + { + const RT dm = distance_measure_sub(sdm_ss2r, sdm_se2r, ss_min_rs, se_min_rs, k); + if(dm < RT(0)) + { + return sq_dist(ss, ray); + } + else + { + if(dm > RT(0)) + return sq_dist(se, ray); + else + // parallel, should not happen (no crossing) + return squared_distance_parallel(seg, ray, k); + } + } + else + { + const RT dm = distance_measure_sub(sdm_ss2r, sdm_se2r, ss_min_rs, se_min_rs, k); + if(dm == RT(0)) + return squared_distance_parallel(seg, ray, k); + + const FT min1 = (dm < RT(0)) ? sq_dist(ss, ray) + : sq_dist(se, ray); + const FT min2 = sq_dist(ray.source(), seg); + + return (min1 < min2) ? min1 : min2; + } + } +} + +template +typename K::FT +squared_distance(const typename K::Ray_3& ray, + const typename K::Segment_3& seg, + const K& k) +{ + return squared_distance(seg, ray, k); +} + +template +typename K::Comparison_result +compare_squared_distance(const typename K::Segment_3& seg, + const typename K::Ray_3& ray, + const K& k, + const typename K::FT& d2) +{ + typedef typename K::RT RT; + typedef typename K::FT FT; + typedef typename K::Point_3 Point_3; + typedef typename K::Vector_3 Vector_3; + + 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(); + + const Point_3& ss = seg.source(); + const Point_3& se = seg.target(); + if(ss == se) return sq_dist(ss, ray); @@ -157,15 +270,6 @@ squared_distance(const typename K::Segment_3& seg, } } -template -typename K::FT -squared_distance(const typename K::Ray_3& ray, - const typename K::Segment_3& seg, - const K& k) -{ - return squared_distance(seg, ray, k); -} - } // namespace internal template @@ -177,24 +281,6 @@ squared_distance_parallel(const Segment_3& seg, return internal::squared_distance_parallel(ray, seg, K()); } -template -inline -typename K::FT -squared_distance(const Segment_3& seg, - const Ray_3& ray) -{ - return K().compute_squared_distance_3_object()(seg, ray); -} - -template -inline -typename K::FT -squared_distance(const Ray_3& ray, - const Segment_3& seg) -{ - return K().compute_squared_distance_3_object()(ray, seg); -} - } // namespace CGAL #endif // CGAL_DISTANCE_3_SEGMENT_3_RAY_3_H 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 43e11bdf4ab..c7787758a12 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 @@ -229,6 +229,35 @@ squared_distance_to_plane(const typename K::Vector_3& normal, return Rational_traits().make_rational(num, den); } +template +void +signed_squared_distance_to_plane_RT(const typename K::Vector_3& normal, + const typename K::Vector_3& diff, + typename K::RT& num, + typename K::RT& den, + const K& k) +{ + typedef typename K::RT RT; + RT dot, squared_length; + dot = wdot(normal, diff, k); + squared_length = wdot(normal, normal, k); + num = dot<0?-square(dot):square(dot); + den = wmult((K*)0, squared_length, diff.hw(), diff.hw()); +} + +template +typename K::FT +signed_squared_distance_to_plane(const typename K::Vector_3& normal, + const typename K::Vector_3& diff, + const K& k) +{ + typedef typename K::RT RT; + typedef typename K::FT FT; + RT num, den; + signed_squared_distance_to_plane_RT(normal, diff, num, den, k); + return Rational_traits().make_rational(num, den); +} + template void squared_distance_to_line_RT(const typename K::Vector_3& dir, diff --git a/Distance_3/include/CGAL/global_functions_distance_3.h b/Distance_3/include/CGAL/global_functions_distance_3.h index 4dbac75e2f6..0a6bdb7d957 100644 --- a/Distance_3/include/CGAL/global_functions_distance_3.h +++ b/Distance_3/include/CGAL/global_functions_distance_3.h @@ -89,9 +89,11 @@ 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) +CGAL_COMPUTE_AND_COMPARE_SQUARED_DISTANCE_FUNCTION(Point_3, Triangle_3) +CGAL_COMPUTE_AND_COMPARE_SQUARED_DISTANCE_FUNCTION(Point_3, Tetrahedron_3) + +CGAL_COMPUTE_AND_COMPARE_SQUARED_DISTANCE_FUNCTION(Segment_3, Ray_3) CGAL_COMPUTE_AND_COMPARE_SQUARED_DISTANCE_FUNCTION(Line_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 0e057eb22a5..44abf29e1a2 100644 --- a/Distance_3/test/Distance_3/test_compare_distance_3.cpp +++ b/Distance_3/test/Distance_3/test_compare_distance_3.cpp @@ -259,14 +259,40 @@ private: } } - // void P_Tet() - // { - // std::cout << "Point - Tetrahedron\n"; - // check_compare_squared_distance (p(0, 0, 0), Tet(p(0, 0, 0), p( 1, 0, 0), p( 0, 1, 0), p( 0, 0, 1)), 0); - // check_compare_squared_distance (p(0, 0, 2), Tet(p(0, 0, 0), p( 1, 0, 0), p( 0, 1, 0), p( 0, 0, 1)), 1); - // check_compare_squared_distance (p(0, 0, -1), Tet(p(0, 0, 0), p( 1, 0, 0), p( 0, 1, 0), p( 0, 0, 1)), 1); - // check_compare_squared_distance (p(5, 0, 0), Tet(p(0, 0, 0), p( 1, 0, 0), p( 0, 1, 0), p( 4, 0, 1)), 2); - // } + void P_Tet() + { + std::cout << "Point - Tetrahedron\n"; + //Degenerate Tetrahedron + check_compare_squared_distance (p(1, 1, 1), Tet(p(0, 0, 0), p( 3, 0, 0), p( 0, 3, 0), p( 3, 3, 0)), 1); + check_compare_squared_distance (p(1.5, 1.5, 0), Tet(p(0, 0, 0), p( 3, 0, 0), p( 0, 3, 0), p( 3, 3, 0)), 0); + check_compare_squared_distance (p(4, 4, 0), Tet(p(0, 0, 0), p( 3, 0, 0), p( 0, 3, 0), p( 3, 3, 0)), 2); + + //Inside Tetrahedron + check_compare_squared_distance (p(1, 1, 1), Tet(p(0, 0, 0), p( 3, 0, 0), p( 0, 3, 0), p( 0, 0, 3)), 0); + check_compare_squared_distance (p(1, 1, 1), Tet(p(0, 0, 0), p( 3, 0, 0), p( 0, 0, 3), p( 0, 3, 0)), 0); + check_compare_squared_distance (p(0, 0, 0), Tet(p(0, 0, 0), p( 1, 0, 0), p( 0, 1, 0), p( 0, 0, 1)), 0); + + //General + check_compare_squared_distance (p(0, 0, 2), Tet(p(0, 0, 0), p( 1, 0, 0), p( 0, 1, 0), p( 0, 0, 1)), 1); + check_compare_squared_distance (p(0, 0, -1), Tet(p(0, 0, 0), p( 1, 0, 0), p( 0, 1, 0), p( 0, 0, 1)), 1); + check_compare_squared_distance (p(5, 0, 0), Tet(p(0, 0, 0), p( 1, 0, 0), p( 0, 1, 0), p( 4, 0, 1)), 2); + check_compare_squared_distance (p(1, 1, -1), Tet(p(0, 0, 1), p( 0, 0, 0), p( 4, 0, 0), p( 0, 4, 0)), 1); + check_compare_squared_distance (p(1, 1, -1), Tet(p(0, 0, 1), p( 0, 0, 0), p( 0, 4, 0), p( 4, 0, 0)), 1); + + for(int i=0; i