Use a faster and more accurate intersection algorithm

path_finding.cpp

@@ -77,37 +77,6 @@ class PathFindingSurface {
   box_type_fp all_vertices_box = {{INFINITY, INFINITY}, {-INFINITY, -INFINITY}};
 };
 
-inline bool is_intersecting(const point_type_fp& p0, const point_type_fp& p1,
-                            const point_type_fp& p2, const point_type_fp& p3) {
-  const coordinate_type_fp s10_x = p1.x() - p0.x();
-  const coordinate_type_fp s10_y = p1.y() - p0.y();
-  const coordinate_type_fp s32_x = p3.x() - p2.x();
-  const coordinate_type_fp s32_y = p3.y() - p2.y();
-
-  coordinate_type_fp denom = s10_x * s32_y - s32_x * s10_y;
-  if (denom == 0) {
-    return false; // Collinear
-  }
-  const bool denomPositive = denom > 0;
-
-  const coordinate_type_fp s02_x = p0.x() - p2.x();
-  const coordinate_type_fp s02_y = p0.y() - p2.y();
-  const coordinate_type_fp s_numer = s10_x * s02_y - s10_y * s02_x;
-  if ((s_numer < 0) == denomPositive) {
-    return false; // No collision
-  }
-
-  const coordinate_type_fp t_numer = s32_x * s02_y - s32_y * s02_x;
-  if ((t_numer < 0) == denomPositive) {
-    return false; // No collision
-  }
-
-  if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) {
-    return false; // No collision
-  }
-  return true;
-}
-
 class PathSurface {
  public:
   PathSurface(const std::shared_ptr<const PathFindingSurface>& base, const point_type_fp begin, const point_type_fp end,

path_finding.hpp

@@ -4,9 +4,77 @@
 #include <boost/optional.hpp>
 
 #include "geometry.hpp"
+#include "bg_operators.hpp"
 
 namespace path_finding {
 
+// is_left(): tests if a point is Left|On|Right of an infinite line.
+//    Input:  three points p0, p1, and p2
+//    Return: >0 for p2 left of the line through p0 and p1
+//            =0 for p2 on the line
+//            <0 for p2 right of the line
+//    See: Algorithm 1 "Area of Triangles and Polygons"
+//    This is p0p1 cross p0p2.
+extern inline coordinate_type_fp is_left(point_type_fp p0, point_type_fp p1, point_type_fp p2) {
+  return ((p1.x() - p0.x()) * (p2.y() - p0.y()) -
+          (p2.x() - p0.x()) * (p1.y() - p0.y()));
+}
+
+// Is x between a and b, where a can be lesser or greater than b.  If
+// x == a or x == b, also returns true. */
+extern inline coordinate_type_fp is_between(coordinate_type_fp a,
+                                            coordinate_type_fp x,
+                                            coordinate_type_fp b) {
+  return x == a || x == b || (a-x>0) == (x-b>0);
+}
+
+// https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
+extern inline bool is_intersecting(const point_type_fp& p0, const point_type_fp& p1,
+                                   const point_type_fp& p2, const point_type_fp& p3) {
+  const coordinate_type_fp left012 = is_left(p0, p1, p2);
+  const coordinate_type_fp left013 = is_left(p0, p1, p3);
+  const coordinate_type_fp left230 = is_left(p2, p3, p0);
+  const coordinate_type_fp left231 = is_left(p2, p3, p1);
+
+  if (p0 != p1) {
+    if (left012 == 0) {
+      if (is_between(p0.x(), p2.x(), p1.x()) &&
+          is_between(p0.y(), p2.y(), p1.y())) {
+        return true; // p2 is on the line p0 to p1
+      }
+    }
+    if (left013 == 0) {
+      if (is_between(p0.x(), p3.x(), p1.x()) &&
+          is_between(p0.y(), p3.y(), p1.y())) {
+        return true; // p3 is on the line p0 to p1
+      }
+    }
+  }
+  if (p2 != p3) {
+    if (left230 == 0) {
+      if (is_between(p2.x(), p0.x(), p3.x()) &&
+          is_between(p2.y(), p0.y(), p3.y())) {
+        return true; // p0 is on the line p2 to p3
+      }
+    }
+    if (left231 == 0) {
+      if (is_between(p2.x(), p1.x(), p3.x()) &&
+          is_between(p2.y(), p1.y(), p3.y())) {
+        return true; // p1 is on the line p2 to p3
+      }
+    }
+  }
+  if ((left012 > 0) == (left013 > 0) ||
+      (left230 > 0) == (left231 > 0)) {
+    if (p1 == p2) {
+      return true;
+    }
+    return false;
+  } else {
+    return true;
+  }
+}
+
 class PathFindingSurface;
 
 struct GiveUp {};

path_finding_tests.cpp

@@ -17,6 +17,83 @@ using namespace std;
 using namespace path_finding;
 
 BOOST_AUTO_TEST_SUITE(path_finding_tests)
+BOOST_AUTO_TEST_SUITE(is_intersecting_tests)
+
+// From: https://stackoverflow.com/a/14409947/4454
+class Adler32 {
+ public:
+  void add(char x) {
+    s1 = (s1 + x) % 65521;
+    s2 = (s2 + s1) % 65521;
+  }
+  uint32_t get() {
+    return (s2 << 16) | s1;
+  }
+
+ private:
+  uint32_t s1 = 1;
+  uint32_t s2 = 0;
+};
+
+BOOST_AUTO_TEST_CASE(big) {
+  Adler32 hasher;
+  for (double x0 = -4; x0 < 4; x0++) {
+    for (double y0 = -4; y0 < 4; y0++) {
+      for (double x1 = -4; x1 < 4; x1++) {
+        for (double y1 = -4; y1 < 4; y1++) {
+          for (double x2 = -4; x2 < 4; x2++) {
+            for (double y2 = -4; y2 < 4; y2++) {
+              for (double x3 = -4; x3 < 4; x3++) {
+                for (double y3 = -4; y3 < 4; y3++) {
+                  point_type_fp p0{x0, y0};
+                  point_type_fp p1{x1, y1};
+                  point_type_fp p2{x2, y2};
+                  point_type_fp p3{x3, y3};
+                  linestring_type_fp ls0{p0,p1};
+                  linestring_type_fp ls1{p2,p3};
+                  hasher.add(is_intersecting(p0, p1, p2, p3));
+                }
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+  BOOST_CHECK_EQUAL(hasher.get(), 4000232678);
+}
+
+// This test is slow but it can be used once to confirm the results of the test above.
+BOOST_AUTO_TEST_CASE(small, *boost::unit_test::disabled()) {
+  for (double x0 = -3; x0 < 3; x0++) {
+    for (double y0 = -3; y0 < 3; y0++) {
+      for (double x1 = -3; x1 < 3; x1++) {
+        for (double y1 = -3; y1 < 3; y1++) {
+          for (double x2 = -3; x2 < 3; x2++) {
+            for (double y2 = -3; y2 < 3; y2++) {
+              for (double x3 = -3; x3 < 3; x3++) {
+                for (double y3 = -3; y3 < 3; y3++) {
+                  point_type_fp p0{x0, y0};
+                  point_type_fp p1{x1, y1};
+                  point_type_fp p2{x2, y2};
+                  point_type_fp p3{x3, y3};
+                  linestring_type_fp ls0{p0,p1};
+                  linestring_type_fp ls1{p2,p3};
+                  BOOST_TEST_INFO("intersection: " << bg::wkt(ls0) << " " << bg::wkt(ls1));
+                  BOOST_CHECK_EQUAL(
+                      path_finding::is_intersecting(p0, p1, p2, p3),
+                      bg::intersects(ls0, ls1));
+                }
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+}
+
+BOOST_AUTO_TEST_SUITE_END()
 
 BOOST_AUTO_TEST_CASE(simple) {
   box_type_fp bounding_box = bg::return_envelope<box_type_fp>(point_type_fp(-100, -100));