Find closest point from points

src/libslic3r/CMakeLists.txt

@@ -30,6 +30,8 @@ set(SLIC3R_SOURCES
     clipper.hpp
     ClipperUtils.cpp
     ClipperUtils.hpp
+    ClosestPoint.cpp
+    ClosestPoint.hpp
     Color.cpp
     Color.hpp
     Config.cpp

src/libslic3r/ClosestPoint.cpp

@@ -0,0 +1,95 @@
+#include "ClosestPoint.hpp"
+
+size_t Slic3r::find_closest_in_sorted(const Point &p, const Points &pts)
+{
+    using namespace closestPoint;
+    // check that input is really sorted
+    assert(std::is_sorted(pts.begin(), pts.end(), sort_fnc));
+
+    // check input
+    if (pts.empty()) return std::numeric_limits<size_t>::max();
+    if (pts.size() == 1) return 0;
+
+    // closest point node in X
+    Points::const_iterator it_x = std::upper_bound(pts.begin(), pts.end(), p.x(), upper_fnc);
+    bool is_it_x_end = it_x == pts.end();
+    // it_x can't pointing to end so change to last point
+    if (is_it_x_end) --it_x;
+    // manhatn distance to closest point
+    uint32_t manhattan_dist = manhattan_size(*it_x - p);
+
+    // node for lower bound
+    Points::const_iterator it_l;
+    if (it_x == pts.begin()) { 
+        it_l = it_x;
+    } else {
+        it_l = std::lower_bound(pts.begin(), it_x, p.x() - manhattan_dist, lower_fnc);
+        for (auto it = it_x - 1; it > it_l; --it) {
+            uint32_t diff_y = std::abs(it->y() - p.y());
+            if (diff_y > manhattan_dist) continue;
+            uint32_t diff_x   = std::abs(it->x() - p.x());
+            uint32_t act_dist = diff_y + diff_x;
+            if (manhattan_dist > act_dist) {
+                manhattan_dist = act_dist;
+                it_l = std::lower_bound(it_l, it_x, p.x() - manhattan_dist, lower_fnc);
+            }
+        }
+    }
+
+    // node for upper bound
+    Points::const_iterator it_u;
+    if (is_it_x_end) { 
+        it_u = pts.end();
+    } else {
+        it_u = std::upper_bound(it_x, pts.end(), p.x() + manhattan_dist, upper_fnc);
+        for (auto it = it_x + 1; it < it_u; ++it) { 
+            uint32_t diff_y = std::abs(it->y() - p.y());
+            if (diff_y > manhattan_dist) continue;
+            uint32_t diff_x = std::abs(it->x() - p.x());
+            uint32_t act_dist = diff_y + diff_x;
+            if (manhattan_dist > act_dist) {
+                // IMPROVE: calc euclid distance when e.g. (diff_Biggery < 2*diff_smaller)
+                manhattan_dist = act_dist;
+                it_u = std::upper_bound(it_x, it_u, p.x() + manhattan_dist, upper_fnc);
+            }
+        }
+    }
+
+    // find closest by squer distance
+    float dist_sq = std::numeric_limits<float>::max();
+    size_t result = it_x - pts.begin();
+    for (Points::const_iterator it = it_l; it < it_u; ++it) {
+        uint32_t diff_y = std::abs(it->y() - p.y());
+        if (diff_y > manhattan_dist) continue;
+        float diff_x = it->x() - p.x();
+        // calculate square distance
+        float d = (float) diff_y * diff_y + diff_x * diff_x;
+        if (dist_sq > d) {
+            dist_sq = d;
+            result  = it - pts.begin();
+        }
+    }
+    return result;
+}
+
+using namespace Slic3r;
+
+bool closestPoint::sort_fnc(const Point &p1, const Point &p2)
+{
+    return p1.x() < p2.x();
+}
+
+bool closestPoint::upper_fnc(coord_t value, const Point &p)
+{
+    return value < p.x();
+}
+
+bool closestPoint::lower_fnc(const Point &p, coord_t value)
+{
+    return value > p.x();
+}
+
+uint32_t closestPoint::manhattan_size(const Point &p)
+{
+    return std::abs(p.x()) + abs(p.y());
+}

src/libslic3r/ClosestPoint.hpp

@@ -0,0 +1,53 @@
+#ifndef slic3r_ClosestPoint_hpp_
+#define slic3r_ClosestPoint_hpp_
+
+#include "Point.hpp"
+#include "ExPolygon.hpp"
+
+namespace Slic3r {
+
+/// <summary>
+/// Sort points and use sweep line algo to find closest point in points
+/// </summary>
+/// <param name="p">Seach for closest index to this point</param>
+/// <param name="pts">Search inside of thoose points</param>
+/// <returns>Index of closest point from sorted_pts</returns>
+//size_t find_closest(const Point &p, const Points& pts);
+
+/// <summary>
+/// Use a plane sweep algorithm to find closest point in sorted points
+/// https://www.cs.mcgill.ca/~cs251/ClosestPair/ClosestPairPS.html
+/// </summary>
+/// <param name="p">Seach for closest index to this point</param>
+/// <param name="sorted_pts">Sorted points by X coordinate</param>
+/// <returns>Index of closest point from sorted_pts</returns>
+size_t find_closest_in_sorted(const Point &p, const Points &sorted_pts);
+
+/// <summary>
+/// Use a plane sweep algorithm to find closest point from pts in sorted_pts
+/// https://www.cs.mcgill.ca/~cs251/ClosestPair/ClosestPairPS.html
+/// </summary>
+/// <param name="pts">Seach for closest index to thoose points</param>
+/// <param name="sorted_pts">Sorted points by X coordinate</param>
+/// <returns>Index of closest point from sorted_pts</returns>
+//size_t find_closest_in_sorted(const Point &pts, const Points &sorted_pts);
+
+namespace closestPoint {
+/// <summary>
+/// Function used to sort points before searching for closest
+/// </summary>
+/// <param name="p1">First point</param>
+/// <param name="p2">Second Point</param>
+/// <returns>True when, p1.x < p2.x </returns>
+bool sort_fnc(const Point &p1, const Point &p2);
+
+/// <summary> Function used to find upper bound in sorted points. </summary>
+bool upper_fnc(coord_t value, const Point &p);
+/// <summary> Function used to find lower bound in sorted points. </summary>
+bool lower_fnc(const Point &p, coord_t value);
+/// <summary> Calc manhatn size of point. Mainly to explain meaning</summary>
+uint32_t manhattan_size(const Point &p);
+
+} // namespace closestPoint
+} // namespace Slic3r
+#endif // slic3r_ClosestPoint_hpp_

tests/libslic3r/CMakeLists.txt

@@ -7,6 +7,7 @@ add_executable(${_TEST_NAME}_tests
 	test_kdtreeindirect.cpp
 	test_clipper_offset.cpp
 	test_clipper_utils.cpp
+	test_closest_point.cpp
 	test_color.cpp
 	test_config.cpp
 	test_curve_fitting.cpp

tests/libslic3r/test_closest_point.cpp

@@ -0,0 +1,42 @@
+#include <catch2/catch.hpp>
+
+#include <libslic3r/ClosestPoint.hpp>
+
+using namespace Slic3r;
+TEST_CASE("Find the closest point from 2", "[ClosestPoint]")
+{
+    Points pts = {{0, 1}, {0, 2}};
+    CHECK(std::is_sorted(pts.begin(), pts.end(), closestPoint::sort_fnc));
+    CHECK(0 == find_closest_in_sorted(Point{0, 0}, pts));
+    CHECK(0 == find_closest_in_sorted(Point{1, 1}, pts));
+    CHECK(1 == find_closest_in_sorted(Point{1, 2}, pts));
+}
+
+TEST_CASE("Find the closest point from 9", "[ClosestPoint]")
+{
+    //  0 - 3 - 6
+    //  |   |   |
+    //  1 - 4 - 7
+    //  |   |   |
+    //  2 - 5 - 8
+    Points pts = {{-3, 3}, {-3, 0}, {-3, -3}, {0, 3}, {0, 0},
+                  {0, -3}, {3, 3}, {3, 0}, {3, -3}};
+    CHECK(std::is_sorted(pts.begin(), pts.end(), closestPoint::sort_fnc));
+
+    CHECK(0 == find_closest_in_sorted(Point{-4, 4}, pts));
+    CHECK(0 == find_closest_in_sorted(Point{-2, 2}, pts));
+    // check center
+    CHECK(4 == find_closest_in_sorted(Point{-1, 1}, pts));
+    CHECK(4 == find_closest_in_sorted(Point{ 0, 1}, pts));
+    CHECK(4 == find_closest_in_sorted(Point{ 1, 1}, pts));
+    CHECK(4 == find_closest_in_sorted(Point{-1, 0}, pts));
+    CHECK(4 == find_closest_in_sorted(Point{ 0, 0}, pts));
+    CHECK(4 == find_closest_in_sorted(Point{ 1, 0}, pts));
+    CHECK(4 == find_closest_in_sorted(Point{-1,-1}, pts));
+    CHECK(4 == find_closest_in_sorted(Point{ 0,-1}, pts));
+    CHECK(4 == find_closest_in_sorted(Point{ 1,-1}, pts));
+    CHECK(4 == find_closest_in_sorted(Point{ 0, 0}, pts));
+
+    CHECK(7 == find_closest_in_sorted(Point{2, 1}, pts));
+    CHECK(8 == find_closest_in_sorted(Point{2,-2}, pts));
+}