diff --git a/path_finding.cpp b/path_finding.cpp index 339a43e13..b9363f461 100644 --- a/path_finding.cpp +++ b/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& base, const point_type_fp begin, const point_type_fp end, diff --git a/path_finding.hpp b/path_finding.hpp index 4169b77fa..4de0204a3 100644 --- a/path_finding.hpp +++ b/path_finding.hpp @@ -4,9 +4,77 @@ #include #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 {}; diff --git a/path_finding_tests.cpp b/path_finding_tests.cpp index 61d0ac57f..ddca47071 100644 --- a/path_finding_tests.cpp +++ b/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(point_type_fp(-100, -100));