From 36ec731adfb2c46780f9e707b549d757fe7afb1b Mon Sep 17 00:00:00 2001 From: tamasmeszaros Date: Thu, 9 Jun 2022 16:26:34 +0200 Subject: [PATCH] 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 | 28 ++++++---- src/libslic3r/BranchingTree/PointCloud.cpp | 19 ++++--- src/libslic3r/BranchingTree/PointCloud.hpp | 52 +++++++++++++------ 3 files changed, 67 insertions(+), 32 deletions(-) diff --git a/src/libslic3r/BranchingTree/BranchingTree.cpp b/src/libslic3r/BranchingTree/BranchingTree.cpp index 3102f213f..4e61ebda3 100644 --- a/src/libslic3r/BranchingTree/BranchingTree.cpp +++ b/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(nodes.reachable_count()); + struct NodeDistance { size_t node_id = Node::ID_NONE; float distance = NaNf; }; + auto distances = reserve_vector(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( + 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; diff --git a/src/libslic3r/BranchingTree/PointCloud.cpp b/src/libslic3r/BranchingTree/PointCloud.cpp index 11b500843..b5b6893c8 100644 --- a/src/libslic3r/BranchingTree/PointCloud.cpp +++ b/src/libslic3r/BranchingTree/PointCloud.cpp @@ -142,16 +142,23 @@ PointCloud::PointCloud(std::vector 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 diff --git a/src/libslic3r/BranchingTree/PointCloud.hpp b/src/libslic3r/BranchingTree/PointCloud.hpp index 3c5e8a9a2..d61013737 100644 --- a/src/libslic3r/BranchingTree/PointCloud.hpp +++ b/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 find_merge_pt(const Vec3f &A, @@ -57,7 +59,11 @@ private: } }; - KDTreeIndirect<3, float, CoordFn> m_ktree; + using PointIndexEl = std::pair; + + boost::geometry::index:: + rtree /* ? */> + 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 support_leafs, - const Properties & props); + PointCloud(const indexed_triangle_set &M, + std::vector support_leafs, + const Properties &props); PointCloud(std::vector meshpts, std::vector 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 void foreach_reachable(const Vec3f &pos, Fn &&visitor) + template + 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()