From 128db4b9381e85a45d9359a6ba263d8dc72460bf Mon Sep 17 00:00:00 2001
From: tamasmeszaros <meszaros.q@gmail.com>
Date: Mon, 2 Jan 2023 17:48:41 +0100
Subject: [PATCH] Still WIP on branching tree avoidance

---
 src/libslic3r/BranchingTree/BranchingTree.cpp | 24 ++++++--
 src/libslic3r/SLA/BranchingTreeSLA.cpp        | 55 ++++++++++++++++---
 2 files changed, 64 insertions(+), 15 deletions(-)

diff --git a/src/libslic3r/BranchingTree/BranchingTree.cpp b/src/libslic3r/BranchingTree/BranchingTree.cpp
index 8f1d322a1..01f4b2724 100644
--- a/src/libslic3r/BranchingTree/BranchingTree.cpp
+++ b/src/libslic3r/BranchingTree/BranchingTree.cpp
@@ -76,16 +76,28 @@ void build_tree(PointCloud &nodes, Builder &builder)
             switch (type) {
             case BED: {
                 closest_node.weight = w;
-                if (closest_it->dst_branching > nodes.properties().max_branch_length()) {
-                    auto hl_br_len = float(nodes.properties().max_branch_length()) / 2.f;
-                    Node new_node {{node.pos.x(), node.pos.y(), node.pos.z() - hl_br_len}, node.Rmin};
-                    new_node.id = int(nodes.next_junction_id());
-                    new_node.weight = nodes.get(node_id).weight + hl_br_len;
-                    new_node.left = node.id;
+                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).squaredNorm();
+                    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);
                     }
+//                    auto hl_br_len = float(nodes.properties().max_branch_length()) / 2.f;
+//                    Node new_node {{node.pos.x(), node.pos.y(), node.pos.z() - hl_br_len}, node.Rmin};
+//                    new_node.id = int(nodes.next_junction_id());
+//                    new_node.weight = nodes.get(node_id).weight + hl_br_len;
+//                    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;
diff --git a/src/libslic3r/SLA/BranchingTreeSLA.cpp b/src/libslic3r/SLA/BranchingTreeSLA.cpp
index d4778a0ac..7f4b9853c 100644
--- a/src/libslic3r/SLA/BranchingTreeSLA.cpp
+++ b/src/libslic3r/SLA/BranchingTreeSLA.cpp
@@ -146,7 +146,7 @@ public:
                          const branchingtree::Node &to) override;
 
     std::optional<Vec3f> suggest_avoidance(const branchingtree::Node &from,
-                                           float max_bridge_len) override;;
+                                           float max_bridge_len) override;
 
     void report_unroutable(const branchingtree::Node &j) override
     {
@@ -293,27 +293,64 @@ bool BranchingTreeBuilder::add_mesh_bridge(const branchingtree::Node &from,
 static std::optional<Vec3f> get_avoidance(const GroundConnection &conn,
                                           float maxdist)
 {
-    return {};
+    std::optional<Vec3f> ret;
+
+    if (conn) {
+        if (conn.path.size() > 1) {
+            ret = conn.path[1].pos.cast<float>();
+        } else {
+            Vec3f pbeg = conn.path[0].pos.cast<float>();
+            Vec3f pend = conn.pillar_base->pos.cast<float>();
+            pbeg.z()   = std::max(pbeg.z() - maxdist, pend.z());
+            ret = pbeg;
+        }
+    }
+
+    return ret;
 }
 
 std::optional<Vec3f> BranchingTreeBuilder::suggest_avoidance(
     const branchingtree::Node &from, float max_bridge_len)
 {
+    std::optional<Vec3f> ret;
+
     double glvl = ground_level(m_sm);
     branchingtree::Node dst = from;
     dst.pos.z() = glvl;
     dst.weight += from.pos.z() - glvl;
-    bool succ = add_ground_bridge(from, dst);
+    sla::Junction j{from.pos.cast<double>(), get_radius(from)};
 
-    std::optional<Vec3f> ret;
+//    auto found_it = m_ground_mem.find(from.id);
+//    if (found_it != m_ground_mem.end()) {
+//        // TODO look up the conn object
+//    }
+//    else if (auto conn = deepsearch_ground_connection(
+//                   beam_ex_policy , m_sm, j, get_radius(dst), sla::DOWN)) {
+//        ret = get_avoidance(conn, max_bridge_len);
+//    }
 
-    if (succ) {
-        auto it = m_gnd_connections.find(from.id);
-        if (it != m_gnd_connections.end())
-            ret = get_avoidance(it->second, max_bridge_len);
-    }
+    auto conn = deepsearch_ground_connection(
+        beam_ex_policy , m_sm, j, get_radius(dst), sla::DOWN);
+
+    ret = get_avoidance(conn, max_bridge_len);
 
     return ret;
+
+//    double glvl = ground_level(m_sm);
+//    branchingtree::Node dst = from;
+//    dst.pos.z() = glvl;
+//    dst.weight += from.pos.z() - glvl;
+//    bool succ = add_ground_bridge(from, dst);
+
+//    std::optional<Vec3f> ret;
+
+//    if (succ) {
+//        auto it = m_gnd_connections.find(m_pillars.size() - 1);
+//        if (it != m_gnd_connections.end())
+//            ret = get_avoidance(it->second, max_bridge_len);
+//    }
+
+//    return ret;
 }
 
 inline void build_pillars(SupportTreeBuilder &builder,