QIDISlicer1.0.0

src/libslic3r/BranchingTree/BranchingTree.cpp

@@ -0,0 +1,196 @@
+#include "BranchingTree.hpp"
+#include "PointCloud.hpp"
+
+#include <numeric>
+#include <optional>
+#include <algorithm>
+
+#include "libslic3r/TriangleMesh.hpp"
+
+namespace Slic3r { namespace branchingtree {
+
+void build_tree(PointCloud &nodes, Builder &builder)
+{
+    constexpr size_t initK = 5;
+
+    auto ptsqueue = nodes.start_queue();
+    auto &properties = nodes.properties();
+
+    struct NodeDistance
+    {
+        size_t node_id       = Node::ID_NONE;
+        float  dst_branching = NaNf;
+        float  dst_euql      = NaNf;
+    };
+    auto distances = reserve_vector<NodeDistance>(initK);
+    double prev_dist_max = 0.;
+    size_t K = initK;
+    bool   routed = true;
+    size_t node_id = Node::ID_NONE;
+
+    while ((!ptsqueue.empty() && builder.is_valid()) || !routed) {
+        if (routed) {
+            node_id = ptsqueue.top();
+            ptsqueue.pop();
+        }
+
+        Node node = nodes.get(node_id);
+        nodes.mark_unreachable(node_id);
+
+        distances.clear();
+        distances.reserve(K);
+
+        float dmax = 0.;
+        nodes.foreach_reachable(
+            node.pos,
+            [&distances, &dmax](size_t id, float dst_branching, float dst_euql) {
+                distances.emplace_back(NodeDistance{id, dst_branching, dst_euql});
+                dmax = std::max(dmax, dst_euql);
+            }, K, prev_dist_max);
+
+        std::sort(distances.begin(), distances.end(),
+                  [](auto &a, auto &b) { return a.dst_branching < b.dst_branching; });
+
+        if (distances.empty()) {
+            builder.report_unroutable(node);
+            K = initK;
+            prev_dist_max = 0.;
+            routed = true;
+
+            continue;
+        }
+
+        prev_dist_max = dmax;
+        K *= 2;
+
+        auto closest_it = distances.begin();
+        routed = false;
+        while (closest_it != distances.end() && !routed && builder.is_valid()) {
+            size_t closest_node_id = closest_it->node_id;
+            Node closest_node = nodes.get(closest_node_id);
+
+            auto type = nodes.get_type(closest_node_id);
+            float w = nodes.get(node_id).weight + closest_it->dst_branching;
+            closest_node.Rmin = std::max(node.Rmin, closest_node.Rmin);
+
+            switch (type) {
+            case BED: {
+                closest_node.weight = w;
+                double max_br_len   = nodes.properties().max_branch_length();
+                if (closest_it->dst_branching > max_br_len) {
+                    std::optional<Vec3f> avo = builder.suggest_avoidance(node, max_br_len);
+                    if (!avo)
+                        break;
+
+                    Node new_node {*avo, node.Rmin};
+                    new_node.weight = nodes.get(node_id).weight + (node.pos - *avo).norm();
+                    new_node.left   = node.id;
+                    if ((routed = builder.add_bridge(node, new_node))) {
+                        size_t new_idx = nodes.insert_junction(new_node);
+                        ptsqueue.push(new_idx);
+                    }
+                } else if ((routed = builder.add_ground_bridge(node, closest_node))) {
+                    closest_node.left = closest_node.right = node_id;
+                    nodes.get(closest_node_id) = closest_node;
+                    nodes.mark_unreachable(closest_node_id);
+                }
+
+                break;
+            }
+            case MESH: {
+                closest_node.weight = w;
+                if ((routed = builder.add_mesh_bridge(node, closest_node))) {
+                    closest_node.left = closest_node.right = node_id;
+                    nodes.get(closest_node_id) = closest_node;
+                    nodes.mark_unreachable(closest_node_id);
+                }
+
+                break;
+            }
+            case LEAF:
+            case JUNCTION: {
+                auto max_slope = float(properties.max_slope());
+
+                if (auto mergept = find_merge_pt(node.pos, closest_node.pos, max_slope)) {
+
+                    float mergedist_closest = (*mergept - closest_node.pos).norm();
+                    float mergedist_node = (*mergept - node.pos).norm();
+                    float Wnode = nodes.get(node_id).weight;
+                    float Wclosest = nodes.get(closest_node_id).weight;
+                    float Wsum = std::max(Wnode, Wclosest);
+                    float distsum = std::max(mergedist_closest, mergedist_node);
+                    w = Wsum + distsum;
+
+                    if (mergedist_closest > EPSILON && mergedist_node > EPSILON) {
+                        Node mergenode{*mergept, closest_node.Rmin};
+                        mergenode.weight = w;
+                        mergenode.id = int(nodes.next_junction_id());
+
+                        if ((routed = builder.add_merger(node, closest_node, mergenode))) {
+                            mergenode.left = node_id;
+                            mergenode.right = closest_node_id;
+                            size_t new_idx = nodes.insert_junction(mergenode);
+                            ptsqueue.push(new_idx);
+                            size_t qid = nodes.get_queue_idx(closest_node_id);
+
+                            if (qid != PointCloud::Unqueued)
+                                ptsqueue.remove(nodes.get_queue_idx(closest_node_id));
+
+                            nodes.mark_unreachable(closest_node_id);
+                        }
+                    } else if (closest_node.pos.z() < node.pos.z() &&
+                               (closest_node.left == Node::ID_NONE ||
+                                closest_node.right == Node::ID_NONE)) {
+                        closest_node.weight = w;
+                        if ((routed = builder.add_bridge(node, closest_node))) {
+                            if (closest_node.left == Node::ID_NONE)
+                                closest_node.left = node_id;
+                            else if (closest_node.right == Node::ID_NONE)
+                                closest_node.right = node_id;
+
+                            nodes.get(closest_node_id) = closest_node;
+                        }
+                    }
+                }
+
+                break;
+            }
+            case NONE:;
+            }
+
+            ++closest_it;
+        }
+
+        if (routed) {
+            prev_dist_max = 0.;
+            K = initK;
+        }
+    }
+}
+
+void build_tree(const indexed_triangle_set &its,
+                const std::vector<Node>    &support_roots,
+                Builder                    &builder,
+                const Properties           &properties)
+{
+    PointCloud nodes(its, support_roots, properties);
+
+    build_tree(nodes, builder);
+}
+
+ExPolygon make_bed_poly(const indexed_triangle_set &its)
+{
+    auto bb = bounding_box(its);
+
+    BoundingBox bbcrd{scaled(to_2d(bb.min)), scaled(to_2d(bb.max))};
+    bbcrd.offset(scaled(10.));
+    Point     min = bbcrd.min, max = bbcrd.max;
+    ExPolygon ret = {{min.x(), min.y()},
+                     {max.x(), min.y()},
+                     {max.x(), max.y()},
+                     {min.x(), max.y()}};
+
+    return ret;
+}
+
+}} // namespace Slic3r::branchingtree

src/libslic3r/BranchingTree/BranchingTree.hpp

@@ -0,0 +1,154 @@
+#ifndef SUPPORTTREEBRANCHING_HPP
+#define SUPPORTTREEBRANCHING_HPP
+
+// For indexed_triangle_set
+#include <admesh/stl.h>
+
+#include "libslic3r/ExPolygon.hpp"
+
+namespace Slic3r { namespace branchingtree {
+
+// Branching tree input parameters. This is an in-line fillable structure with
+// setters returning self references.
+class Properties
+{
+    double m_max_slope       = PI / 4.;
+    double m_ground_level    = 0.;
+    double m_sampling_radius = .5;
+    double m_max_branch_len  = 10.;
+
+    ExPolygons m_bed_shape;
+
+public:
+
+    // Maximum slope for bridges of the tree
+    Properties &max_slope(double val) noexcept
+    {
+        m_max_slope = val;
+        return *this;
+    }
+    // Z level of the ground
+    Properties &ground_level(double val) noexcept
+    {
+        m_ground_level = val;
+        return *this;
+    }
+    // How far should sample points be in the mesh and the ground
+    Properties &sampling_radius(double val) noexcept
+    {
+        m_sampling_radius = val;
+        return *this;
+    }
+    // Shape of the print bed (ground)
+    Properties &bed_shape(ExPolygons bed) noexcept
+    {
+        m_bed_shape = std::move(bed);
+        return *this;
+    }
+
+    Properties &max_branch_length(double val) noexcept
+    {
+        m_max_branch_len = val;
+        return *this;
+    }
+
+    double max_slope() const noexcept { return m_max_slope; }
+    double ground_level() const noexcept { return m_ground_level; }
+    double sampling_radius() const noexcept { return m_sampling_radius; }
+    double max_branch_length() const noexcept { return m_max_branch_len; }
+    const ExPolygons &bed_shape() const noexcept { return m_bed_shape; }
+};
+
+// A junction of the branching tree with position and radius.
+struct Node
+{
+    static constexpr int ID_NONE = -1;
+
+    int id = ID_NONE, left = ID_NONE, right = ID_NONE;
+
+    Vec3f pos;
+    float Rmin = 0.f;
+
+    // Tracking the weight of each junction, which is essentially the sum of
+    // the lenghts of all branches emanating from this junction.
+    float weight = 0.f;
+
+    Node(const Vec3f &p, float r_min = .0f) : pos{p}, Rmin{r_min}, weight{0.f}
+    {}
+};
+
+inline bool is_occupied(const Node &n)
+{
+    return n.left != Node::ID_NONE && n.right != Node::ID_NONE;
+}
+
+// An output interface for the branching tree generator function. Consider each
+// method as a callback and implement the actions that need to be done.
+class Builder
+{
+public:
+    virtual ~Builder() = default;
+
+    // A simple bridge from junction to junction.
+    virtual bool add_bridge(const Node &from, const Node &to) = 0;
+
+    // An Y shaped structure with two starting points and a merge point below
+    // them. The angles will respect the max_slope setting.
+    virtual bool add_merger(const Node &node,
+                            const Node &closest,
+                            const Node &merge_node) = 0;
+
+    // Add an anchor bridge to the ground (print bed)
+    virtual bool add_ground_bridge(const Node &from,
+                                   const Node &to) = 0;
+
+    // Add an anchor bridge to the model body
+    virtual bool add_mesh_bridge(const Node &from, const Node &to) = 0;
+
+    virtual std::optional<Vec3f> suggest_avoidance(const Node &from,
+                                                   float max_bridge_len) const
+    {
+        return {};
+    }
+
+    // Report nodes that can not be routed to an endpoint (model or ground)
+    virtual void report_unroutable(const Node &j) = 0;
+
+    // If returns false, the tree building process shall stop
+    virtual bool is_valid() const { return true; }
+};
+
+// Build the actual tree.
+// its:           The input mesh
+// support_leafs: The input support points
+// builder:       The output interface, describes how to build the tree
+// properties:    Parameters of the tree
+//
+// Notes:
+// The original algorithm implicitly ensures that the generated tree avoids
+// the model body. This implementation uses point sampling of the mesh thus an
+// explicit check is needed if the part of the tree being inserted properly
+// avoids the model. This can be done in the builder implementation. Each
+// method can return a boolean indicating whether the given branch can or
+// cannot be inserted. If a particular path is unavailable, the algorithm
+// will try a few other paths as well. If all of them fail, one of the
+// report_unroutable_* methods will be called as a last resort.
+void build_tree(const indexed_triangle_set &its,
+                const std::vector<Node>    &support_leafs,
+                Builder                    &builder,
+                const Properties           &properties = {});
+
+inline void build_tree(const indexed_triangle_set &its,
+                       const std::vector<Node>    &support_leafs,
+                       Builder                   &&builder,
+                       const Properties           &properties = {})
+{
+    build_tree(its, support_leafs, builder, properties);
+}
+
+// Helper function to derive a bed polygon only from the model bounding box.
+ExPolygon make_bed_poly(const indexed_triangle_set &its);
+
+}} // namespace Slic3r::branchingtree
+
+#endif // SUPPORTTREEBRANCHING_HPP

src/libslic3r/BranchingTree/PointCloud.cpp

@@ -0,0 +1,186 @@
+#include "PointCloud.hpp"
+
+#include "libslic3r/Tesselate.hpp"
+#include "libslic3r/SLA/SupportTreeUtils.hpp"
+
+#include <igl/random_points_on_mesh.h>
+
+namespace Slic3r { namespace branchingtree {
+
+std::optional<Vec3f> find_merge_pt(const Vec3f &A, const Vec3f &B, float max_slope)
+{
+    return sla::find_merge_pt(A, B, max_slope);
+}
+
+void to_eigen_mesh(const indexed_triangle_set &its,
+                   Eigen::MatrixXd            &V,
+                   Eigen::MatrixXi            &F)
+{
+    V.resize(its.vertices.size(), 3);
+    F.resize(its.indices.size(), 3);
+    for (unsigned int i = 0; i < its.indices.size(); ++i)
+        F.row(i) = its.indices[i];
+
+    for (unsigned int i = 0; i < its.vertices.size(); ++i)
+        V.row(i) = its.vertices[i].cast<double>();
+}
+
+std::vector<Node> sample_mesh(const indexed_triangle_set &its, double radius)
+{
+    std::vector<Node> ret;
+
+    double surface_area = 0.;
+    for (const Vec3i &face : its.indices) {
+        std::array<Vec3f, 3> tri = {its.vertices[face(0)],
+                                    its.vertices[face(1)],
+                                    its.vertices[face(2)]};
+
+        auto U = tri[1] - tri[0], V = tri[2] - tri[0];
+        surface_area += 0.5 * U.cross(V).norm();
+    }
+
+    int N = surface_area / (PI * radius * radius);
+
+    Eigen::MatrixXd B;
+    Eigen::MatrixXi FI;
+    Eigen::MatrixXd V;
+    Eigen::MatrixXi F;
+    to_eigen_mesh(its, V, F);
+    igl::random_points_on_mesh(N, V, F, B, FI);
+
+    ret.reserve(size_t(N));
+    for (int i = 0; i < FI.size(); i++) {
+        int face_id = FI(i);
+
+        if (face_id < 0 || face_id >= int(its.indices.size()))
+            continue;
+
+        Vec3i face = its.indices[face_id];
+
+        if (face(0) >= int(its.vertices.size()) ||
+            face(1) >= int(its.vertices.size()) ||
+            face(2) >= int(its.vertices.size()))
+            continue;
+
+        Vec3f c = B.row(i)(0) * its.vertices[face(0)] +
+                  B.row(i)(1) * its.vertices[face(1)] +
+                  B.row(i)(2) * its.vertices[face(2)];
+
+        ret.emplace_back(c);
+    }
+
+    return ret;
+}
+
+std::vector<Node> sample_bed(const ExPolygons &bed, float z, double radius)
+{
+    auto triangles = triangulate_expolygons_3d(bed, z);
+    indexed_triangle_set its;
+    its.vertices.reserve(triangles.size());
+
+    for (size_t i = 0; i < triangles.size(); i += 3) {
+        its.vertices.emplace_back(triangles[i].cast<float>());
+        its.vertices.emplace_back(triangles[i + 1].cast<float>());
+        its.vertices.emplace_back(triangles[i + 2].cast<float>());
+
+        its.indices.emplace_back(i, i + 1, i + 2);
+    }
+
+    return sample_mesh(its, radius);
+}
+
+PointCloud::PointCloud(const indexed_triangle_set &M,
+                       std::vector<Node>           support_leafs,
+                       const Properties           &props)
+    : PointCloud{sample_mesh(M, props.sampling_radius()),
+                 sample_bed(props.bed_shape(),
+                            props.ground_level(),
+                            props.sampling_radius()),
+                 std::move(support_leafs), props}
+{}
+
+PointCloud::PointCloud(std::vector<Node> meshpts,
+                       std::vector<Node> bedpts,
+                       std::vector<Node> support_leafs,
+                       const Properties &props)
+    : m_leafs{std::move(support_leafs)}
+    , m_meshpoints{std::move(meshpts)}
+    , m_bedpoints{std::move(bedpts)}
+    , m_props{props}
+    , cos2bridge_slope{std::cos(props.max_slope()) *
+                       std::abs(std::cos(props.max_slope()))}
+    , MESHPTS_BEGIN{m_bedpoints.size()}
+    , LEAFS_BEGIN{MESHPTS_BEGIN + m_meshpoints.size()}
+    , JUNCTIONS_BEGIN{LEAFS_BEGIN + m_leafs.size()}
+    , 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()}
+{
+    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) {
+        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) {
+        Node &n = m_leafs[i];
+        n.id    = int(LEAFS_BEGIN + i);
+        n.left  = Node::ID_NONE;
+        n.right = Node::ID_NONE;
+
+        m_ktree.insert({n.pos, n.id});
+    }
+}
+
+float PointCloud::get_distance(const Vec3f &p, size_t node_id) const
+{
+    auto t = get_type(node_id);
+    auto ret = std::numeric_limits<float>::infinity();
+    const auto &node = get(node_id);
+    
+    switch (t) {
+    case MESH:
+    case BED: {
+        // Points of mesh or bed which are outside of the support cone of
+        // 'pos' must be discarded.
+        if (is_outside_support_cone(p, node.pos))
+            ret = std::numeric_limits<float>::infinity();
+        else
+            ret  = (node.pos - p).norm();
+        
+        break;
+    }
+    case LEAF:
+    case JUNCTION:{
+        auto mergept = find_merge_pt(p, node.pos, m_props.max_slope());
+        double maxL2 = m_props.max_branch_length() * m_props.max_branch_length();
+
+        if (!mergept || mergept->z() < (m_props.ground_level() + 2 * node.Rmin))
+            ret = std::numeric_limits<float>::infinity();
+        else if (double a = (node.pos - *mergept).squaredNorm(),
+                 b        = (p - *mergept).squaredNorm();
+                 a < maxL2 && b < maxL2)
+            ret = std::sqrt(b);
+
+        break;
+    }
+    case NONE:
+        ;
+    }
+    
+    // Setting the ret val to infinity will effectively discard this
+    // connection of nodes. max_branch_length property is used here
+    // to discard node=>node and node=>mesh connections longer than this
+    // property.
+    if (t != BED && ret > m_props.max_branch_length())
+        ret = std::numeric_limits<float>::infinity();
+    
+    return ret;
+}
+
+}} // namespace Slic3r::branchingtree

src/libslic3r/BranchingTree/PointCloud.hpp

@@ -0,0 +1,296 @@
+#ifndef POINTCLOUD_HPP
+#define POINTCLOUD_HPP
+
+#include <optional>
+
+#include "BranchingTree.hpp"
+
+//#include "libslic3r/Execution/Execution.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,
+                                   const Vec3f &B,
+                                   float        max_slope);
+
+void to_eigen_mesh(const indexed_triangle_set &its,
+                   Eigen::MatrixXd            &V,
+                   Eigen::MatrixXi            &F);
+
+std::vector<Node> sample_mesh(const indexed_triangle_set &its, double radius);
+
+std::vector<Node> sample_bed(const ExPolygons &bed,
+                                 float             z,
+                                 double            radius = 1.);
+
+enum PtType { LEAF, MESH, BED, JUNCTION, NONE };
+
+inline BoundingBox3Base<Vec3f> get_support_cone_bb(const Vec3f &p, const Properties &props)
+{
+    double gnd = props.ground_level() - EPSILON;
+    double h   = p.z() - gnd;
+    double phi = PI / 2 - props.max_slope();
+    auto   r   = float(std::min(h * std::tan(phi), props.max_branch_length() * std::sin(phi)));
+
+    Vec3f bb_min = {p.x() - r, p.y() - r, float(gnd)};
+    Vec3f bb_max = {p.x() + r, p.y() + r, p.z()};
+
+    return {bb_min, bb_max};
+}
+
+// A cloud of points including support points, mesh points, junction points
+// and anchor points on the bed. Junction points can be added or removed, all
+// the other point types are established on creation and remain unchangeable.
+class PointCloud {
+    std::vector<Node> m_leafs, m_junctions, m_meshpoints, m_bedpoints;
+
+    const branchingtree::Properties &m_props;
+
+    const double cos2bridge_slope;
+    const size_t MESHPTS_BEGIN, LEAFS_BEGIN, JUNCTIONS_BEGIN;
+
+private:
+
+    // These vectors have the same size as there are indices for nodes to keep
+    // access complexity constant. WARN: there might be cache non-locality costs
+    std::vector<bool> m_searchable_indices; // searchable flag value of a node
+    std::vector<size_t> m_queue_indices;    // queue id of a node if queued
+
+    size_t m_reachable_cnt;
+
+    struct CoordFn
+    {
+        const PointCloud *self;
+        CoordFn(const PointCloud *s) : self{s} {}
+        float operator()(size_t nodeid, size_t dim) const
+        {
+            return self->get(nodeid).pos(int(dim));
+        }
+    };
+
+    using PointIndexEl = std::pair<Vec3f, unsigned>;
+
+    boost::geometry::index::
+        rtree<PointIndexEl, boost::geometry::index::rstar<16, 4> /* ? */>
+            m_ktree;
+
+    template<class PC>
+    static auto *get_node(PC &&pc, size_t id)
+    {
+        auto *ret = decltype(pc.m_bedpoints.data())(nullptr);
+
+        switch(pc.get_type(id)) {
+        case BED: ret = &pc.m_bedpoints[id]; break;
+        case MESH: ret = &pc.m_meshpoints[id - pc.MESHPTS_BEGIN]; break;
+        case LEAF: ret = &pc.m_leafs [id - pc.LEAFS_BEGIN]; break;
+        case JUNCTION: ret = &pc.m_junctions[id - pc.JUNCTIONS_BEGIN]; break;
+        case NONE: assert(false);
+        }
+
+        return ret;
+    }
+
+public:
+
+    bool is_outside_support_cone(const Vec3f &supp, const Vec3f &pt) const
+    {
+        Vec3d D = (pt - supp).cast<double>();
+        double dot_sq = -D.z() * std::abs(-D.z());
+
+        return dot_sq < D.squaredNorm() * cos2bridge_slope;
+    }
+
+    static constexpr auto Unqueued = size_t(-1);
+
+    struct ZCompareFn
+    {
+        const PointCloud *self;
+        ZCompareFn(const PointCloud *s) : self{s} {}
+        bool operator()(size_t node_a, size_t node_b) const
+        {
+            return self->get(node_a).pos.z() > self->get(node_b).pos.z();
+        }
+    };
+
+    PointCloud(const indexed_triangle_set &M,
+               std::vector<Node>           support_leafs,
+               const Properties           &props);
+
+    PointCloud(std::vector<Node> meshpts,
+               std::vector<Node> bedpts,
+               std::vector<Node> support_leafs,
+               const Properties &props);
+
+    PtType get_type(size_t node_id) const
+    {
+        PtType ret = NONE;
+
+        if (node_id < MESHPTS_BEGIN && !m_bedpoints.empty()) ret = BED;
+        else if (node_id < LEAFS_BEGIN && !m_meshpoints.empty()) ret = MESH;
+        else if (node_id < JUNCTIONS_BEGIN && !m_leafs.empty()) ret = LEAF;
+        else if (node_id >= JUNCTIONS_BEGIN && !m_junctions.empty()) ret = JUNCTION;
+
+        return ret;
+    }
+
+    const Node &get(size_t node_id) const
+    {
+        return *get_node(*this, node_id);
+    }
+
+    Node &get(size_t node_id)
+    {
+        return *get_node(*this, node_id);
+    }
+
+    const Node *find(size_t node_id) const { return get_node(*this, node_id); }
+    Node *find(size_t node_id) { return get_node(*this, node_id); }
+
+    // Return the original index of a leaf in the input array, if the given
+    // node id is indeed of type SUPP
+    int get_leaf_id(size_t node_id) const
+    {
+        return node_id >= LEAFS_BEGIN && node_id < JUNCTIONS_BEGIN ?
+                   node_id - LEAFS_BEGIN :
+                   Node::ID_NONE;
+    }
+
+    size_t get_queue_idx(size_t node_id) const { return m_queue_indices[node_id]; }
+
+    float get_distance(const Vec3f &p, size_t node) const;
+
+    size_t next_junction_id() const
+    {
+        return JUNCTIONS_BEGIN + m_junctions.size();
+    }
+
+    size_t insert_junction(const Node &p)
+    {
+        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;
+
+        return new_id;
+    }
+
+    const std::vector<Node> &get_junctions() const noexcept  { return m_junctions; }
+    const std::vector<Node> &get_bedpoints() const noexcept  { return m_bedpoints; }
+    const std::vector<Node> &get_meshpoints() const noexcept { return m_meshpoints; }
+    const std::vector<Node> &get_leafs() const noexcept      { return m_leafs; }
+    const Properties & properties() const noexcept { return m_props; }
+
+    void mark_unreachable(size_t node_id)
+    {
+        assert(node_id < m_searchable_indices.size());
+
+        m_searchable_indices[node_id] = false;
+        m_queue_indices[node_id] = Unqueued;
+        --m_reachable_cnt;
+    }
+
+    size_t reachable_count() const { return m_reachable_cnt; }
+
+    template<class Fn>
+    void foreach_reachable(const Vec3f &pos,
+                           Fn         &&visitor,
+                           size_t       k,
+                           double       min_dist = 0.)
+    {
+        // Fake output iterator
+        struct Output {
+            const PointCloud *self;
+            Vec3f p;
+            Fn &visitorfn;
+
+            Output& operator *() { return *this; }
+            void operator=(const PointIndexEl &el) {
+                visitorfn(el.second, self->get_distance(p, el.second),
+                             (p - el.first).squaredNorm());
+            }
+            Output& operator++() { return *this; }
+        };
+
+        namespace bgi = boost::geometry::index;
+        float brln = 2 * m_props.max_branch_length();
+        BoundingBox3Base<Vec3f> bb{{pos.x() - brln, pos.y() - brln,
+                                    float(m_props.ground_level() - EPSILON)},
+                                   {pos.x() + brln, pos.y() + brln,
+                                    m_ktree.bounds().max_corner().get<Z>()}};
+
+        // Extend upwards to find mergable junctions and support points
+        bb.max.z() = m_ktree.bounds().max_corner().get<Z>();
+
+        auto filter = bgi::satisfies(
+                          [this, &pos, min_dist](const PointIndexEl &e) {
+                              double D_branching = get_distance(pos, e.second);
+                              double D_euql      = (pos - e.first).squaredNorm() ;
+                              return m_searchable_indices[e.second] &&
+                                     !std::isinf(D_branching) && D_euql > min_dist;
+                          });
+
+        m_ktree.query(bgi::intersects(bb) && filter && bgi::nearest(pos, k),
+                      Output{this, pos, visitor});
+    }
+
+    auto start_queue()
+    {
+        auto ptsqueue = make_mutable_priority_queue<size_t, true>(
+            [this](size_t el, size_t idx) { m_queue_indices[el] = idx; },
+            ZCompareFn{this});
+
+        ptsqueue.reserve(m_leafs.size());
+        size_t iend = LEAFS_BEGIN + m_leafs.size();
+        for (size_t i = LEAFS_BEGIN; i < iend; ++i)
+            ptsqueue.push(i);
+
+        return ptsqueue;
+    }
+};
+
+template<class Fn> constexpr bool IsTraverseFn = std::is_invocable_v<Fn, Node&>;
+
+struct TraverseReturnT
+{
+    bool to_left : 1; // if true, continue traversing to the left
+    bool to_right : 1; // if true, continue traversing to the right
+};
+
+template<class PC, class Fn> void traverse(PC &&pc, size_t root, Fn &&fn)
+{
+    if (auto nodeptr = pc.find(root); nodeptr != nullptr) {
+        auto &nroot = *nodeptr;
+        TraverseReturnT ret{true, true};
+
+        if constexpr (std::is_same_v<std::invoke_result_t<Fn, decltype(nroot)>, void>) {
+            // Our fn has no return value
+            fn(nroot);
+        } else {
+            // Fn returns instructions about how to continue traversing
+            ret = fn(nroot);
+        }
+
+        if (ret.to_left && nroot.left >= 0)
+            traverse(pc, nroot.left, fn);
+        if (ret.to_right && nroot.right >= 0)
+            traverse(pc, nroot.right, fn);
+    }
+}
+
+void build_tree(PointCloud &pcloud, Builder &builder);
+
+inline void build_tree(PointCloud &&pc, Builder &builder)
+{
+    build_tree(pc, builder);
+}
+
+}} // namespace Slic3r::branchingtree
+
+#endif // POINTCLOUD_HPP