diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index 3d0c729b8..997320d70 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/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 diff --git a/src/libslic3r/ClosestPoint.cpp b/src/libslic3r/ClosestPoint.cpp new file mode 100644 index 000000000..e147683c3 --- /dev/null +++ b/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::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::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()); +} \ No newline at end of file diff --git a/src/libslic3r/ClosestPoint.hpp b/src/libslic3r/ClosestPoint.hpp new file mode 100644 index 000000000..996d6ed4d --- /dev/null +++ b/src/libslic3r/ClosestPoint.hpp @@ -0,0 +1,53 @@ +#ifndef slic3r_ClosestPoint_hpp_ +#define slic3r_ClosestPoint_hpp_ + +#include "Point.hpp" +#include "ExPolygon.hpp" + +namespace Slic3r { + +/// +/// Sort points and use sweep line algo to find closest point in points +/// +/// Seach for closest index to this point +/// Search inside of thoose points +/// Index of closest point from sorted_pts +//size_t find_closest(const Point &p, const Points& pts); + +/// +/// Use a plane sweep algorithm to find closest point in sorted points +/// https://www.cs.mcgill.ca/~cs251/ClosestPair/ClosestPairPS.html +/// +/// Seach for closest index to this point +/// Sorted points by X coordinate +/// Index of closest point from sorted_pts +size_t find_closest_in_sorted(const Point &p, const Points &sorted_pts); + +/// +/// Use a plane sweep algorithm to find closest point from pts in sorted_pts +/// https://www.cs.mcgill.ca/~cs251/ClosestPair/ClosestPairPS.html +/// +/// Seach for closest index to thoose points +/// Sorted points by X coordinate +/// Index of closest point from sorted_pts +//size_t find_closest_in_sorted(const Point &pts, const Points &sorted_pts); + +namespace closestPoint { +/// +/// Function used to sort points before searching for closest +/// +/// First point +/// Second Point +/// True when, p1.x < p2.x +bool sort_fnc(const Point &p1, const Point &p2); + +/// Function used to find upper bound in sorted points. +bool upper_fnc(coord_t value, const Point &p); +/// Function used to find lower bound in sorted points. +bool lower_fnc(const Point &p, coord_t value); +/// Calc manhatn size of point. Mainly to explain meaning +uint32_t manhattan_size(const Point &p); + +} // namespace closestPoint +} // namespace Slic3r +#endif // slic3r_ClosestPoint_hpp_ diff --git a/tests/libslic3r/CMakeLists.txt b/tests/libslic3r/CMakeLists.txt index 4712f35e7..e21709dd2 100644 --- a/tests/libslic3r/CMakeLists.txt +++ b/tests/libslic3r/CMakeLists.txt @@ -7,10 +7,11 @@ 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 - test_cut_surface.cpp + test_cut_surface.cpp test_elephant_foot_compensation.cpp test_geometry.cpp test_placeholder_parser.cpp diff --git a/tests/libslic3r/test_closest_point.cpp b/tests/libslic3r/test_closest_point.cpp new file mode 100644 index 000000000..2204f21ac --- /dev/null +++ b/tests/libslic3r/test_closest_point.cpp @@ -0,0 +1,42 @@ +#include + +#include + +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)); +}