songsenand
/
cgal
mirror of https://github.com/CGAL/cgal
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

compare square distance Seg3 Seg3

This commit is contained in:
Léo Valque 2025-02-18 13:36:55 +01:00
parent 06b511cc65
commit 73d79f8194
2 changed files with 146 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
Distance_3/include/CGAL/Distance_3/Segment_3_Segment_3.h
View File

@ -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>
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<FT>((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<FT>(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<FT>((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

42
Distance_3/include/CGAL/Distance_3/Triangle_3_Triangle_3.h
View File

@ -213,8 +213,50 @@ squared_distance(const typename K::Triangle_3& tr1,
#endif
}
template <typename K>
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<Distance_3::internal::Segment_3_Segment_3_Result<K>, 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 <class K>
inline
typename K::FT