Keep track of avoidance paths and merge them if possible

src/libslic3r/BranchingTree/BranchingTree.hpp

@@ -76,6 +76,11 @@ struct Node
     {}
 };
 
+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

src/libslic3r/BranchingTree/PointCloud.hpp

@@ -78,14 +78,6 @@ private:
         rtree<PointIndexEl, boost::geometry::index::rstar<16, 4> /* ? */>
             m_ktree;
 
-    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;
-    }
-
     template<class PC>
     static auto *get_node(PC &&pc, size_t id)
     {
@@ -104,6 +96,14 @@ private:
 
 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
@@ -255,13 +255,32 @@ public:
     }
 };
 
+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;
-        bool r = fn(nroot);
+        TraverseReturnT ret{true, true};
-        if (r && nroot.left  >= 0) traverse(pc, nroot.left, fn);
+
-        if (r && nroot.right >= 0) traverse(pc, nroot.right, fn);
+        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);
     }
 }
 

src/libslic3r/SLA/BranchingTreeSLA.cpp

@@ -20,6 +20,14 @@ class BranchingTreeBuilder: public branchingtree::Builder {
 
     std::set<int /*ground node_id that was already processed*/> m_ground_mem;
 
+    // Establish an index of
+    using PointIndexEl = std::pair<Vec3f, unsigned>;
+    boost::geometry::index::
+        rtree<PointIndexEl, boost::geometry::index::rstar<16, 4> /* ? */>
+            m_pillar_index;
+
+    std::vector<branchingtree::Node> m_pillars;
+
     // Scaling of the input value 'widening_factor:<0, 1>' to produce resonable
     // widening behaviour
     static constexpr double WIDENING_SCALE = 0.02;
@@ -61,8 +69,6 @@ class BranchingTreeBuilder: public branchingtree::Builder {
                                          toR);
                 m_builder.add_junction(tod, toR);
             }
-
-            return true;
         });
     }
 
@@ -72,7 +78,7 @@ class BranchingTreeBuilder: public branchingtree::Builder {
         // As a last resort, try to route child nodes to ground and stop
         // traversing if any child branch succeeds.
         traverse(m_cloud, root, [this](const branchingtree::Node &node) {
-            bool ret = true;
+            branchingtree::TraverseReturnT ret{true, true};
 
             int suppid_parent = m_cloud.get_leaf_id(node.id);
             int suppid_left   = branchingtree::Node::ID_NONE;
@@ -83,12 +89,12 @@ class BranchingTreeBuilder: public branchingtree::Builder {
             dst.Rmin = std::max(node.Rmin, dst.Rmin);
 
             if (node.left >= 0 && add_ground_bridge(m_cloud.get(node.left), dst))
-                ret = false;
+                ret.to_left = false;
             else
                 suppid_left = m_cloud.get_leaf_id(node.left);
 
             if (node.right >= 0 && add_ground_bridge(m_cloud.get(node.right), dst))
-                ret = false;
+                ret.to_right = false;
             else
                 suppid_right = m_cloud.get_leaf_id(node.right);
 
@@ -141,7 +147,7 @@ public:
 };
 
 bool BranchingTreeBuilder::add_bridge(const branchingtree::Node &from,
-                                  const branchingtree::Node &to)
+                                      const branchingtree::Node &to)
 {
     Vec3d fromd = from.pos.cast<double>(), tod = to.pos.cast<double>();
     double fromR = get_radius(from), toR = get_radius(to);
@@ -183,12 +189,72 @@ bool BranchingTreeBuilder::add_ground_bridge(const branchingtree::Node &from,
 {
     bool ret = false;
 
+    namespace bgi = boost::geometry::index;
+
+    struct Output {
+        std::optional<PointIndexEl> &res;
+
+        Output& operator *() { return *this; }
+        void operator=(const PointIndexEl &el) { res = el; }
+        Output& operator++() { return *this; }
+    };
+
     auto it = m_ground_mem.find(from.id);
     if (it == m_ground_mem.end()) {
-        ret = search_ground_route(ex_tbb, m_builder, m_sm,
+        std::optional<PointIndexEl> result;
-                                   sla::Junction{from.pos.cast<double>(),
+        auto filter = bgi::satisfies(
-                                                 get_radius(from)},
+            [this, &from](const PointIndexEl &e) {
-                                   get_radius(to)).first;
+                auto len = (from.pos - e.first).norm();
+                return !branchingtree::is_occupied(m_pillars[e.second])
+                       && len < m_sm.cfg.max_bridge_length_mm
+                       && !m_cloud.is_outside_support_cone(from.pos, e.first)
+                       && beam_mesh_hit(ex_tbb,
+                                        m_sm.emesh,
+                                        Beam{Ball{from.pos.cast<double>(),
+                                                  get_radius(from)},
+                                             Ball{e.first.cast<double>(),
+                                                  get_radius(
+                                                      m_pillars[e.second])}},
+                                        0.9 * m_sm.cfg.safety_distance_mm)
+                                  .distance()
+                              > len;
+            });
+        m_pillar_index.query(filter && bgi::nearest(from.pos, 1), Output{result});
+
+        sla::Junction j{from.pos.cast<double>(), get_radius(from)};
+        if (!result) {
+            auto [found_conn, cjunc] = optimize_ground_connection(
+                                           ex_tbb,
+                                           m_builder,
+                                           m_sm,
+                                           j,
+                                           get_radius(to));
+
+            if (found_conn) {
+                Vec3d endp = cjunc? cjunc->pos : j.pos;
+                double R   = cjunc? cjunc->r   : j.r;
+                Vec3d  dir = cjunc? Vec3d((j.pos - cjunc->pos).normalized()) : DOWN;
+                auto plr = create_ground_pillar(ex_tbb, m_builder, m_sm, endp, dir, R, get_radius(to));
+
+                if (plr.second >= 0) {
+                    m_builder.add_junction(endp, R);
+                    if (cjunc) {
+                        m_builder.add_diffbridge(j.pos, endp, j.r, R);
+                        branchingtree::Node n{cjunc->pos.cast<float>(), float(R)};
+                        n.left = from.id;
+                        m_pillars.emplace_back(n);
+                        m_pillar_index.insert({n.pos, m_pillars.size() - 1});
+                    }
+
+                    ret = true;
+                }
+            }
+        } else {
+            const auto &resnode = m_pillars[result->second];
+            m_builder.add_diffbridge(j.pos, resnode.pos.cast<double>(), j.r, get_radius(resnode));
+            m_pillars[result->second].right = from.id;
+            ret = true;
+        }
 
         // Remember that this node was tested if can go to ground, don't
         // test it with any other destination ground point because

src/libslic3r/SLA/SupportTreeBuilder.hpp

@@ -189,6 +189,10 @@ struct DiffBridge: public Bridge {
     DiffBridge(const Vec3d &p_s, const Vec3d &p_e, double r_s, double r_e)
         : Bridge{p_s, p_e, r_s}, end_r{r_e}
     {}
+
+    DiffBridge(const Junction &j_s, const Junction &j_e)
+        : Bridge{j_s.pos, j_e.pos, j_s.r}, end_r{j_e.r}
+    {}
 };
 
 // This class will hold the support tree parts (not meshes, but logical parts)

src/libslic3r/SLA/SupportTreeUtils.hpp

@@ -283,17 +283,17 @@ Hit pinhead_mesh_hit(Ex              ex,
 
 template<class Ex>
 Hit pinhead_mesh_hit(Ex              ex,
-                           const AABBMesh &mesh,
+                     const AABBMesh &mesh,
-                           const Head     &head,
+                     const Head     &head,
-                           double          safety_d)
+                     double          safety_d)
 {
     return pinhead_mesh_hit(ex, mesh, head.pos, head.dir, head.r_pin_mm,
-                                  head.r_back_mm, head.width_mm, safety_d);
+                            head.r_back_mm, head.width_mm, safety_d);
 }
 
 template<class Ex>
-std::optional<DiffBridge> search_widening_path(Ex policy,
+std::optional<DiffBridge> search_widening_path(Ex                     policy,
-                                               const SupportableMesh     &sm,
+                                               const SupportableMesh &sm,
                                                const Vec3d           &jp,
                                                const Vec3d           &dir,
                                                double                 radius,
@@ -635,56 +635,61 @@ std::optional<Head> calculate_pinhead_placement(Ex                     policy,
 }
 
 template<class Ex>
-std::pair<bool, long> connect_to_ground(Ex                     policy,
+std::pair<bool, std::optional<Junction>> find_ground_connection(
-                                        SupportTreeBuilder    &builder,
+    Ex                     policy,
-                                        const SupportableMesh &sm,
+    SupportTreeBuilder    &builder,
-                                        const Junction        &j,
+    const SupportableMesh &sm,
-                                        const Vec3d           &dir,
+    const Junction        &j,
-                                        double                 end_r)
+    const Vec3d           &dir,
+    double                 end_r)
 {
     auto   hjp = j.pos;
     double r   = j.r;
     auto   sd  = r * sm.cfg.safety_distance_mm / sm.cfg.head_back_radius_mm;
     double r2  = j.r + (end_r - j.r) / (j.pos.z() - ground_level(sm));
 
-    double t   = beam_mesh_hit(policy, sm.emesh, Beam{hjp, dir, r, r2}, sd).distance();
+    double t = beam_mesh_hit(policy, sm.emesh, Beam{hjp, dir, r, r2}, sd)
-    double d   = 0, tdown = 0;
+                   .distance();
-    t          = std::min(t, sm.cfg.max_bridge_length_mm * r / sm.cfg.head_back_radius_mm);
+    double d = 0, tdown = 0;
+    t = std::min(t,
+                 sm.cfg.max_bridge_length_mm * r / sm.cfg.head_back_radius_mm);
 
-    while (d < t &&
+    while (
-           !std::isinf(tdown = beam_mesh_hit(policy, sm.emesh,
+        d < t &&
-                                             Beam{hjp + d * dir, DOWN, r, r2}, sd)
+        !std::isinf(tdown = beam_mesh_hit(policy, sm.emesh,
-                                   .distance())) {
+                                          Beam{hjp + d * dir, DOWN, r, r2}, sd)
+                                .distance())) {
         d += r;
     }
 
-    if(!std::isinf(tdown))
+    std::pair<bool, std::optional<Junction>> ret;
-        return {false, SupportTreeNode::ID_UNSET};
 
-    Vec3d endp = hjp + d * dir;
+    if (std::isinf(tdown)) {
-    double bridge_ratio =  d / (d + (endp.z() - sm.emesh.ground_level()));
+        ret.first = true;
-    double pill_r  = r + bridge_ratio * (end_r - r);
+        if (d > 0) {
-    auto ret = create_ground_pillar(policy, builder, sm, endp, dir, pill_r, end_r);
+            Vec3d  endp         = hjp + d * dir;
+            double bridge_ratio = d / (d + (endp.z() - sm.emesh.ground_level()));
+            double pill_r = r + bridge_ratio * (end_r - r);
 
-    if (ret.second >= 0) {
+            ret.second = Junction{endp, pill_r};
-        builder.add_diffbridge(hjp, endp, r, pill_r);
+        }
-        builder.add_junction(endp, pill_r);
     }
 
     return ret;
 }
 
 template<class Ex>
-std::pair<bool, long> search_ground_route(Ex                     policy,
+std::pair<bool, std::optional<Junction>> optimize_ground_connection(
-                                          SupportTreeBuilder    &builder,
+    Ex                     policy,
-                                          const SupportableMesh &sm,
+    SupportTreeBuilder    &builder,
-                                          const Junction        &j,
+    const SupportableMesh &sm,
-                                          double                 end_radius,
+    const Junction        &j,
-                                          const Vec3d &init_dir = DOWN)
+    double                 end_radius,
+    const Vec3d           &init_dir = DOWN)
 {
     double downdst = j.pos.z() - ground_level(sm);
 
-    auto res = connect_to_ground(policy, builder, sm, j, init_dir, end_radius);
+    auto res = find_ground_connection(policy, builder, sm, j, init_dir, end_radius);
     if (res.first)
         return res;
 
@@ -710,7 +715,59 @@ std::pair<bool, long> search_ground_route(Ex                     policy,
 
     Vec3d bridgedir = spheric_to_dir(oresult.optimum).normalized();
 
-    return connect_to_ground(policy, builder, sm, j, bridgedir, end_radius);
+    return find_ground_connection(policy, builder, sm, j, bridgedir, end_radius);
+}
+
+template<class Ex>
+std::pair<bool, long> connect_to_ground(Ex                     policy,
+                                        SupportTreeBuilder    &builder,
+                                        const SupportableMesh &sm,
+                                        const Junction        &j,
+                                        const Vec3d           &dir,
+                                        double                 end_r)
+{
+    std::pair<bool, long> ret = {false, SupportTreeNode::ID_UNSET};
+
+    auto [found_c, cjunc] = find_ground_connection(policy, builder, sm, j, dir, end_r);
+
+    if (found_c) {
+        Vec3d endp = cjunc? cjunc->pos : j.pos;
+        double R   = cjunc? cjunc->r   : j.r;
+        ret = create_ground_pillar(policy, builder, sm, endp, dir, R, end_r);
+
+        if (ret.second >= 0) {
+            builder.add_diffbridge(j.pos, endp, j.r, R);
+            builder.add_junction(endp, R);
+        }
+    }
+
+    return ret;
+}
+
+template<class Ex>
+std::pair<bool, long> search_ground_route(Ex                     policy,
+                                          SupportTreeBuilder    &builder,
+                                          const SupportableMesh &sm,
+                                          const Junction        &j,
+                                          double                 end_r,
+                                          const Vec3d &init_dir = DOWN)
+{
+    std::pair<bool, long> ret = {false, SupportTreeNode::ID_UNSET};
+
+    auto [found_c, cjunc] = optimize_ground_connection(policy, builder, sm, j, end_r, init_dir);
+    if (found_c) {
+        Vec3d endp = cjunc? cjunc->pos : j.pos;
+        double R   = cjunc? cjunc->r   : j.r;
+        Vec3d  dir = cjunc? Vec3d((j.pos - cjunc->pos).normalized()) : DOWN;
+        ret = create_ground_pillar(policy, builder, sm, endp, dir, R, end_r);
+
+        if (ret.second >= 0) {
+            builder.add_diffbridge(j.pos, endp, j.r, R);
+            builder.add_junction(endp, R);
+        }
+    }
+
+    return ret;
 }
 
 template<class Ex>