ground route merges

wip on separating ground route search and actual creation 
wip


Some fixes but still problems with pedestals

src/libslic3r/SLA/BranchingTreeSLA.cpp

@@ -32,7 +32,7 @@ class BranchingTreeBuilder: public branchingtree::Builder {
     // widening behaviour
     static constexpr double WIDENING_SCALE = 0.02;
 
-    double get_radius(const branchingtree::Node &j)
+    double get_radius(const branchingtree::Node &j) const
     {
         double w = WIDENING_SCALE * m_sm.cfg.pillar_widening_factor * j.weight;
 
@@ -109,6 +109,44 @@ class BranchingTreeBuilder: public branchingtree::Builder {
         });
     }
 
+    std::optional<PointIndexEl>
+    search_for_existing_pillar(const branchingtree::Node &from) const
+    {
+        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; }
+        };
+
+        std::optional<PointIndexEl> result;
+
+        auto filter = bgi::satisfies([this, &from](const PointIndexEl &e) {
+            assert(e.second < m_pillars.size());
+
+            auto len = (from.pos - e.first).norm();
+            const branchingtree::Node &to = m_pillars[e.second];
+            double sd = m_sm.cfg.safety_distance_mm;
+
+            Beam beam{Ball{from.pos.cast<double>(), get_radius(from)},
+                      Ball{e.first.cast<double>(), get_radius(to)}};
+
+            return !branchingtree::is_occupied(to) &&
+                   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, sd).distance() > len;
+        });
+
+        m_pillar_index.query(filter && bgi::nearest(from.pos, 1),
+                             Output{result});
+
+        return result;
+    }
+
 public:
     BranchingTreeBuilder(SupportTreeBuilder          &builder,
                      const SupportableMesh       &sm,
@@ -153,7 +191,7 @@ bool BranchingTreeBuilder::add_bridge(const branchingtree::Node &from,
     double fromR = get_radius(from), toR = get_radius(to);
     Beam beam{Ball{fromd, fromR}, Ball{tod, toR}};
     auto   hit = beam_mesh_hit(ex_tbb, m_sm.emesh, beam,
-                               0.9 * m_sm.cfg.safety_distance_mm);
+                               m_sm.cfg.safety_distance_mm);
 
     bool ret = hit.distance() > (tod - fromd).norm();
 
@@ -201,53 +239,43 @@ bool BranchingTreeBuilder::add_ground_bridge(const branchingtree::Node &from,
 
     auto it = m_ground_mem.find(from.id);
     if (it == m_ground_mem.end()) {
-        std::optional<PointIndexEl> result;
+        std::optional<PointIndexEl> result = search_for_existing_pillar(from);
-        auto filter = bgi::satisfies(
-            [this, &from](const PointIndexEl &e) {
-                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(
+            auto conn = optimize_ground_connection(
                                            ex_tbb,
                                            m_builder,
                                            m_sm,
                                            j,
                                            get_radius(to));
 
-            if (found_conn) {
+            if (conn) {
-                Vec3d endp = cjunc? cjunc->pos : j.pos;
+                build_ground_connection(m_builder, m_sm, conn);
-                double R   = cjunc? cjunc->r   : j.r;
+                Junction connlast = conn.path.back();
-                Vec3d  dir = cjunc? Vec3d((j.pos - cjunc->pos).normalized()) : DOWN;
+                branchingtree::Node n{connlast.pos.cast<float>(), float(connlast.r)};
-                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;
-                }
+
+//                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];

src/libslic3r/SLA/DefaultSupportTree.cpp

@@ -340,15 +340,13 @@ bool DefaultSupportTree::connect_to_nearpillar(const Head &head,
     return true;
 }
 
-bool DefaultSupportTree::create_ground_pillar(const Vec3d &hjp,
+bool DefaultSupportTree::create_ground_pillar(const Junction &hjp,
                                              const Vec3d &sourcedir,
-                                             double       radius,
                                              long         head_id)
 {
     auto [ret, pillar_id] = sla::create_ground_pillar(suptree_ex_policy,
                                                       m_builder, m_sm, hjp,
-                                                      sourcedir, radius, radius,
+                                                      sourcedir, hjp.r, head_id);
-                                                      head_id);
 
     if (pillar_id >= 0) // Save the pillar endpoint in the spatial index
         m_pillar_index.guarded_insert(m_builder.pillar(pillar_id).endpt,
@@ -587,7 +585,7 @@ void DefaultSupportTree::routing_to_ground()
 
         Head &h = m_builder.head(hid);
 
-        if (!create_ground_pillar(h.junction_point(), h.dir, h.r_back_mm, h.id)) {
+        if (!create_ground_pillar(h.junction(), h.dir, h.id)) {
             BOOST_LOG_TRIVIAL(warning)
                 << "Pillar cannot be created for support point id: " << hid;
             m_iheads_onmodel.emplace_back(h.id);
@@ -615,10 +613,9 @@ void DefaultSupportTree::routing_to_ground()
 
                 if (!connect_to_nearpillar(sidehead, centerpillarID) &&
                     !search_pillar_and_connect(sidehead)) {
-                    Vec3d pstart = sidehead.junction_point();
                     // Vec3d pend = Vec3d{pstart.x(), pstart.y(), gndlvl};
                     // Could not find a pillar, create one
-                    create_ground_pillar(pstart, sidehead.dir, sidehead.r_back_mm, sidehead.id);
+                    create_ground_pillar(sidehead.junction(), sidehead.dir, sidehead.id);
                 }
             }
         }

src/libslic3r/SLA/DefaultSupportTree.hpp

@@ -176,9 +176,8 @@ class DefaultSupportTree {
     // jp is the starting junction point which needs to be routed down.
     // sourcedir is the allowed direction of an optional bridge between the
     // jp junction and the final pillar.
-    bool create_ground_pillar(const Vec3d &jp,
+    bool create_ground_pillar(const Junction &jp,
                               const Vec3d &sourcedir,
-                              double       radius,
                               long         head_id = SupportTreeNode::ID_UNSET);
 
     void add_pillar_base(long pid)

src/libslic3r/SLA/SupportTreeBuilder.hpp

@@ -67,6 +67,14 @@ struct SupportTreeNode
     long id = ID_UNSET; // For identification withing a tree.
 };
 
+// A junction connecting bridges and pillars
+struct Junction: public SupportTreeNode {
+    double r = 1;
+    Vec3d pos;
+
+    Junction(const Vec3d &tr, double r_mm) : r(r_mm), pos(tr) {}
+};
+
 // A pinhead originating from a support point
 struct Head: public SupportTreeNode {
     Vec3d dir = DOWN;
@@ -77,7 +85,6 @@ struct Head: public SupportTreeNode {
     double width_mm = 2;
     double penetration_mm = 0.5;
 
-    
     // If there is a pillar connecting to this head, then the id will be set.
     long pillar_id = ID_UNSET;
     
@@ -104,20 +111,20 @@ struct Head: public SupportTreeNode {
         return real_width() - penetration_mm;
     }
 
+    inline Junction junction() const
+    {
+        Junction j{pos + (fullwidth() - r_back_mm) * dir, r_back_mm};
+        j.id = -this->id; // Remember that this junction is from a head
+
+        return j;
+    }
+
     inline Vec3d junction_point() const
     {
-        return pos + (fullwidth() - r_back_mm) * dir;
+        return junction().pos;
     }
 };
 
-// A junction connecting bridges and pillars
-struct Junction: public SupportTreeNode {
-    double r = 1;
-    Vec3d pos;
-
-    Junction(const Vec3d &tr, double r_mm) : r(r_mm), pos(tr) {}
-};
-
 // A straight pillar. Only has an endpoint and a height. No explicit starting
 // point is given, as it would allow the pillar to be angled.
 // Some connection info with other primitives can also be tracked.

src/libslic3r/SLA/SupportTreeUtils.hpp

@@ -10,6 +10,8 @@
 #include <libslic3r/Geometry.hpp>
 #include <libslic3r/SLA/SupportTreeBuilder.hpp>
 
+#include <boost/variant.hpp>
+#include <boost/container/small_vector.hpp>
 #include <boost/log/trivial.hpp>
 
 namespace Slic3r { namespace sla {
@@ -358,19 +360,19 @@ std::pair<bool, long> create_ground_pillar(
     Ex                     policy,
     SupportTreeBuilder    &builder,
     const SupportableMesh &sm,
-    const Vec3d           &pinhead_junctionpt,
+    const Junction        &pinhead_junctionpt,
     const Vec3d           &sourcedir,
-    double                 radius,
     double                 end_radius,
     long                   head_id = SupportTreeNode::ID_UNSET)
 {
-    Vec3d  jp           = pinhead_junctionpt, endp = jp, dir = sourcedir;
+    Vec3d  jp           = pinhead_junctionpt.pos, endp = jp, dir = sourcedir;
     long   pillar_id    = SupportTreeNode::ID_UNSET;
     bool   can_add_base = false, non_head = false;
 
     double gndlvl = 0.; // The Z level where pedestals should be
     double jp_gnd = 0.; // The lowest Z where a junction center can be
     double gap_dist = 0.; // The gap distance between the model and the pad
+    double radius = pinhead_junctionpt.r;
 
     double r2 = radius + (end_radius - radius) / (jp.z() - ground_level(sm));
 
@@ -634,8 +636,185 @@ std::optional<Head> calculate_pinhead_placement(Ex                     policy,
     return {};
 }
 
+struct GroundConnection {
+    // Currently, a ground connection will contain at most 2 additional junctions
+    // which will not require any allocations. If I come up with an algo that
+    // can produce a route to ground with more junctions, this design will be
+    // able to handle that.
+    static constexpr size_t MaxExpectedJunctions = 3;
+
+    boost::container::small_vector<Junction, MaxExpectedJunctions> path;
+    double end_radius;
+    std::optional<Pedestal> pillar_base;
+
+    operator bool() const { return !path.empty(); }
+};
+
 template<class Ex>
-std::pair<bool, std::optional<Junction>> find_ground_connection(
+GroundConnection find_pillar_route(Ex policy,
+//                                   SupportTreeBuilder &builder,
+                                   const SupportableMesh &sm,
+                                   const Junction &source,
+                                   const Vec3d &sourcedir,
+                                   double end_radius)
+{
+    GroundConnection ret;
+
+    Vec3d  jp           = source.pos, endp = jp, dir = sourcedir;
+//    long   pillar_id    = SupportTreeNode::ID_UNSET;
+    bool   can_add_base = false/*, non_head = false*/;
+
+    double gndlvl = 0.; // The Z level where pedestals should be
+    double jp_gnd = 0.; // The lowest Z where a junction center can be
+    double gap_dist = 0.; // The gap distance between the model and the pad
+    double radius = source.r;
+
+    double r2 = radius + (end_radius - radius) / (jp.z() - ground_level(sm));
+
+    auto to_floor = [&gndlvl](const Vec3d &p) { return Vec3d{p.x(), p.y(), gndlvl}; };
+
+    auto eval_limits = [&sm, &radius, &can_add_base, &gndlvl, &gap_dist, &jp_gnd]
+        (bool base_en = true)
+    {
+        can_add_base  = base_en && radius >= sm.cfg.head_back_radius_mm;
+        double base_r = can_add_base ? sm.cfg.base_radius_mm : 0.;
+        gndlvl        = ground_level(sm);
+        if (!can_add_base) gndlvl -= sm.pad_cfg.wall_thickness_mm;
+        jp_gnd   = gndlvl + (can_add_base ? 0. : sm.cfg.head_back_radius_mm);
+        gap_dist = sm.cfg.pillar_base_safety_distance_mm + base_r + EPSILON;
+    };
+
+    eval_limits();
+
+    // We are dealing with a mini pillar that's potentially too long
+    if (radius < sm.cfg.head_back_radius_mm && jp.z() - gndlvl > 20 * radius)
+    {
+        std::optional<DiffBridge> diffbr =
+            search_widening_path(policy, sm, jp, dir, radius,
+                                 sm.cfg.head_back_radius_mm);
+
+        if (diffbr && diffbr->endp.z() > jp_gnd) {
+//            auto &br = builder.add_diffbridge(*diffbr);
+//            if (head_id >= 0)
+//                builder.head(head_id).bridge_id = br.id;
+            ret.path.emplace_back(source);
+            endp = diffbr->endp;
+            radius = diffbr->end_r;
+//            builder.add_junction(endp, radius);
+            ret.path.emplace_back(endp, radius);
+//            non_head = true;
+            dir = diffbr->get_dir();
+            eval_limits();
+        } else return ret;//return {false, pillar_id};
+    }
+
+    if (sm.cfg.object_elevation_mm < EPSILON)
+    {
+        // get a suitable direction for the corrector bridge. It is the
+        // original sourcedir's azimuth but the polar angle is saturated to the
+        // configured bridge slope.
+        auto [polar, azimuth] = dir_to_spheric(dir);
+        polar = PI - sm.cfg.bridge_slope;
+        Vec3d d = spheric_to_dir(polar, azimuth).normalized();
+        auto sd = radius * sm.cfg.safety_distance_mm / sm.cfg.head_back_radius_mm;
+        double t = beam_mesh_hit(policy, sm.emesh, Beam{endp, d, radius, r2}, sd).distance();
+        double tmax = std::min(sm.cfg.max_bridge_length_mm, t);
+        t = 0.;
+
+        double zd = endp.z() - jp_gnd;
+        double tmax2 = zd / std::sqrt(1 - sm.cfg.bridge_slope * sm.cfg.bridge_slope);
+        tmax = std::min(tmax, tmax2);
+
+        Vec3d nexp = endp;
+        double dlast = 0.;
+        while (((dlast = std::sqrt(sm.emesh.squared_distance(to_floor(nexp)))) < gap_dist ||
+                !std::isinf(beam_mesh_hit(policy, sm.emesh, Beam{nexp, DOWN, radius, r2}, sd).distance())) &&
+               t < tmax)
+        {
+            t += radius;
+            nexp = endp + t * d;
+        }
+
+        if (dlast < gap_dist && can_add_base) {
+            nexp         = endp;
+            t            = 0.;
+            can_add_base = false;
+            eval_limits(can_add_base);
+
+            zd = endp.z() - jp_gnd;
+            tmax2 = zd / std::sqrt(1 - sm.cfg.bridge_slope * sm.cfg.bridge_slope);
+            tmax = std::min(tmax, tmax2);
+
+            while (((dlast = std::sqrt(sm.emesh.squared_distance(to_floor(nexp)))) < gap_dist ||
+                    !std::isinf(beam_mesh_hit(policy, sm.emesh, Beam{nexp, DOWN, radius}, sd).distance())) && t < tmax) {
+                t += radius;
+                nexp = endp + t * d;
+            }
+        }
+
+        // Could not find a path to avoid the pad gap
+        if (dlast < gap_dist) {
+            ret.path.clear();
+            return ret;
+            //return {false, pillar_id};
+        }
+
+        if (t > 0.) { // Need to make additional bridge
+//            const Bridge& br = builder.add_bridge(endp, nexp, radius);
+//            if (head_id >= 0)
+//                builder.head(head_id).bridge_id = br.id;
+
+//            builder.add_junction(nexp, radius);
+            ret.path.emplace_back(nexp, radius);
+            endp = nexp;
+//            non_head = true;
+        }
+    }
+
+    Vec3d gp = to_floor(endp);
+    double h = endp.z() - gp.z();
+
+//    pillar_id = head_id >= 0 && !non_head ? builder.add_pillar(head_id, h) :
+//                                            builder.add_pillar(gp, h, radius, end_radius);
+    ret.end_radius = end_radius;
+
+    if (can_add_base) {
+        ret.pillar_base = Pedestal{gp, h, sm.cfg.base_height_mm, sm.cfg.base_radius_mm};
+//        builder.add_pillar_base(pillar_id, sm.cfg.base_height_mm,
+//                                sm.cfg.base_radius_mm);
+    }
+
+    return ret; //{true, pillar_id};
+}
+
+inline long build_ground_connection(SupportTreeBuilder &builder,
+                                    const SupportableMesh &sm,
+                                    const GroundConnection &conn)
+{
+    long ret = SupportTreeNode::ID_UNSET;
+
+    if (!conn)
+        return ret;
+
+    auto it = conn.path.begin();
+    auto itnx = std::next(it);
+
+    while (itnx != conn.path.end()) {
+        builder.add_diffbridge(*it, *itnx);
+    }
+
+    auto gp = conn.path.back().pos;
+    gp.z() = ground_level(sm);
+    double h = conn.path.back().pos.z() - gp.z();
+    ret = builder.add_pillar(gp, h, conn.path.back().r, conn.end_radius);
+    if (conn.pillar_base)
+        builder.add_pillar_base(ret, conn.pillar_base->height, conn.pillar_base->r_bottom);
+
+    return ret;
+}
+
+template<class Ex>
+GroundConnection find_ground_connection(
     Ex                     policy,
     SupportTreeBuilder    &builder,
     const SupportableMesh &sm,
@@ -662,16 +841,23 @@ std::pair<bool, std::optional<Junction>> find_ground_connection(
         d += r;
     }
 
-    std::pair<bool, std::optional<Junction>> ret;
+    GroundConnection ret;
+    ret.end_radius = end_r;
 
     if (std::isinf(tdown)) {
-        ret.first = true;
+        ret.path.emplace_back(j);
         if (d > 0) {
             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);
 
-            ret.second = Junction{endp, pill_r};
+//            ret.path.emplace_back(endp, pill_r);
+            auto route = find_pillar_route(policy, sm, {endp, pill_r}, dir, end_r);
+            for (auto &j : route.path)
+                ret.path.emplace_back(j);
+
+            ret.pillar_base = route.pillar_base;
+            ret.end_radius  = end_r;
         }
     }
 
@@ -679,7 +865,7 @@ std::pair<bool, std::optional<Junction>> find_ground_connection(
 }
 
 template<class Ex>
-std::pair<bool, std::optional<Junction>> optimize_ground_connection(
+GroundConnection optimize_ground_connection(
     Ex                     policy,
     SupportTreeBuilder    &builder,
     const SupportableMesh &sm,
@@ -690,7 +876,7 @@ std::pair<bool, std::optional<Junction>> optimize_ground_connection(
     double downdst = j.pos.z() - ground_level(sm);
 
     auto res = find_ground_connection(policy, builder, sm, j, init_dir, end_radius);
-    if (res.first)
+    if (!res)
         return res;
 
     // Optimize bridge direction:
@@ -728,18 +914,9 @@ std::pair<bool, long> connect_to_ground(Ex                     policy,
 {
     std::pair<bool, long> ret = {false, SupportTreeNode::ID_UNSET};
 
-    auto [found_c, cjunc] = find_ground_connection(policy, builder, sm, j, dir, end_r);
+    auto conn = find_ground_connection(policy, builder, sm, j, dir, end_r);
-
+    ret.first = bool(conn);
-    if (found_c) {
+    ret.second = build_ground_connection(builder, sm, conn);
-        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;
 }
@@ -754,18 +931,11 @@ std::pair<bool, long> search_ground_route(Ex                     policy,
 {
     std::pair<bool, long> ret = {false, SupportTreeNode::ID_UNSET};
 
-    auto [found_c, cjunc] = optimize_ground_connection(policy, builder, sm, j, end_r, init_dir);
+    auto conn = optimize_ground_connection(policy, builder, sm, j,
-    if (found_c) {
+                                                       end_r, init_dir);
-        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) {
+    ret.first = bool(conn);
-            builder.add_diffbridge(j.pos, endp, j.r, R);
+    ret.second = build_ground_connection(builder, sm, conn);
-            builder.add_junction(endp, R);
-        }
-    }
 
     return ret;
 }