Using aabb tree for lines to detect closest expolygons

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.
+// Returns squared distance to the closest point or -1 if the input is empty 
-template<typename LineType, typename TreeType, typename VectorType>
+// or no closer point than max_sq_dist
+template<typename LineType, typename TreeType, typename VectorType, 
+    typename Scalar = typename VectorType::Scalar>
 inline typename VectorType::Scalar squared_distance_to_indexed_lines(
-        const std::vector<LineType> &lines,
+    const std::vector<LineType> &lines,
-        const TreeType &tree,
+    const TreeType &tree,
-        const VectorType &point,
+    const VectorType &point,
-        size_t &hit_idx_out,
+    size_t &hit_idx_out,
-        Eigen::PlainObjectBase<VectorType> &hit_point_out)
+    Eigen::PlainObjectBase<VectorType> &hit_point_out,
-        {
+    Scalar max_sqr_dist = std::numeric_limits<Scalar>::infinity())
-    using Scalar = typename VectorType::Scalar;
+{
+    if (tree.empty()) return Scalar(-1);
     auto distancer = detail::IndexedLinesDistancer<LineType, TreeType, VectorType>
             { lines, tree, point };
-    return tree.empty() ?
+    hit_idx_out     = std::numeric_limits<size_t>::max();
-                          Scalar(-1) :
+    Scalar distance = AABBTreeIndirect::detail::squared_distance_to_indexed_primitives_recursive(
-                          AABBTreeIndirect::detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0),
+        distancer, size_t(0), Scalar(0), max_sqr_dist, hit_idx_out, hit_point_out);
-                                  std::numeric_limits<Scalar>::infinity(), hit_idx_out, hit_point_out);
+    if (hit_idx_out == std::numeric_limits<size_t>::max()) return Scalar(-1);
+    return distance;
 }
 
 }
-
 }
 
 #endif /* SRC_LIBSLIC3R_AABBTREELINES_HPP_ */

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

src/libslic3r/ClosestPoint.cpp

@@ -1,6 +0,0 @@
-#include "ClosestPoint.hpp"
-#include "Point.hpp"
-
-// Compile template for specific data type
-template<>
-size_t Slic3r::find_closest_in_sorted<Slic3r::Point>(const Slic3r::Point &p, const Slic3r::Points &pts);

src/libslic3r/ClosestPoint.hpp

@@ -1,252 +0,0 @@
-#ifndef slic3r_ClosestPoint_hpp_
-#define slic3r_ClosestPoint_hpp_
-
-#include <vector>
-#include <type_traits>
-
-namespace Slic3r {
-
-/// <summary>
-/// Sort points and use find_closest_in_sorted
-/// </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>
-template<class P>
-// /*SFINAE*/ typename std::enable_if<std::is_member_function_pointer_v<decltype(&P::x)>>::type * = nullptr >
-size_t find_closest(const P &p, const std::vector<P> &pts){
-    // check input
-    if (pts.empty()) return std::numeric_limits<size_t>::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<PP> 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 pp{0, &p};
-    size_t closest_index = find_closest_in_sorted(pp, pts_ord);
-    return pts_ord[closest_index].ord;
-}
-
-/// <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>
-template<class P>
-size_t find_closest_in_sorted(const P &p, const std::vector<P> &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 from thoose points</param>
-/// <param name="sorted_pts">Seach for closest index to thoose points
-/// Sorted points by X coordinate(by function closestPoint::sort_fnc)</param>
-/// <returns>Indices to pts(first) and sorted_pts(second)</returns>
-template<class P>
-std::pair<size_t, size_t> find_closest_in_sorted(
-    const std::vector<P> &pts, const std::vector<P> &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>
-template<class P> bool sort_fnc(const P &p1, const P &p2){ return p1.x() < p2.x(); }
-
-/// <summary> Function used to find upper bound in sorted points. </summary>
-template<class P, typename V> bool upper_fnc(V value, const P &p){ return value < p.x(); }
-/// <summary> Function used to find lower bound in sorted points. </summary>
-template<class P, typename V> bool lower_fnc(const P &p, V value){ return value > p.x(); }
-/// <summary> Calc manhatn size of point. Mainly to explain meaning</summary>
-template<class P> uint32_t manhattan_size(const P &p1, const P &p2)
-{ return std::abs(p1.x()-p2.x()) + abs(p1.y()-p2.y()); }
-
-template<class P>
-typename std::vector<P>::const_iterator get_lower_bound(uint32_t& dist, const P &p, 
-    typename std::vector<P>::const_iterator it_x, const std::vector<P>& 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<P>::const_iterator;
-    It res = std::lower_bound(pts.begin(), it_x, p.x() - dist, lower_fnc<P,V>);
-    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<P,V>);
-        }
-    }
-    return res;
-}
-
-template<class P>
-typename std::vector<P>::const_iterator get_uppper_bound(uint32_t& dist, const P &p, 
-    typename std::vector<P>::const_iterator it_x, const std::vector<P>& pts)
-{
-    assert(it_x >= pts.begin() && it_x < pts.end());
-    
-    using V = decltype(p.x());
-    using It = std::vector<P>::const_iterator;
-    It res = std::upper_bound(it_x, pts.end(), p.x() + dist, upper_fnc<P,V>);
-    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<P,V>);
-        }
-    }
-    return res;
-}
-
-} // namespace closestPoint
-} // namespace Slic3r
-
-template<class P>
-size_t Slic3r::find_closest_in_sorted(const P &p, const std::vector<P> &pts)
-{
-    using namespace closestPoint;
-    // check that input is really sorted
-    assert(std::is_sorted(pts.begin(), pts.end(), sort_fnc<P>));
-
-    // check input
-    if (pts.empty()) return std::numeric_limits<size_t>::max();
-    if (pts.size() == 1) return 0;
-
-    using V = decltype(p.x());
-    using It = std::vector<P>::const_iterator;
-    // closest point node in X
-    It it_x = std::upper_bound(pts.begin(), pts.end(), p.x(), upper_fnc<P,V>);
-    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<float>::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<class P>
-std::pair<size_t, size_t> Slic3r::find_closest_in_sorted(
-    const std::vector<P> &pts, const std::vector<P> &sorted_pts)
-{
-    using namespace closestPoint;
-    // check that input is really sorted
-    assert(std::is_sorted(sorted_pts.begin(), sorted_pts.end(), sort_fnc<P>));
-
-    std::pair<size_t, size_t> res = {std::numeric_limits<size_t>::max(),
-                                     std::numeric_limits<size_t>::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<P>::const_iterator;
-        
-    uint32_t res_dist = std::numeric_limits<uint32_t>::max();
-    float res_dist_sq = std::numeric_limits<float>::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<uint32_t>(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<P, V>);        
-        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_

src/libslic3r/CutSurface.cpp

@@ -411,11 +411,13 @@ VDistances calc_distances(const SurfacePatches &patches,
 /// </summary>
 /// <param name="distances">All distances - Vector distances for each shape point</param>
 /// <param name="shapes">Vector of letters</param>
+/// <param name="center">Center of projection</param>
 /// <param name="shape_point_2_index">Convert index to addresss inside of shape</param>
 /// <returns>Best projection distances</returns>
 ProjectionDistances choose_best_distance(
     const VDistances       &distances,
     const ExPolygons       &shapes,
+    const Point            &center,
     const ShapePoint2index &shape_point_2_index);
 
 /// <summary>
@@ -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<bool>& mask_distances, const std::vector<std::pair<size_t, size_t>>& 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<indexed_triangle_set> &models, const std::string &obj_filename);
 void store(const std::vector<CutMesh>&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<bool> 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<float>::max();
 };
 
-// search in all shapes points to found closest point to given point
+struct SearchData{
-uint32_t get_closest_point_index(const Point &p, const ExPolygons &shapes, const VDistances &distances);
+// IMPROVE: float lines are enough
+std::vector<Linef> lines;
+// convert line index into Shape point index
+std::vector<size_t> cvt;
+// contain lines from prev point to Point index
+AABBTreeIndirect::Tree<2, double> tree;
+};
 
-using PNode = std::pair<Point, uint32_t>;
+SearchData create_search_data(const ExPolygons &shapes, const std::vector<bool>& mask);
-using PNodes = std::vector<PNode>;
+uint32_t get_closest_point_index(const SearchData &sd, size_t line_idx, const Vec2d &hit_point, const ExPolygons &shapes, const ShapePoint2index &s2i);
-PNodes create_nodes(const ExPolygons &shapes, const VDistances &distances, size_t shapes_point_count);
 
-/// <summary>
+// use AABB Tree Lines to find closest point
-/// Search in all shapes points(nodes) to found closest point to given point
+uint32_t find_closest_point_index(const Point &p, const ExPolygons &shapes, const ShapePoint2index &s2i, const std::vector<bool> &mask);
-/// </summary>
+
-/// <param name="p">Point to search closest</param>
+std::pair<uint32_t, uint32_t> find_closest_point_pair(const ExPolygons &shapes, const std::vector<bool> &done_shapes, const ShapePoint2index  &s2i, const std::vector<bool> &mask);
-/// <param name="nodes">Points sorted by X</param>
-/// <returns>Index into shapes of point closest to given point</returns>
-uint32_t get_closest_point_index2(const Point &p, const PNodes& nodes);
 
 // Search for closest projection to wanted distance
 const ProjectionDistance *get_closest_projection(const ProjectionDistances &distance, float wanted_distance);
@@ -1730,135 +1738,178 @@ ClosePoint find_close_point(const Point &p, ProjectionDistances &result, std::ve
 }
 
 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,
+priv::SearchData priv::create_search_data(const ExPolygons        &shapes,
-                                       const ExPolygons &shapes,
+                                          const std::vector<bool> &mask)
-                                       const VDistances &distances)
 {
-    ClosePoint cp;
+    // IMPROVE: Use float precission (it is enough)
-    uint32_t id{0};
+    SearchData sd;
-    auto get_closest = [&distances, &p, &id, &cp]
+    sd.lines.reserve(mask.size());
-    (const Points &pts) {
+    sd.cvt.reserve(mask.size());
-        for (const Point &p_ : pts) {
+    size_t index     = 0;
-            if (distances[id].empty()) { 
+    auto   add_lines = [&sd, &index, &mask]
-                ++id;
+    (const Polygon &poly) {
-                continue;
+        Vec2d prev = poly.back().cast<double>();
-            }
+        bool use_point = mask[index + poly.points.size() - 1];
-            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) {
         for (const Point &p : poly.points) {
-            // skip empty distances
+            if (!use_point) {
-            if (distances[index].empty()) {
+                use_point = mask[index];
-                ++index;
+                if (use_point) prev = p.cast<double>();
-                continue;
+            } else if (!mask[index]) {
+                use_point = false;
+            } else {
+                Vec2d p_d = p.cast<double>();
+                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);
-    }
 
-    std::sort(pts.begin(), pts.end(), [](const PNode &n1, const PNode &n2) {
+    for (const ExPolygon &shape : shapes) {
-        return n1.first.x() < n2.first.x();
+        add_lines(shape.contour);
-    });
+        for (const Polygon &hole : shape.holes) add_lines(hole);
-    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;
 
-    // function to find upper point node
+    bool use_index = abs(line.a[i] - hit_point[i]) >
-    auto f_u = [](coord_t value, const PNode& n) {
+                     abs(line.b[i] - hit_point[i]);
-        return value < n.first.x();
+    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<int>();
+        return p_ == poly[id.point_index];
     };
 
-    // function to find lower point node
+    if (use_index) { 
-    auto f_l = [](const PNode &n, coord_t value) {
+        assert(is_same(line.b, point_index));
-        return value > n.first.x();
+        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;
+}
 
-    // closest point node in X
+// use AABB Tree Lines
-    auto it_x = std::upper_bound(nodes.begin(), nodes.end(), p.x(), f_u);
+uint32_t priv::find_closest_point_index(const Point            &p,
-    // manhatn distance to closest point
+                                        const ExPolygons       &shapes,
-    auto manhattan_size = [](const Point &p) -> uint32_t {
+                                        const ShapePoint2index &s2i,
-        return std::abs(p.x()) + abs(p.y());
+                                        const std::vector<bool> &mask)
-    };
+{    
-    uint32_t manhattan_dist = (it_x != nodes.end())?
+    SearchData sd = create_search_data(shapes, mask);
-        manhattan_size(it_x->first - p) :
+    size_t line_idx = std::numeric_limits<size_t>::max();
-        manhattan_size(nodes.back().first - p);
+    Vec2d  hit_point;
-    // node for lower bound
+    Vec2d  p_d = p.cast<double>();
-    auto  it_l = std::lower_bound(nodes.begin(), it_x, p.x() - manhattan_dist, f_l);
+    double distance_sq = AABBTreeLines::squared_distance_to_indexed_lines(
-    auto it = it_x;
+        sd.lines, sd.tree, p_d, line_idx, hit_point);
-    while (it > it_l) {
+    assert(distance_sq > 0);
-        uint32_t diff_y = std::abs(it->first.y() - p.y());
+    
-        if (diff_y > manhattan_dist) {
+    // IMPROVE: one could use line ratio to find closest point
-            --it;
+    return get_closest_point_index(sd, line_idx, hit_point, shapes, s2i);
+}
+
+std::pair<uint32_t, uint32_t> priv::find_closest_point_pair(
+    const ExPolygons        &shapes,
+    const std::vector<bool> &done_shapes,
+    const ShapePoint2index  &s2i,
+    const std::vector<bool> &mask)
+{
+    assert(mask.size() == s2i.get_count());
+    assert(done_shapes.size() == shapes.size());
+    std::vector<bool> unfinished_mask = mask; // copy
+
+    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<size_t>::max();
+        size_t unfinished_line_idx = std::numeric_limits<size_t>::max();
+        Vec2d  hit_point           = Vec2d();
+        double distance_sq         = std::numeric_limits<double>::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;
+        auto search_in_polygon = [&index, &cp, &sd, &mask](const Polygon& polygon) {
-        if (manhattan_dist > act_dist) {
+            for (size_t i = 0; i < polygon.size(); ++i, ++index) { 
-            manhattan_dist = act_dist;
+                if (mask[index] == false) continue;
-            it_l = std::lower_bound(it_l, it_x, p.x() - manhattan_dist, f_l);
+                Vec2d p_d = polygon[i].cast<double>();
-        }
+                size_t line_idx = std::numeric_limits<size_t>::max();
-        --it;
+                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<size_t>::max()) {
+        return std::make_pair(std::numeric_limits<size_t>::max(),
+                              std::numeric_limits<size_t>::max());
     }
 
-    // node for upper bound
+    size_t unfinished_idx = get_closest_point_index(sd, cp.unfinished_line_idx, cp.hit_point, shapes, s2i);
-    auto  it_u = std::upper_bound(it_x, nodes.end(), p.x() + manhattan_dist, f_u);
+    return std::make_pair(cp.finish_idx, unfinished_idx);
-    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;
 }
 
 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            &center,
     const ShapePoint2index &s2i)
 {
     // collect one closest projection for each outline point
@@ -2078,13 +2130,18 @@ priv::ProjectionDistances priv::choose_best_distance(
     // so first wanted distance is the closest one (ZERO)
     float wanted_distance = 0.f;
     
-    // NOTE: Shapes are centered to respect allign of text
+    std::vector<bool> mask_distances(s2i.get_count(), {true});
-    Point center(0, 0);
+    for (const auto &d : distances)
-    // Select point from shapes(text contour) which is closest to center (all in 2d)
+        if (d.empty()) mask_distances[&d - &distances.front()] = false;
-    uint32_t unfinished_index = get_closest_point_index(center, shapes, distances);
 
-    //PNodes pts = create_nodes(shapes, distances, s2i.get_count());
+    // Select point from shapes(text contour) which is closest to center (all in 2d)
-    //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<std::pair<size_t, size_t>> 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<uint32_t>::max();
+        auto [finished, unfinished] = find_closest_point_pair(
-        ClosePoint best_cp; // must be finished
+            shapes, finished_shapes, s2i, mask_distances);
 
-        // for each unfinished points 
+        // detection of end (best doesn't have value)
-        for (uint32_t shape_index = 0; shape_index < shapes.size(); ++shape_index) {
+        if (finished == std::numeric_limits<uint32_t>::max()) break;
-            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<uint32_t>::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<uint32_t>::max()) break;
 
-        const ProjectionDistance &closest_pd = result[best_cp.index];
+        assert(unfinished != std::numeric_limits<uint32_t>::max());
+        const ProjectionDistance &closest_pd = result[finished];
         // check that best_cp is finished and has result
         assert(closest_pd.aoi_index != std::numeric_limits<uint32_t>::max());
         wanted_distance = closest_pd.distance;
-    } while (unfinished_index != std::numeric_limits<uint32_t>::max());
+        unfinished_index = unfinished;
+
+#ifdef DEBUG_OUTPUT_DIR
+        connections.emplace_back(finished, unfinished);
+#endif // DEBUG_OUTPUT_DIR
+    } while (true); //(unfinished_index != std::numeric_limits<uint32_t>::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<bool>            &mask,
+           const std::vector<std::pair<size_t, size_t>> &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 {
 /// <summary>
 /// Create model consist of rectangles for each contour edge

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

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<Linef> lines;
+    auto add_lines = [&lines](const Polygon& poly) {
+        for (const auto &line : poly.lines())
+            lines.emplace_back(
+                line.a.cast<double>(), 
+                line.b.cast<double>());
+    };
+    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<size_t>::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<size_t>::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<size_t>::max());
+
+    //double distance = sqrt(distance_sq);
+    //const Linef &line = lines[hit_idx_out];
+}
+
 #if 0
 #include "libslic3r/EdgeGrid.hpp"
 #include <iostream>

tests/libslic3r/test_closest_point.cpp

@@ -1,81 +0,0 @@
-#include <catch2/catch.hpp>
-
-#include <libslic3r/ClosestPoint.hpp>
-#include <libslic3r/Point.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<Point>));
-    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<Point>));
-
-    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));
-}