fix(Distace): use dichotomic method to compute segment segment distan…

.clang-tidy

@@ -8,26 +8,27 @@ Checks: >
   -fuchsia*,
   -llvmlibc*,
   -llvm-header-guard,
+  -llvm-prefer-static-over-anonymous-namespace,
   -misc-no-recursion,
   -modernize-use-trailing-return-type,
   -readability-redundant-access-specifiers,
   -readability-convert-member-functions-to-static,
   -cppcoreguidelines-avoid-const-or-ref-data-members
 
 CheckOptions:
-  - key:    misc-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic
-    value:  '1'
+  - key: misc-non-private-member-variables-in-classes.IgnoreClassesWithAllMemberVariablesBeingPublic
+    value: "1"
   - key: readability-identifier-length.MinimumLoopCounterNameLength
     value: 1
   - key: readability-identifier-length.IgnoredVariableNames
-    value: '^[defijkptuvw]$'
+    value: "^[defijkptuvw]$"
   # More options here: https://clang.llvm.org/extra/clang-tidy/checks/readability/identifier-naming.html
   - key: readability-identifier-naming.NamespaceCase
     value: lower_case
   - key: readability-identifier-naming.ClassCase
-    value: CamelCase 
+    value: CamelCase
   - key: readability-identifier-naming.StructCase
-    value: CamelCase 
+    value: CamelCase
   - key: readability-identifier-naming.FunctionCase
     value: lower_case
   - key: readability-identifier-naming.VariableCase
@@ -51,14 +52,12 @@ CheckOptions:
   - key: readability-identifier-naming.GlobalFunctionCase
     value: lower_case
   - key: readability-identifier-naming.MemberConstantCase
-    value: CamelCase 
+    value: CamelCase
   - key: readability-identifier-naming.StaticConstantCase
-    value: lower_case 
+    value: lower_case
   - key: readability-function-cognitive-complexity.Threshold
     value: 10
   - key: readability-function-size.ParameterThreshold
     value: 4
   - key: misc-include-cleaner.IgnoreHeaders
-    value:  utility;cstddef;geode/.*_export\.h;geode/.*/common\.h;geode/basic/types\.h;geode/basic/assert\.h;
-
-
+    value: utility;cstddef;geode/.*_export\.h;geode/.*/common\.h;geode/basic/types\.h;geode/basic/assert\.h;

bindings/python/tests/geometry/test-py-bounding-box.py

@@ -22,13 +22,14 @@
 import os
 import sys
 import platform
+
 if sys.version_info >= (3, 8, 0) and platform.system() == "Windows":
-    for path in [x.strip() for x in os.environ['PATH'].split(';') if x]:
+    for path in [x.strip() for x in os.environ["PATH"].split(";") if x]:
         os.add_dll_directory(path)
 
 import opengeode_py_geometry as geom
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     box = geom.BoundingBox2D()
     box.add_point(geom.Point2D([-1, -1]))
     box.add_point(geom.Point2D([1, 1]))
@@ -52,29 +53,33 @@
     bbox3 = geom.BoundingBox3D()
     bbox3.add_point(geom.Point3D([-1, -1, -1]))
     bbox3.add_point(geom.Point3D([1, 1, 1]))
-
+    # Points
+    point0 = geom.Point3D([0, 0, 0])
+    point1 = geom.Point3D([0, 0, 2])
+    point2 = geom.Point3D([0, 0, -2])
     # Rays
-    ray_inside = geom.Ray3D( geom.Vector3D([0, 0, 1]),geom.Point3D([0, 0, 0]))
+
+    ray_inside = geom.Ray3D(geom.Vector3D([0, 0, 1]), point0)
     if not bbox3.intersects_ray(ray_inside):
         raise ValueError("[Test] Wrong result with ray_inside")
 
-    ray_up = geom.Ray3D( geom.Vector3D([0, 0, 1]),geom.Point3D([0, 0, 2]))
+    ray_up = geom.Ray3D(geom.Vector3D([0, 0, 1]), point1)
     if bbox3.intersects_ray(ray_up):
         raise ValueError("[Test] Wrong result with ray_up")
 
-    ray_down = geom.Ray3D( geom.Vector3D([0, 0, 1]),geom.Point3D([0, 0, -2]))
+    ray_down = geom.Ray3D(geom.Vector3D([0, 0, 1]), point2)
     if not bbox3.intersects_ray(ray_down):
         raise ValueError("[Test] Wrong result with ray_down")
 
     # Infinite lines
-    line_inside = geom.InfiniteLine3D( geom.Vector3D([0, 0, 1]),geom.Point3D([0, 0, 0]))
+    line_inside = geom.InfiniteLine3D(geom.Vector3D([0, 0, 1]), point0)
     if not bbox3.intersects_infinite_line(line_inside):
         raise ValueError("[Test] Wrong result with line_inside")
 
-    line_up = geom.InfiniteLine3D( geom.Vector3D([0, 0, 1]),geom.Point3D([0, 0, 2]))
+    line_up = geom.InfiniteLine3D(geom.Vector3D([0, 0, 1]), point1)
     if not bbox3.intersects_infinite_line(line_up):
         raise ValueError("[Test] Wrong result with line_up")
 
-    line_down = geom.InfiniteLine3D( geom.Vector3D([0, 0, 1]),geom.Point3D([0, 0, -2]))
+    line_down = geom.InfiniteLine3D(geom.Vector3D([0, 0, 1]), point2)
     if not bbox3.intersects_infinite_line(line_down):
-        raise ValueError("[Test] Wrong result with line_down")
+        raise ValueError("[Test] Wrong result with line_down")

src/geode/geometry/distance.cpp

@@ -476,81 +476,37 @@ namespace
         }
         return std::make_tuple( min_distance, point0, point1 );
     }
-} // namespace
-
-namespace geode
-{
-    template < index_t dimension >
-    double point_point_distance(
-        const Point< dimension >& point0, const Point< dimension >& point1 )
-    {
-        double diff2{ 0 };
-        for( const auto d : LRange{ dimension } )
-        {
-            const auto diff = point0.value( d ) - point1.value( d );
-            diff2 += diff * diff;
-        }
-        return std::sqrt( diff2 );
-    }
 
-    template < index_t dimension >
-    double point_segment_distance(
-        const Point< dimension >& point, const Segment< dimension >& segment )
-    {
-        const auto length = segment.length();
-        const auto length0 =
-            point_point_distance( segment.vertices()[0].get(), point );
-        if( length <= GLOBAL_EPSILON )
-        {
-            return length0;
-        }
-        const auto length1 =
-            point_point_distance( segment.vertices()[1].get(), point );
-        const auto sqr_length = length * length;
-        const auto sqr_length0 = length0 * length0;
-        const auto sqr_length1 = length1 * length1;
-        if( length0 >= length && length0 >= length1
-            && sqr_length + sqr_length1 <= sqr_length0 )
-        { // obtuse by vertex 1
-            return length1;
-        }
-        if( length1 >= length && length1 >= length0
-            && sqr_length + sqr_length0 <= sqr_length1 )
-        { // obtuse by vertex 0
-            return length0;
-        }
-        // acute angles
-        if( const auto distance =
-                compute_point_line_distance( length, length0, length1 ) )
-        {
-            return distance.value();
-        }
-        return std::get< 0 >(
-            point_segment_distance_using_projection( point, segment ) );
-    }
-
-    template < index_t dimension >
-    std::tuple< double, Point< dimension >, Point< dimension > >
-        segment_segment_distance( const Segment< dimension >& segment0,
-            const Segment< dimension >& segment1 )
+    template < geode::index_t dimension >
+    std::optional< std::tuple< double,
+        geode::Point< dimension >,
+        geode::Point< dimension > > >
+        approximate_segment_segment_distance(
+            const geode::Segment< dimension >& segment0,
+            const geode::Segment< dimension >& segment1 )
     {
         /* Algorithm and code found on
          * https://github.com/davideberly/GeometricTools/blob/master/GTE/Mathematics/DistSegmentSegment.h
          */
         const auto P1mP0 = segment0.direction();
         const auto Q1mQ0 = segment1.direction();
-        const Vector< dimension > P0mQ0{ segment1.vertices()[0],
+        const geode::Vector< dimension > P0mQ0{ segment1.vertices()[0],
             segment0.vertices()[0] };
         const auto a = P1mP0.dot( P1mP0 );
         const auto b = P1mP0.dot( Q1mQ0 );
         const auto c = Q1mQ0.dot( Q1mQ0 );
         const auto d = P1mP0.dot( P0mQ0 );
         const auto e = Q1mQ0.dot( P0mQ0 );
-        const auto det = a * c - b * b;
+        const auto ac = a * c;
+        const auto bb = b * b;
         double s, t, nd, bmd, bte, ctd, bpe, ate, btd;
-
-        if( det > 0 )
+        if( ac > bb )
         {
+            if( std::log2( std::abs( ac ) / std::abs( ac - bb ) ) > 20 )
+            {
+                return std::nullopt;
+            }
+            const auto det = ac - bb;
             bte = b * e;
             ctd = c * d;
             if( bte <= ctd ) // s <= 0
@@ -594,6 +550,10 @@ namespace geode
             }
             else // s > 0
             {
+                if( std::log2( std::abs( bte ) / std::abs( bte - ctd ) ) > 20 )
+                {
+                    return std::nullopt;
+                }
                 s = bte - ctd;
                 if( s >= det ) // s >= 1
                 {
@@ -660,6 +620,11 @@ namespace geode
                     }
                     else // t > 0
                     {
+                        if( std::log2( std::abs( ate ) / std::abs( ate - btd ) )
+                            > 20 )
+                        {
+                            return std::nullopt;
+                        }
                         t = ate - btd;
                         if( t >= det ) // t >= 1
                         {
@@ -766,21 +731,21 @@ namespace geode
             segment1.vertices()[0].get() + Q1mQ0 * t;
         const auto distance =
             point_point_distance( closest_on_segment0, closest_on_segment1 );
-        if( distance < GLOBAL_EPSILON )
+        if( distance < geode::GLOBAL_EPSILON )
         {
             return std::make_tuple(
                 distance, closest_on_segment0, closest_on_segment1 );
         }
         const auto distance_to_closest0 =
             point_segment_distance( closest_on_segment0, segment1 );
-        if( distance_to_closest0 < GLOBAL_EPSILON )
+        if( distance_to_closest0 < geode::GLOBAL_EPSILON )
         {
             return std::make_tuple( distance_to_closest0, closest_on_segment0,
                 point_segment_projection( closest_on_segment0, segment1 ) );
         }
         const auto distance_to_closest1 =
             point_segment_distance( closest_on_segment1, segment0 );
-        if( distance_to_closest1 < GLOBAL_EPSILON )
+        if( distance_to_closest1 < geode::GLOBAL_EPSILON )
         {
             return std::make_tuple( distance_to_closest1,
                 point_segment_projection( closest_on_segment1, segment0 ),
@@ -807,6 +772,121 @@ namespace geode
             distance, closest_on_segment0, closest_on_segment1 );
     }
 
+    template < geode::index_t dimension >
+    std::tuple< double, geode::Point< dimension >, geode::Point< dimension > >
+        dichotomic_segment_segment_distance(
+            const geode::Segment< dimension >& segment0,
+            const geode::Segment< dimension >& segment1 )
+    {
+        const auto longest_segment =
+            ( segment0.length() > segment1.length() ) ? segment0 : segment1;
+        const auto shortest_segment =
+            ( segment0.length() < segment1.length() ) ? segment0 : segment1;
+        auto current_point =
+            ( geode::point_segment_distance(
+                  longest_segment.vertices()[0].get(), shortest_segment )
+                < geode::point_segment_distance(
+                    longest_segment.vertices()[1].get(), shortest_segment ) )
+                ? longest_segment.vertices()[0].get()
+                : longest_segment.vertices()[1].get();
+        auto step = longest_segment.length() / 2;
+        auto current_distance =
+            geode::point_segment_distance( current_point, shortest_segment );
+        const auto segment_direction = longest_segment.normalized_direction();
+        while( step > geode::GLOBAL_EPSILON )
+        {
+            const auto point_at_step_plus =
+                current_point + segment_direction * step;
+            const auto point_at_step_minus =
+                current_point - segment_direction * step;
+            const auto distance_plus = geode::point_segment_distance(
+                point_at_step_plus, shortest_segment );
+            const auto distance_minus = geode::point_segment_distance(
+                point_at_step_minus, shortest_segment );
+            if( distance_plus < current_distance
+                && current_point != longest_segment.vertices()[1].get() )
+            {
+                current_distance = distance_plus;
+                current_point = point_at_step_plus;
+            }
+            if( distance_minus < current_distance
+                && current_point != longest_segment.vertices()[0].get() )
+            {
+                current_distance = distance_minus;
+                current_point = point_at_step_minus;
+            }
+            step /= 2;
+        }
+        return std::make_tuple(
+            current_distance, current_point, current_point );
+    }
+
+} // namespace
+
+namespace geode
+{
+    template < index_t dimension >
+    double point_point_distance(
+        const Point< dimension >& point0, const Point< dimension >& point1 )
+    {
+        double diff2{ 0 };
+        for( const auto d : LRange{ dimension } )
+        {
+            const auto diff = point0.value( d ) - point1.value( d );
+            diff2 += diff * diff;
+        }
+        return std::sqrt( diff2 );
+    }
+
+    template < index_t dimension >
+    double point_segment_distance(
+        const Point< dimension >& point, const Segment< dimension >& segment )
+    {
+        const auto length = segment.length();
+        const auto length0 =
+            point_point_distance( segment.vertices()[0].get(), point );
+        if( length <= GLOBAL_EPSILON )
+        {
+            return length0;
+        }
+        const auto length1 =
+            point_point_distance( segment.vertices()[1].get(), point );
+        const auto sqr_length = length * length;
+        const auto sqr_length0 = length0 * length0;
+        const auto sqr_length1 = length1 * length1;
+        if( length0 >= length && length0 >= length1
+            && sqr_length + sqr_length1 <= sqr_length0 )
+        { // obtuse by vertex 1
+            return length1;
+        }
+        if( length1 >= length && length1 >= length0
+            && sqr_length + sqr_length0 <= sqr_length1 )
+        { // obtuse by vertex 0
+            return length0;
+        }
+        // acute angles
+        if( const auto distance =
+                compute_point_line_distance( length, length0, length1 ) )
+        {
+            return distance.value();
+        }
+        return std::get< 0 >(
+            point_segment_distance_using_projection( point, segment ) );
+    }
+
+    template < index_t dimension >
+    std::tuple< double, Point< dimension >, Point< dimension > >
+        segment_segment_distance( const Segment< dimension >& segment0,
+            const Segment< dimension >& segment1 )
+    {
+        if( const auto approximation =
+                approximate_segment_segment_distance( segment0, segment1 ) )
+        {
+            return approximation.value();
+        }
+        return dichotomic_segment_segment_distance( segment0, segment1 );
+    }
+
     template < index_t dimension >
     std::tuple< double, Point< dimension >, Point< dimension > >
         segment_line_distance( const Segment< dimension >& segment,