Solve bug in SQ Point-Tetrahedron, CSQ Point-Tetrahedron, improve test of these functions

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 <class K>
+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 <class K>
-typename K::FT
-squared_distance(const Tetrahedron_3<K>& tet,
-                 const Point_3<K>& pt)
-{
-  return K().compute_squared_distance_3_object()(tet, pt);
-}
-
-template <class K>
-typename K::FT
-squared_distance(const Point_3<K>& pt,
-                 const Tetrahedron_3<K>& tet)
-{
-  return K().compute_squared_distance_3_object()(pt, tet);
-}
-
 } // namespace CGAL
 
 #endif // CGAL_DISTANCE_3_POINT_3_TETRAHEDRON_3_H

Distance_3/include/CGAL/Distance_3/Point_3_Triangle_3.h

@@ -295,12 +295,12 @@ squared_distance(const typename K::Triangle_3& t,
 template <class K>
 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& t0,
-                             const typename K::Point_3& t1,
+                                     const typename K::Point_3& t1,
-                             const typename K::Point_3& t2,
+                                     const typename K::Point_3& t2,
-                             const K& k,
+                                     const K& k,
-                             const typename K::FT &d2,
+                                     const typename K::FT &d2,
-                             bool& inside)
+                                     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 <class K>

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 <class K>
+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 <class K>
+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 <class K>
-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 <class K>
@@ -177,24 +281,6 @@ squared_distance_parallel(const Segment_3<K>& seg,
   return internal::squared_distance_parallel(ray, seg, K());
 }
 
-template <class K>
-inline
-typename K::FT
-squared_distance(const Segment_3<K>& seg,
-                 const Ray_3<K>& ray)
-{
-  return K().compute_squared_distance_3_object()(seg, ray);
-}
-
-template <class K>
-inline
-typename K::FT
-squared_distance(const Ray_3<K>& ray,
-                 const Segment_3<K>& seg)
-{
-  return K().compute_squared_distance_3_object()(ray, seg);
-}
-
 } // namespace CGAL
 
 #endif // CGAL_DISTANCE_3_SEGMENT_3_RAY_3_H

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<FT>().make_rational(num, den);
 }
 
+template <class K>
+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 <class K>
+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<FT>().make_rational(num, den);
+}
+
 template <class K>
 void
 squared_distance_to_line_RT(const typename K::Vector_3& dir,

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)
 

Distance_3/test/Distance_3/test_compare_distance_3.cpp

@@ -259,14 +259,40 @@ private:
     }
   }
 
-  // void P_Tet()
+  void P_Tet()
-  // {
+  {
-  //   std::cout << "Point - Tetrahedron\n";
+    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);
+    //Degenerate Tetrahedron
-  //   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(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(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(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(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(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<N; ++i)
+    {
+      P p0 = random_point();
+      P p1 = random_point();
+      P p2 = random_point();
+      P p3 = random_point();
+      P q = random_point();
+
+      check_compare_squared_distance_with_bound(q, Tet(p0, p1, p2, p3), squared_distance(q, T(p0, p1, p2)));
+      check_compare_squared_distance_with_bound(q, Tet(p0, p1, p2, p3), squared_distance(q, T(p1, p2, p3)));
+      check_compare_squared_distance_with_bound(q, Tet(p0, p1, p2, p3), squared_distance(q, T(p0, p3, p2)));
+      check_compare_squared_distance_with_bound(q, Tet(p0, p1, p2, p3), squared_distance(q, T(p0, p3, p1)));
+    }
+  }
 
   void S_S()
   {
@@ -745,8 +771,8 @@ int main()
 
   std::cout << "3D Distance tests" << std::endl;
 
-  CGAL::Random r(1740056029);
+  // CGAL::Random r(1740056029);
-  // CGAL::Random r;
+  CGAL::Random r;
   std::cout << "random seed = " << r.get_seed() << std::endl;
 
   // @todo Some tests are too difficult for these kernels

Distance_3/test/Distance_3/test_distance_3.cpp

@@ -241,10 +241,39 @@ private:
   void P_Tet()
   {
     std::cout << "Point - Tetrahedron\n";
+    //Degenerate Tetrahedron
+    check_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_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_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_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_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_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_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_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_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_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_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<N; ++i)
+    {
+      P p0 = random_point();
+      P p1 = random_point();
+      P p2 = random_point();
+      P p3 = random_point();
+      P q = random_point();
+
+      // Failed on seed (1740146243) with an inexact kernel without the bound increasing
+      // The failed is due of the minimum realized by an edge and the computation is slightly different
+      // between the distance compute between the two triangles
+      check_squared_distance_with_bound(q, Tet(p0, p1, p2, p3), squared_distance(q, T(p0, p1, p2))*(1 + 1e-10));
+      check_squared_distance_with_bound(q, Tet(p0, p1, p2, p3), squared_distance(q, T(p1, p3, p2))*(1 + 1e-10));
+      check_squared_distance_with_bound(q, Tet(p0, p1, p2, p3), squared_distance(q, T(p0, p3, p2))*(1 + 1e-10));
+      check_squared_distance_with_bound(q, Tet(p0, p1, p2, p3), squared_distance(q, T(p0, p3, p1))*(1 + 1e-10));
+    }
   }
 
   void S_S()