From a0eecb91c869232136b46a8ab456ef6c3825811c Mon Sep 17 00:00:00 2001 From: Filip Sykala - NTB T15p Date: Fri, 22 Jul 2022 15:18:34 +0200 Subject: [PATCH] Using aabb tree for lines to detect closest expolygons --- src/libslic3r/AABBTreeLines.hpp | 31 +- src/libslic3r/CMakeLists.txt | 2 - src/libslic3r/ClosestPoint.cpp | 6 - src/libslic3r/ClosestPoint.hpp | 252 --------------- src/libslic3r/CutSurface.cpp | 414 +++++++++++++++---------- tests/libslic3r/CMakeLists.txt | 1 - tests/libslic3r/test_aabbindirect.cpp | 43 +++ tests/libslic3r/test_closest_point.cpp | 81 ----- 8 files changed, 308 insertions(+), 522 deletions(-) delete mode 100644 src/libslic3r/ClosestPoint.cpp delete mode 100644 src/libslic3r/ClosestPoint.hpp delete mode 100644 tests/libslic3r/test_closest_point.cpp diff --git a/src/libslic3r/AABBTreeLines.hpp b/src/libslic3r/AABBTreeLines.hpp index 7b9595419..6f75edd3d 100644 --- a/src/libslic3r/AABBTreeLines.hpp +++ b/src/libslic3r/AABBTreeLines.hpp @@ -87,26 +87,29 @@ inline AABBTreeIndirect::Tree<2, typename LineType::Scalar> build_aabb_tree_over } // Finding a closest line, its closest point and squared distance to the closest point -// Returns squared distance to the closest point or -1 if the input is empty. -template +// Returns squared distance to the closest point or -1 if the input is empty +// or no closer point than max_sq_dist +template inline typename VectorType::Scalar squared_distance_to_indexed_lines( - const std::vector &lines, - const TreeType &tree, - const VectorType &point, - size_t &hit_idx_out, - Eigen::PlainObjectBase &hit_point_out) - { - using Scalar = typename VectorType::Scalar; + const std::vector &lines, + const TreeType &tree, + const VectorType &point, + size_t &hit_idx_out, + Eigen::PlainObjectBase &hit_point_out, + Scalar max_sqr_dist = std::numeric_limits::infinity()) +{ + if (tree.empty()) return Scalar(-1); auto distancer = detail::IndexedLinesDistancer { lines, tree, point }; - return tree.empty() ? - Scalar(-1) : - AABBTreeIndirect::detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0), - std::numeric_limits::infinity(), hit_idx_out, hit_point_out); + hit_idx_out = std::numeric_limits::max(); + Scalar distance = AABBTreeIndirect::detail::squared_distance_to_indexed_primitives_recursive( + distancer, size_t(0), Scalar(0), max_sqr_dist, hit_idx_out, hit_point_out); + if (hit_idx_out == std::numeric_limits::max()) return Scalar(-1); + return distance; } } - } #endif /* SRC_LIBSLIC3R_AABBTREELINES_HPP_ */ diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt index b20d9c94c..319013455 100644 --- a/src/libslic3r/CMakeLists.txt +++ b/src/libslic3r/CMakeLists.txt @@ -34,8 +34,6 @@ 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 deleted file mode 100644 index 6850d96b3..000000000 --- a/src/libslic3r/ClosestPoint.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "ClosestPoint.hpp" -#include "Point.hpp" - -// Compile template for specific data type -template<> -size_t Slic3r::find_closest_in_sorted(const Slic3r::Point &p, const Slic3r::Points &pts); \ No newline at end of file diff --git a/src/libslic3r/ClosestPoint.hpp b/src/libslic3r/ClosestPoint.hpp deleted file mode 100644 index 43512b0ce..000000000 --- a/src/libslic3r/ClosestPoint.hpp +++ /dev/null @@ -1,252 +0,0 @@ -#ifndef slic3r_ClosestPoint_hpp_ -#define slic3r_ClosestPoint_hpp_ - -#include -#include - -namespace Slic3r { - -/// -/// Sort points and use find_closest_in_sorted -/// -/// Seach for closest index to this point -/// Search inside of thoose points -/// Index of closest point from sorted_pts -template -// /*SFINAE*/ typename std::enable_if>::type * = nullptr > -size_t find_closest(const P &p, const std::vector

&pts){ - // check input - if (pts.empty()) return std::numeric_limits::max(); - if (pts.size() == 1) return 0; - - using V = decltype(p.x()); - // extend P with order - struct PP - { - size_t ord; - const P* p; - V x() const { return p->x(); } - V y() const { return p->y(); } - }; - std::vector pts_ord; - pts_ord.reserve(pts.size()); - size_t ord = 0; - for (const P &pp : pts) pts_ord.push_back({ord++, &pp}); - std::stable_sort(pts_ord.begin(), pts_ord.end(), closestPoint::sort_fnc); - PP pp{0, &p}; - size_t closest_index = find_closest_in_sorted(pp, pts_ord); - return pts_ord[closest_index].ord; -} - -///

-/// 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 -template -size_t find_closest_in_sorted(const P &p, const std::vector

&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 from thoose points -/// Seach for closest index to thoose points -/// Sorted points by X coordinate(by function closestPoint::sort_fnc) -/// Indices to pts(first) and sorted_pts(second) -template -std::pair find_closest_in_sorted( - const std::vector

&pts, const std::vector

&sorted_pts); - -namespace closestPoint { -///

-/// Function used to sort points before searching for closest -/// -/// First point -/// Second Point -/// True when, p1.x < p2.x -template bool sort_fnc(const P &p1, const P &p2){ return p1.x() < p2.x(); } - -/// Function used to find upper bound in sorted points. -template bool upper_fnc(V value, const P &p){ return value < p.x(); } -/// Function used to find lower bound in sorted points. -template bool lower_fnc(const P &p, V value){ return value > p.x(); } -/// Calc manhatn size of point. Mainly to explain meaning -template uint32_t manhattan_size(const P &p1, const P &p2) -{ return std::abs(p1.x()-p2.x()) + abs(p1.y()-p2.y()); } - -template -typename std::vector

::const_iterator get_lower_bound(uint32_t& dist, const P &p, - typename std::vector

::const_iterator it_x, const std::vector

& pts) -{ - if(it_x == pts.begin()) return pts.begin(); - assert(it_x > pts.begin() && it_x < pts.end()); - - using V = decltype(p.x()); - using It = std::vector

::const_iterator; - It res = std::lower_bound(pts.begin(), it_x, p.x() - dist, lower_fnc); - for (auto it = it_x - 1; it > res; --it) { - uint32_t diff_y = std::abs(it->y() - p.y()); - if (diff_y > dist) continue; - uint32_t diff_x = std::abs(it->x() - p.x()); - uint32_t act_dist = diff_y + diff_x; - if (dist > act_dist) { - dist = act_dist; - res = std::lower_bound(res, it_x, p.x() - dist, lower_fnc); - } - } - return res; -} - -template -typename std::vector

::const_iterator get_uppper_bound(uint32_t& dist, const P &p, - typename std::vector

::const_iterator it_x, const std::vector

& pts) -{ - assert(it_x >= pts.begin() && it_x < pts.end()); - - using V = decltype(p.x()); - using It = std::vector

::const_iterator; - It res = std::upper_bound(it_x, pts.end(), p.x() + dist, upper_fnc); - for (auto it = it_x + 1; it < res; ++it) { - uint32_t diff_y = std::abs(it->y() - p.y()); - if (diff_y > dist) continue; - uint32_t diff_x = std::abs(it->x() - p.x()); - uint32_t act_dist = diff_y + diff_x; - if (dist > act_dist) { - // IMPROVE: calc euclid distance when e.g. (diff_Biggery < 2*diff_smaller) - dist = act_dist; - res = std::upper_bound(it_x, res, p.x() + dist, upper_fnc); - } - } - return res; -} - -} // namespace closestPoint -} // namespace Slic3r - -template -size_t Slic3r::find_closest_in_sorted(const P &p, const std::vector

&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; - - using V = decltype(p.x()); - using It = std::vector

::const_iterator; - // closest point node in X - It 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); - It it_l = get_lower_bound(manhattan_dist,p, it_x, pts); - - // node for upper bound - It it_u = (is_it_x_end) ? pts.end() : - get_uppper_bound(manhattan_dist, p, it_x, pts); - - // find closest by squer distance - float dist_sq = std::numeric_limits::max(); - size_t result = it_x - pts.begin(); - for (It 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; -} - -template -std::pair Slic3r::find_closest_in_sorted( - const std::vector

&pts, const std::vector

&sorted_pts) -{ - using namespace closestPoint; - // check that input is really sorted - assert(std::is_sorted(sorted_pts.begin(), sorted_pts.end(), sort_fnc

)); - - std::pair res = {std::numeric_limits::max(), - std::numeric_limits::max()}; - // check inputs - if (pts.empty() || sorted_pts.empty()) return res; - // Speed up for one element in vector - if (pts.size() == 1) - return {0, find_closest_in_sorted(pts.front(), sorted_pts)}; - if (sorted_pts.size() == 1) - return {find_closest(sorted_pts.front(), pts), 0}; - - using V = decltype(p.x()); - using It = std::vector

::const_iterator; - - uint32_t res_dist = std::numeric_limits::max(); - float res_dist_sq = std::numeric_limits::max(); - auto set_res_dist_sq = [&res_dist_sq, &res_dist](float dist_sq) { - res_dist_sq = dist_sq; - float dist = sqrt(dist_sq); - // crop dist to unsigned int - uint32_t dist_ui = static_cast(std::ceil(dist)); - if (res_dist < dist_ui) res_dist = dist_ui; - }; - // find limit for manhatn distance - bool is_first = true; - for (const P &p : pts) { - if (is_first) { - is_first = false; - size_t index = find_closest_in_sorted(p, sorted_pts); - res = {0, index}; - const P &p2 = sorted_pts[index]; - V d_x = p.x()-p2.x(); - V d_y = p.y()-p2.y(); - set_res_dist_sq(d_x * d_x + d_y * d_y); - continue; - } - // closest point node in X - It 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 p_dist = manhattan_size(*it_x, p); - - It it_l = get_lower_bound(res_dist, p, it_x, pts); - - // node for upper bound - It it_u = pts.end(); - if (!is_it_x_end) it_u = get_uppper_bound(res_dist, p, it_x, pts); - - // find closest by squer distance - float dist_sq = res_dist_sq; - for (It it = it_l; it < it_u; ++it) { - uint32_t diff_y = std::abs(it->y() - p.y()); - if (diff_y > res_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; - size_t i1 = &p - &pts.begin(); - size_t i2 = it - pts.begin(); - res = {i1, i2}; - } - } - - // found closer? - if (res_dist_sq > dist_sq) - set_res_dist_sq(dist_sq); - } - return res; -} - -#endif // slic3r_ClosestPoint_hpp_ diff --git a/src/libslic3r/CutSurface.cpp b/src/libslic3r/CutSurface.cpp index 9619c7791..953222591 100644 --- a/src/libslic3r/CutSurface.cpp +++ b/src/libslic3r/CutSurface.cpp @@ -411,11 +411,13 @@ VDistances calc_distances(const SurfacePatches &patches, /// /// All distances - Vector distances for each shape point /// Vector of letters +/// Center of projection /// Convert index to addresss inside of shape /// Best projection distances ProjectionDistances choose_best_distance( const VDistances &distances, const ExPolygons &shapes, + const Point ¢er, const ShapePoint2index &shape_point_2_index); ///

@@ -454,6 +456,7 @@ void store(const CutAOIs &aois, const CutMesh &mesh, const std::string &dir); void store(const SurfacePatches &patches, const std::string &dir); void store(const Vec3f &vertex, const Vec3f &normal, const std::string &file, float size = 2.f); void store(const ProjectionDistances &pds, const VCutAOIs &aois, const CutMeshes &meshes, const std::string &file, float width = 0.2f/* [in mm] */); +void store(const ExPolygons& shapes, const std::vector& mask_distances, const std::vector>& connections, const std::string& file_svg); void store(const SurfaceCut &cut, const std::string &file, const std::string &contour_dir); void store(const std::vector &models, const std::string &obj_filename); void store(const std::vector&models, const std::string &dir); @@ -541,7 +544,7 @@ SurfaceCut Slic3r::cut_surface(const ExPolygons &shapes, priv::VDistances distances = priv::calc_distances(patches, cgal_models, cgal_shape, shapes_points, projection_ratio); // for each point select best projection - priv::ProjectionDistances best_projection = priv::choose_best_distance(distances, shapes, s2i); + priv::ProjectionDistances best_projection = priv::choose_best_distance(distances, shapes, shapes_bb.center(), s2i); std::vector use_patch = priv::select_patches(best_projection, patches, model_cuts); SurfaceCut result = merge_patches(patches, use_patch); #ifdef DEBUG_OUTPUT_DIR @@ -1685,6 +1688,9 @@ priv::VDistances priv::calc_distances(const SurfacePatches &patches, return result; } + +#include "libslic3r/AABBTreeLines.hpp" +#include "libslic3r/Line.hpp" // functions for choose_best_distance namespace priv { @@ -1700,20 +1706,22 @@ struct ClosePoint float dist_sq = std::numeric_limits::max(); }; -// search in all shapes points to found closest point to given point -uint32_t get_closest_point_index(const Point &p, const ExPolygons &shapes, const VDistances &distances); +struct SearchData{ +// IMPROVE: float lines are enough +std::vector lines; +// convert line index into Shape point index +std::vector cvt; +// contain lines from prev point to Point index +AABBTreeIndirect::Tree<2, double> tree; +}; -using PNode = std::pair; -using PNodes = std::vector; -PNodes create_nodes(const ExPolygons &shapes, const VDistances &distances, size_t shapes_point_count); +SearchData create_search_data(const ExPolygons &shapes, const std::vector& mask); +uint32_t get_closest_point_index(const SearchData &sd, size_t line_idx, const Vec2d &hit_point, const ExPolygons &shapes, const ShapePoint2index &s2i); -/// -/// Search in all shapes points(nodes) to found closest point to given point -/// -/// Point to search closest -/// Points sorted by X -/// Index into shapes of point closest to given point -uint32_t get_closest_point_index2(const Point &p, const PNodes& nodes); +// use AABB Tree Lines to find closest point +uint32_t find_closest_point_index(const Point &p, const ExPolygons &shapes, const ShapePoint2index &s2i, const std::vector &mask); + +std::pair find_closest_point_pair(const ExPolygons &shapes, const std::vector &done_shapes, const ShapePoint2index &s2i, const std::vector &mask); // Search for closest projection to wanted distance const ProjectionDistance *get_closest_projection(const ProjectionDistances &distance, float wanted_distance); @@ -1729,136 +1737,179 @@ ClosePoint find_close_point(const Point &p, ProjectionDistances &result, std::ve } -float priv::calc_size_sq(const Point &p){ +float priv::calc_size_sq(const Point &p){ + // NOTE: p.squaredNorm() can't be use due to overflow max int value return (float) p.x() * p.x() + (float) p.y() * p.y(); } -uint32_t priv::get_closest_point_index(const Point &p, - const ExPolygons &shapes, - const VDistances &distances) +priv::SearchData priv::create_search_data(const ExPolygons &shapes, + const std::vector &mask) { - ClosePoint cp; - uint32_t id{0}; - auto get_closest = [&distances, &p, &id, &cp] - (const Points &pts) { - for (const Point &p_ : pts) { - if (distances[id].empty()) { - ++id; - continue; - } - float d = calc_size_sq(p - p_); - if (cp.dist_sq > d) { - cp.dist_sq = d; - cp.index = id; - } - ++id; - } - }; - for (const ExPolygon &shape : shapes) { - get_closest(shape.contour.points); - for (const Polygon &hole : shape.holes) - get_closest(hole.points); - } - return cp.index; -} - -priv::PNodes priv::create_nodes(const ExPolygons &shapes, const VDistances &distances, size_t shapes_point_count) { - PNodes pts; - pts.reserve(shapes_point_count); - uint32_t index = 0; - auto add_polygon = [&pts, &index, &distances](const Polygon &poly) { + // IMPROVE: Use float precission (it is enough) + SearchData sd; + sd.lines.reserve(mask.size()); + sd.cvt.reserve(mask.size()); + size_t index = 0; + auto add_lines = [&sd, &index, &mask] + (const Polygon &poly) { + Vec2d prev = poly.back().cast(); + bool use_point = mask[index + poly.points.size() - 1]; for (const Point &p : poly.points) { - // skip empty distances - if (distances[index].empty()) { - ++index; - continue; + if (!use_point) { + use_point = mask[index]; + if (use_point) prev = p.cast(); + } else if (!mask[index]) { + use_point = false; + } else { + Vec2d p_d = p.cast(); + sd.lines.emplace_back(prev, p_d); + sd.cvt.push_back(index); + prev = p_d; } - pts.emplace_back(p, index++); + ++index; } }; - for (const ExPolygon s : shapes) { - add_polygon(s.contour); - for (const Polygon &h : s.holes) add_polygon(h); + + for (const ExPolygon &shape : shapes) { + add_lines(shape.contour); + for (const Polygon &hole : shape.holes) add_lines(hole); } - - std::sort(pts.begin(), pts.end(), [](const PNode &n1, const PNode &n2) { - return n1.first.x() < n2.first.x(); - }); - return pts; + sd.tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(sd.lines); + return sd; } -uint32_t priv::get_closest_point_index2(const Point &p, const PNodes &nodes) +uint32_t priv::get_closest_point_index(const SearchData &sd, + size_t line_idx, + const Vec2d &hit_point, + const ExPolygons &shapes, + const ShapePoint2index &s2i) { - assert(!nodes.empty()); + const Linef &line = sd.lines[line_idx]; + Vec2d dir = line.a - line.b; + Vec2d dir_abs = dir.cwiseAbs(); + // use x coordinate + int i = (dir_abs.x() > dir_abs.y())? 0 :1; + + bool use_index = abs(line.a[i] - hit_point[i]) > + abs(line.b[i] - hit_point[i]); + size_t point_index = sd.cvt[line_idx]; + + // Lambda used only for check result + auto is_same = [&s2i, &shapes](const Vec2d &p, size_t i) -> bool { + auto id = s2i.calc_id(i); + const ExPolygon &shape = shapes[id.expolygons_index]; + const Polygon &poly = (id.polygon_index == 0) ? + shape.contour : + shape.holes[id.polygon_index - 1]; + Vec2i p_ = p.cast(); + return p_ == poly[id.point_index]; + }; + + if (use_index) { + assert(is_same(line.b, point_index)); + return point_index; + } + auto id = s2i.calc_id(point_index); + if (id.point_index != 0) { + assert(is_same(line.a, point_index - 1)); + return point_index - 1; + } + const ExPolygon &shape = shapes[id.expolygons_index]; + size_t count_polygon_points = (id.polygon_index == 0) ? + shape.contour.size() : + shape.holes[id.polygon_index - 1].size(); + size_t prev_point_index = point_index + (count_polygon_points - 1); + assert(is_same(line.a, prev_point_index)); + // return previous point index + return prev_point_index; +} + +// use AABB Tree Lines +uint32_t priv::find_closest_point_index(const Point &p, + const ExPolygons &shapes, + const ShapePoint2index &s2i, + const std::vector &mask) +{ + SearchData sd = create_search_data(shapes, mask); + size_t line_idx = std::numeric_limits::max(); + Vec2d hit_point; + Vec2d p_d = p.cast(); + double distance_sq = AABBTreeLines::squared_distance_to_indexed_lines( + sd.lines, sd.tree, p_d, line_idx, hit_point); + assert(distance_sq > 0); - // function to find upper point node - auto f_u = [](coord_t value, const PNode& n) { - return value < n.first.x(); - }; + // IMPROVE: one could use line ratio to find closest point + return get_closest_point_index(sd, line_idx, hit_point, shapes, s2i); +} - // function to find lower point node - auto f_l = [](const PNode &n, coord_t value) { - return value > n.first.x(); - }; +std::pair priv::find_closest_point_pair( + const ExPolygons &shapes, + const std::vector &done_shapes, + const ShapePoint2index &s2i, + const std::vector &mask) +{ + assert(mask.size() == s2i.get_count()); + assert(done_shapes.size() == shapes.size()); + std::vector unfinished_mask = mask; // copy - // closest point node in X - auto it_x = std::upper_bound(nodes.begin(), nodes.end(), p.x(), f_u); - // manhatn distance to closest point - auto manhattan_size = [](const Point &p) -> uint32_t { - return std::abs(p.x()) + abs(p.y()); - }; - uint32_t manhattan_dist = (it_x != nodes.end())? - manhattan_size(it_x->first - p) : - manhattan_size(nodes.back().first - p); - // node for lower bound - auto it_l = std::lower_bound(nodes.begin(), it_x, p.x() - manhattan_dist, f_l); - auto it = it_x; - while (it > it_l) { - uint32_t diff_y = std::abs(it->first.y() - p.y()); - if (diff_y > manhattan_dist) { - --it; + size_t index = 0; + for (size_t shape_index = 0; shape_index < shapes.size(); shape_index++) { + size_t count = count_points(shapes[shape_index]); + if (done_shapes[shape_index]) { + for (size_t i = 0; i < count; ++i, ++index) + unfinished_mask[index] = false; + } else { + index += count; + } + } + assert(index == s2i.get_count()); + SearchData sd = create_search_data(shapes, unfinished_mask); + + struct ClosestPair + { + size_t finish_idx = std::numeric_limits::max(); + size_t unfinished_line_idx = std::numeric_limits::max(); + Vec2d hit_point = Vec2d(); + double distance_sq = std::numeric_limits::max(); + } cp; + + index = 0; + for (size_t shape_index = 0; shape_index < shapes.size(); shape_index++) { + const ExPolygon shape = shapes[shape_index]; + if (!done_shapes[shape_index]) { + index += count_points(shape); continue; - } - uint32_t diff_x = std::abs(it->first.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, f_l); - } - --it; + } + + auto search_in_polygon = [&index, &cp, &sd, &mask](const Polygon& polygon) { + for (size_t i = 0; i < polygon.size(); ++i, ++index) { + if (mask[index] == false) continue; + Vec2d p_d = polygon[i].cast(); + size_t line_idx = std::numeric_limits::max(); + Vec2d hit_point; + double distance_sq = AABBTreeLines::squared_distance_to_indexed_lines( + sd.lines, sd.tree, p_d, line_idx, hit_point, cp.distance_sq); + if (distance_sq < 0) continue; + assert(distance_sq <= cp.distance_sq); + cp.distance_sq = distance_sq; + cp.unfinished_line_idx = line_idx; + cp.hit_point = hit_point; + cp.finish_idx = index; + } + }; + search_in_polygon(shape.contour); + for (const Polygon& hole: shape.holes) + search_in_polygon(hole); + } + assert(index == s2i.get_count()); + // check that exists result + if (cp.finish_idx == std::numeric_limits::max()) { + return std::make_pair(std::numeric_limits::max(), + std::numeric_limits::max()); } - // node for upper bound - auto it_u = std::upper_bound(it_x, nodes.end(), p.x() + manhattan_dist, f_u); - it = it_x; - while (it < it_u) { - ++it; - uint32_t diff_y = std::abs(it->first.y() - p.y()); - if (diff_y > manhattan_dist) continue; - uint32_t diff_x = std::abs(it->first.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, f_u); - } - } - - // find closest by squer distance - it = it_l; - ClosePoint cp; - for (it = it_l; it < it_u; ++it) { - uint32_t diff_y = std::abs(it->first.y() - p.y()); - if (diff_y > manhattan_dist) continue; - float diff_x = it->first.x() - p.x(); - // calculate square distance - float d = (float) diff_y * diff_y + diff_x * diff_x; - if (cp.dist_sq > d) { - cp.dist_sq = d; - cp.index = it->second; - } - } - return cp.index; + size_t unfinished_idx = get_closest_point_index(sd, cp.unfinished_line_idx, cp.hit_point, shapes, s2i); + return std::make_pair(cp.finish_idx, unfinished_idx); } const priv::ProjectionDistance *priv::get_closest_projection( @@ -2065,6 +2116,7 @@ priv::ClosePoint priv::find_close_point(const Point &p, priv::ProjectionDistances priv::choose_best_distance( const VDistances &distances, const ExPolygons &shapes, + const Point ¢er, const ShapePoint2index &s2i) { // collect one closest projection for each outline point @@ -2077,14 +2129,19 @@ priv::ProjectionDistances priv::choose_best_distance( // Distances are relative to projection distance // so first wanted distance is the closest one (ZERO) float wanted_distance = 0.f; + + std::vector mask_distances(s2i.get_count(), {true}); + for (const auto &d : distances) + if (d.empty()) mask_distances[&d - &distances.front()] = false; - // NOTE: Shapes are centered to respect allign of text - Point center(0, 0); // Select point from shapes(text contour) which is closest to center (all in 2d) - uint32_t unfinished_index = get_closest_point_index(center, shapes, distances); - - //PNodes pts = create_nodes(shapes, distances, s2i.get_count()); - //uint32_t unfinished_index2 = get_closest_point_index2(center, pts); + uint32_t unfinished_index = find_closest_point_index(center, shapes, s2i, mask_distances); + +#ifdef DEBUG_OUTPUT_DIR + std::vector> connections; + connections.reserve(shapes.size()); + connections.emplace_back(unfinished_index, unfinished_index); +#endif // DEBUG_OUTPUT_DIR do { const ProjectionDistance* pd = get_closest_projection(distances[unfinished_index], wanted_distance); @@ -2094,45 +2151,26 @@ priv::ProjectionDistances priv::choose_best_distance( fill_shape_distances(unfinished_index, *pd, result, finished_shapes, s2i, shapes, distances); // The most close points between finished and unfinished shapes - unfinished_index = std::numeric_limits::max(); - ClosePoint best_cp; // must be finished - - // for each unfinished points - for (uint32_t shape_index = 0; shape_index < shapes.size(); ++shape_index) { - if (finished_shapes[shape_index]) continue; - const ExPolygon &shape = shapes[shape_index]; - uint32_t index = s2i.calc_index({shape_index, 0, 0}); - auto find_close_point_in_points = - [&unfinished_index, &best_cp, - &index, &result, &finished_shapes, &distances, &s2i, &shapes] - (const Points &pts) { - for (const Point &p : pts) { - if (distances[index].empty()){ - ++index; - continue; - } - ClosePoint cp = find_close_point(p, result, finished_shapes, s2i, shapes); - if (cp.index != std::numeric_limits::max() && - best_cp.dist_sq > cp.dist_sq) { - best_cp = cp; // copy - unfinished_index = index; - } - ++index; - } - }; - find_close_point_in_points(shape.contour.points); - // shape could be inside of another shape's hole - for (const Polygon &h : shape.holes) - find_close_point_in_points(h.points); - } - // detect finish (best doesn't have value) - if (best_cp.index == std::numeric_limits::max()) break; + auto [finished, unfinished] = find_closest_point_pair( + shapes, finished_shapes, s2i, mask_distances); - const ProjectionDistance &closest_pd = result[best_cp.index]; + // detection of end (best doesn't have value) + if (finished == std::numeric_limits::max()) break; + + assert(unfinished != std::numeric_limits::max()); + const ProjectionDistance &closest_pd = result[finished]; // check that best_cp is finished and has result assert(closest_pd.aoi_index != std::numeric_limits::max()); wanted_distance = closest_pd.distance; - } while (unfinished_index != std::numeric_limits::max()); + unfinished_index = unfinished; + +#ifdef DEBUG_OUTPUT_DIR + connections.emplace_back(finished, unfinished); +#endif // DEBUG_OUTPUT_DIR + } while (true); //(unfinished_index != std::numeric_limits::max()); +#ifdef DEBUG_OUTPUT_DIR + store(shapes, mask_distances, connections, DEBUG_OUTPUT_DIR + "closest_points.svg"); +#endif // DEBUG_OUTPUT_DIR return result; } @@ -3411,6 +3449,50 @@ void priv::store(const ProjectionDistances &pds, its_write_obj(its, file.c_str()); } +#include "SVG.hpp" +void priv::store(const ExPolygons &shapes, + const std::vector &mask, + const std::vector> &connections, + const std::string &file_svg) +{ + auto bb = get_extents(shapes); + int width = get_extents(shapes.front()).size().x() / 70; + + SVG svg(file_svg, bb); + svg.draw(shapes); + + ShapePoint2index s2i(shapes); + auto get_point = [&shapes, &s2i](size_t i)->Point { + auto id = s2i.calc_id(i); + const ExPolygon &s = shapes[id.expolygons_index]; + const Polygon &p = (id.polygon_index == 0) ? + s.contour : + s.holes[id.polygon_index - 1]; + return p[id.point_index]; + }; + + bool is_first = true; + for (auto c : connections) { + if (is_first) { + is_first = false; + Point p = get_point(c.first); + svg.draw(p, "purple", 4 * width); + continue; + } + Point p1 = get_point(c.first); + Point p2 = get_point(c.second); + svg.draw(Line(p1, p2), "red", width); + } + + for (size_t i = 0; i < s2i.get_count(); i++) { + Point p = get_point(i); + svg.draw(p, "black", 2*width); + if (!mask[i]) + svg.draw(p, "white", width); + } + svg.Close(); +} + namespace priv { /// /// Create model consist of rectangles for each contour edge diff --git a/tests/libslic3r/CMakeLists.txt b/tests/libslic3r/CMakeLists.txt index e21709dd2..4afc121bd 100644 --- a/tests/libslic3r/CMakeLists.txt +++ b/tests/libslic3r/CMakeLists.txt @@ -7,7 +7,6 @@ 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 diff --git a/tests/libslic3r/test_aabbindirect.cpp b/tests/libslic3r/test_aabbindirect.cpp index 87cf80943..2f1c4e652 100644 --- a/tests/libslic3r/test_aabbindirect.cpp +++ b/tests/libslic3r/test_aabbindirect.cpp @@ -87,6 +87,49 @@ TEST_CASE("Creating a several 2d lines, testing closest point query", "[AABBIndi REQUIRE(hit_point_out.y() == Approx(0.5)); } +TEST_CASE("Find the closest point from ExPolys", "[ClosestPoint]") { + ////////////////////////////// + // 0 - 3 + // |Ex0| 0 - 3 + // | |p |Ex1| + // 1 - 2 | | + // 1 - 2 + //[0,0] + /////////////////// + ExPolygons ex_polys{ + /*Ex0*/ {{0, 4}, {0, 1}, {2, 1}, {2, 4}}, + /*Ex1*/ {{4, 3}, {4, 0}, {6, 0}, {6, 3}} + }; + Vec2d p{2.5, 3.5}; + + std::vector lines; + auto add_lines = [&lines](const Polygon& poly) { + for (const auto &line : poly.lines()) + lines.emplace_back( + line.a.cast(), + line.b.cast()); + }; + for (const ExPolygon &ex_poly : ex_polys) { + add_lines(ex_poly.contour); + for (const Polygon &hole : ex_poly.holes) + add_lines(hole); + } + AABBTreeIndirect::Tree<2, double> tree = + AABBTreeLines::build_aabb_tree_over_indexed_lines(lines); + + size_t hit_idx_out = std::numeric_limits::max(); + Vec2d hit_point_out; + double distance_sq = AABBTreeLines::squared_distance_to_indexed_lines( + lines, tree, p, hit_idx_out, hit_point_out, 0.24/* < (0.5*0.5) */); + CHECK(hit_idx_out == std::numeric_limits::max()); + distance_sq = AABBTreeLines::squared_distance_to_indexed_lines( + lines, tree, p, hit_idx_out, hit_point_out, 0.26); + CHECK(hit_idx_out != std::numeric_limits::max()); + + //double distance = sqrt(distance_sq); + //const Linef &line = lines[hit_idx_out]; +} + #if 0 #include "libslic3r/EdgeGrid.hpp" #include diff --git a/tests/libslic3r/test_closest_point.cpp b/tests/libslic3r/test_closest_point.cpp deleted file mode 100644 index 75f7d53e3..000000000 --- a/tests/libslic3r/test_closest_point.cpp +++ /dev/null @@ -1,81 +0,0 @@ -#include - -#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)); -} - -TEST_CASE("Find the closest point from 9 unsorted", "[ClosestPoint]") -{ - // 4 - 3 - 0 - // | | | - // 1 - 6 - 5 - // | | | - // 2 - 7 - 8 - Points pts = { - /*0*/ {3, 3}, - /*1*/ {-3, 0}, - /*2*/ {-3, -3}, - /*3*/ {0, 3}, - /*4*/ {-3, 3}, - /*5*/ {3, 0}, - /*6*/ {0, 0}, - /*7*/ {0, -3}, - /*8*/ {3, -3} - }; - CHECK(4 == find_closest(Point{-4, 4}, pts)); - CHECK(4 == find_closest(Point{-2, 2}, pts)); - // check center - CHECK(6 == find_closest(Point{-1, 1}, pts)); - CHECK(6 == find_closest(Point{ 0, 1}, pts)); - CHECK(6 == find_closest(Point{ 1, 1}, pts)); - CHECK(6 == find_closest(Point{-1, 0}, pts)); - CHECK(6 == find_closest(Point{ 0, 0}, pts)); - CHECK(6 == find_closest(Point{ 1, 0}, pts)); - CHECK(6 == find_closest(Point{-1,-1}, pts)); - CHECK(6 == find_closest(Point{ 0,-1}, pts)); - CHECK(6 == find_closest(Point{ 1,-1}, pts)); - CHECK(6 == find_closest(Point{ 0, 0}, pts)); - - CHECK(5 == find_closest(Point{2, 1}, pts)); - CHECK(8 == find_closest(Point{2,-2}, pts)); -} \ No newline at end of file