From 73d79f8194422fa135e6573cedff3ca8feff864b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A9o=20Valque?= Date: Tue, 18 Feb 2025 13:36:55 +0100 Subject: [PATCH] compare square distance Seg3 Seg3 --- .../CGAL/Distance_3/Segment_3_Segment_3.h | 112 ++++++++++++++++-- .../CGAL/Distance_3/Triangle_3_Triangle_3.h | 42 +++++++ 2 files changed, 146 insertions(+), 8 deletions(-) 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 ad933787204..12bba07926e 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 @@ -59,14 +59,6 @@ squared_distance(const typename K::Segment_3& s1, const Vector_3 v1 = cv(p1, q1), v2 = cv(p2, q2); const Vector_3 p1p2 = cv(p1, p2); - // @todo compute these only when needed? - const FT a = sp(v1, v1); - const FT b = - sp(v1, v2); - const FT c = - b; - const FT d = - sp(v2, v2); - const FT e = sp(v1, p1p2); - const FT f = sp(v2, p1p2); - if(p1 == q1) { if(p2 == q2) @@ -77,6 +69,9 @@ squared_distance(const typename K::Segment_3& s1, return res; } + const FT d = - sp(v2, v2); + const FT f = sp(v2, p1p2); + CGAL_assertion(d < 0); res.x = 0; @@ -87,6 +82,9 @@ squared_distance(const typename K::Segment_3& s1, } else if(p2 == q2) { + const FT a = sp(v1, v1); + const FT e = sp(v1, p1p2); + CGAL_assertion(a > 0); res.y = 0; @@ -96,6 +94,13 @@ squared_distance(const typename K::Segment_3& s1, return res; } + const FT a = sp(v1, v1); + const FT b = - sp(v1, v2); + const FT c = - b; + const FT d = - sp(v2, v2); + const FT e = sp(v1, p1p2); + const FT f = sp(v2, p1p2); + CGAL_assertion(a > 0 && d < 0); const FT det = a*d - b*c; @@ -135,6 +140,97 @@ squared_distance(const typename K::Segment_3& s1, return res; } +// Using Lumelsky, 'On Fast Computation of Distance Between Line Segments' 1984 +template +typename K::Comparison_result +compare_squared_distance(const typename K::Segment_3& s1, + const typename K::Segment_3& s2, + const typename K::FT& dist, + const K& k) +{ + typedef typename K::FT FT; + typedef typename K::Point_3 Point_3; + typedef typename K::Vector_3 Vector_3; + + typename K::Construct_vertex_3 vertex = k.construct_vertex_3_object(); + typename K::Construct_vector_3 cv = k.construct_vector_3_object(); + typename K::Compute_scalar_product_3 sp = k.compute_scalar_product_3_object(); + typename K::Compute_squared_distance_3 sq_dist = k.compute_squared_distance_3_object(); + + const Point_3& p1 = vertex(s1, 0); + const Point_3& q1 = vertex(s1, 1); + const Point_3& p2 = vertex(s2, 0); + const Point_3& q2 = vertex(s2, 1); + const Vector_3 v1 = cv(p1, q1), v2 = cv(p2, q2); + const Vector_3 p1p2 = cv(p1, p2); + + //If degenerate segment, compare distance point segment + if(p1 == q1) + if(p2 == q2) + return compare_square_distance(p1,p2,dist,k); + else + return compare_square_distance(p1,s2,dist,k); + else if(p2 == q2) + return compare_square_distance(s1,p2,dist,k); + + //Compare distance between the lines + const Vector_3 normal = wcross(v1, v2, k); + const Vector_3 diff = p1p2; + + //If the lines are at distance more than d, the segment do not be at distance less than d + typename K::Comparison_result res_ll=compare_squared_distance(v1.supporting_line(), v2.supporting_line(), dist, k); + if(res_ll==LARGER) + return LARGER; //Early exit + + //Compute distance between the segments + //Since we already compare the distance between the lines, we care about only the cases where + //the distance are realized by vertices aka res.x or res.y are 0 or 1 + + const FT a = sp(v1, v1); + const FT b = - sp(v1, v2); + const FT c = - b; + const FT d = - sp(v2, v2); + const FT e = sp(v1, p1p2); + const FT f = sp(v2, p1p2); + + CGAL_assertion(a > 0 && d < 0); + const FT det = a*d - b*c; + const FT res_x; + if(det == 0) + res_x = 0; + else + res_x = boost::algorithm::clamp((e*d - b*f) / det, 0, 1); + + FT xc = res_x*c; + // res.y = (f - xc) / d, by definition, but building it up more efficiently + if(f > xc) // y < 0 <=> f - xc / d < 0 <=> f - xc > 0 (since d is -||v2||) + { + // res_y = 0; + res_x = boost::algorithm::clamp(e/a, 0, 1); // (e + y*c) / a + return compare(sq_dist(p1+res_x*v1,p2), dist); + } + else // y >= 0 + { + FT n = f - xc; // delay the division by d + if(n < d) // y > 1 <=> n / d > 1 <=> n < d (once again, important to note that d is negative!) + { + // 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), dist); + } + else if(res_x==0 || res_x==1) // 0 <= y <= 1 + { + FT res_y = n / d; + return compare(sq_dist(p1 + res_x*v1, p2 + res_y*v2), dist); + } + else + { + //Already computed by distance line line + return res_ll; + } + } +} + } // namespace internal } // namespace Distance_3 diff --git a/Distance_3/include/CGAL/Distance_3/Triangle_3_Triangle_3.h b/Distance_3/include/CGAL/Distance_3/Triangle_3_Triangle_3.h index f6bf77fd39b..f5e66c37d21 100644 --- a/Distance_3/include/CGAL/Distance_3/Triangle_3_Triangle_3.h +++ b/Distance_3/include/CGAL/Distance_3/Triangle_3_Triangle_3.h @@ -213,8 +213,50 @@ squared_distance(const typename K::Triangle_3& tr1, #endif } +template +typename K::Comparison_result +compare_squared_distance(const typename K::Triangle_3& tr1, + const typename K::Triangle_3& tr2, + const typename K::FT& d, + const K& k) +{ + typedef typename K::FT FT; + + typename K::Construct_vertex_3 vertex = k.construct_vertex_3_object(); + + bool tr_contains_proj_p=[](const typename K::Triangle_3& tr, const typename K::Point_3 &p) + { + typename K::Point_3 p_proj= tr.supporting_plane().projection(p); + return tr.has_on(p_proj); + }; + + typename K::Comparison_result temp_res(CGAL::LARGER); + + std::pair, bool> ss_res; + for(int i=0; i<3; ++i) + { + for(int j=0; j<3; ++j) + { + if(CGAL::possibly(CGAL::compare_squared_distance(Line_3(vertex(tr1, i%3), vertex(tr1, (i+1)%3)),Line_3(vertex(tr2, j%3), vertex(tr2, (j+1)%3)),d)!=CGAL::LARGER)) + { + typename K::comparison_result res_seg_seg= CGAL::compare_squared_distance(Segment_3(vertex(tr1, i%3), vertex(tr1, (i+1)%3)),Segment_3(vertex(tr2, j%3), vertex(tr2, (j+1)%3)),d); + if(is_certain(res_seg_seg) && res_seg_seg==CGAL::SMALLER) + return CGAL::SMALLER; + } + } + + typename K::comparison_result res_v_pl= CGAL::compare_squared_distance(vertex(tr1, i), tr2.supporting_plane(),d); + if(tr_contains_proj_p(tr2, vertex(tr1, i))) + if(res_v_pl==CGAL::SMALLER) + return CGAL::SMALLER; + + } + +} + } // namespace internal + template inline typename K::FT