Replace KDTreeIndirect with boost::rtree for queries of PointCloud

- rtree can be populated with junction points gradually
- Use repeated queries of 5 nearest reachable points in branching tree alg

src/libslic3r/BranchingTree/BranchingTree.cpp

@@ -11,35 +11,42 @@ namespace Slic3r { namespace branchingtree {
 
 bool build_tree(PointCloud &nodes, Builder &builder)
 {
+    constexpr size_t ReachablesToExamine = 5;
+
     auto ptsqueue = nodes.start_queue();
     auto &properties = nodes.properties();
 
-    struct NodeDistance { size_t node_id; float distance; };
-    auto distances = reserve_vector<NodeDistance>(nodes.reachable_count());
+    struct NodeDistance { size_t node_id = Node::ID_NONE; float distance = NaNf; };
+    auto distances = reserve_vector<NodeDistance>(ReachablesToExamine);
+    double prev_dist_max = 0.;
 
     while (!ptsqueue.empty()) {
         size_t node_id = ptsqueue.top();
-        ptsqueue.pop();
 
         Node node = nodes.get(node_id);
         nodes.mark_unreachable(node_id);
 
         distances.clear();
-        distances.reserve(nodes.reachable_count());
 
-        nodes.foreach_reachable(node.pos, [&distances](size_t id, float distance) {
-            if (!std::isinf(distance))
+        nodes.foreach_reachable<ReachablesToExamine>(
+            node.pos,
+            [&distances](size_t id, float distance) {
                 distances.emplace_back(NodeDistance{id, distance});
-        });
+            },
+            prev_dist_max);
 
         std::sort(distances.begin(), distances.end(),
                   [](auto &a, auto &b) { return a.distance < b.distance; });
 
         if (distances.empty()) {
             builder.report_unroutable(node);
+            ptsqueue.pop();
+            prev_dist_max = 0.;
             continue;
         }
 
+        prev_dist_max = distances.back().distance;
+
         auto closest_it = distances.begin();
         bool routed = false;
         while (closest_it != distances.end() && !routed) {
@@ -136,8 +143,11 @@ bool build_tree(PointCloud &nodes, Builder &builder)
             ++closest_it;
         }
 
-        if (!routed)
-            builder.report_unroutable(node);
+        if (routed) {
+            ptsqueue.pop();
+            prev_dist_max = 0.;
+        }
+
     }
 
     return true;

src/libslic3r/BranchingTree/PointCloud.cpp

@@ -142,16 +142,23 @@ PointCloud::PointCloud(std::vector<Node> meshpts,
     , m_searchable_indices(JUNCTIONS_BEGIN + m_junctions.size(), true)
     , m_queue_indices(JUNCTIONS_BEGIN + m_junctions.size(), Unqueued)
     , m_reachable_cnt{JUNCTIONS_BEGIN + m_junctions.size()}
-    , m_ktree{CoordFn{this}, LEAFS_BEGIN} // Only for bed and mesh points
 {
-    for (size_t i = 0; i < m_bedpoints.size(); ++i)
+    for (size_t i = 0; i < m_bedpoints.size(); ++i) {
         m_bedpoints[i].id = int(i);
+        m_ktree.insert({m_bedpoints[i].pos, i});
+    }
     
-    for (size_t i = 0; i < m_meshpoints.size(); ++i)
-        m_meshpoints[i].id = int(MESHPTS_BEGIN + i);
+    for (size_t i = 0; i < m_meshpoints.size(); ++i) {
+        Node &n = m_meshpoints[i];
+        n.id = int(MESHPTS_BEGIN + i);
+        m_ktree.insert({n.pos, n.id});
+    }
     
-    for (size_t i = 0; i < m_leafs.size(); ++i)
-        m_leafs[i].id = int(LEAFS_BEGIN + i);
+    for (size_t i = 0; i < m_leafs.size(); ++i) {
+        Node &n = m_leafs[i];
+        n.id = int(LEAFS_BEGIN + i);
+        m_ktree.insert({n.pos, n.id});
+    }
 }
 
 float PointCloud::get_distance(const Vec3f &p, size_t node_id) const

src/libslic3r/BranchingTree/PointCloud.hpp

@@ -6,9 +6,11 @@
 #include "BranchingTree.hpp"
 
 #include "libslic3r/Execution/Execution.hpp"
-#include "libslic3r/KDTreeIndirect.hpp"
 #include "libslic3r/MutablePriorityQueue.hpp"
 
+#include "libslic3r/BoostAdapter.hpp"
+#include "boost/geometry/index/rtree.hpp"
+
 namespace Slic3r { namespace branchingtree {
 
 std::optional<Vec3f> find_merge_pt(const Vec3f &A,
@@ -57,7 +59,11 @@ private:
         }
     };
 
-    KDTreeIndirect<3, float, CoordFn> m_ktree;
+    using PointIndexEl = std::pair<Vec3f, unsigned>;
+
+    boost::geometry::index::
+        rtree<PointIndexEl, boost::geometry::index::rstar<16, 4> /* ? */>
+            m_ktree;
 
     bool is_outside_support_cone(const Vec3f &supp, const Vec3f &pt) const
     {
@@ -97,9 +103,9 @@ public:
         }
     };
 
-    PointCloud(const indexed_triangle_set & M,
-               std::vector<Node>        support_leafs,
-               const Properties &           props);
+    PointCloud(const indexed_triangle_set &M,
+               std::vector<Node>           support_leafs,
+               const Properties           &props);
 
     PointCloud(std::vector<Node> meshpts,
                std::vector<Node> bedpts,
@@ -154,6 +160,7 @@ public:
         size_t new_id = next_junction_id();
         m_junctions.emplace_back(p);
         m_junctions.back().id = int(new_id);
+        m_ktree.insert({m_junctions.back().pos, new_id});
         m_searchable_indices.emplace_back(true);
         m_queue_indices.emplace_back(Unqueued);
         ++m_reachable_cnt;
@@ -178,21 +185,32 @@ public:
 
     size_t reachable_count() const { return m_reachable_cnt; }
 
-    template<class Fn> void foreach_reachable(const Vec3f &pos, Fn &&visitor)
+    template<size_t K, class Fn>
+    void foreach_reachable(const Vec3f &pos, Fn &&visitor, double min_dist = 0.)
     {
-        auto closest_anchors =
-            find_closest_points<5>(m_ktree, pos, [this, &pos](size_t id) {
-                return m_searchable_indices[id] &&
-                       !is_outside_support_cone(pos, get(id).pos);
-            });
+        // Fake output iterator
+        struct Output {
+            const PointCloud *self;
+            Vec3f p;
+            Fn &visitorfn;
 
-        for (size_t anchor : closest_anchors)
-            if (anchor != m_ktree.npos)
-                visitor(anchor, get_distance(pos, anchor));
+            Output& operator *() { return *this; }
+            void operator=(const PointIndexEl &el) {
+                visitorfn(el.second, self->get_distance(p, el.second));
+            }
+            Output& operator++() { return *this; }
+        };
 
-        for (size_t i = LEAFS_BEGIN; i < m_searchable_indices.size(); ++i)
-            if (m_searchable_indices[i])
-                visitor(i, get_distance(pos, i));
+        namespace bgi = boost::geometry::index;
+
+        size_t cnt = 0;
+        auto filter = bgi::satisfies([this, &pos, min_dist, &cnt](const PointIndexEl &e) {
+            double d = get_distance(pos, e.second);
+            cnt++;
+            return m_searchable_indices[e.second] && !isinf(d) && d > min_dist;
+        });
+
+        m_ktree.query(bgi::nearest(pos, K) && filter, Output{this, pos, visitor});
     }
 
     auto start_queue()