Still WIP on branching tree avoidance
This commit is contained in:
parent
234167534b
commit
128db4b938
@ -76,16 +76,28 @@ void build_tree(PointCloud &nodes, Builder &builder)
|
|||||||
switch (type) {
|
switch (type) {
|
||||||
case BED: {
|
case BED: {
|
||||||
closest_node.weight = w;
|
closest_node.weight = w;
|
||||||
if (closest_it->dst_branching > nodes.properties().max_branch_length()) {
|
double max_br_len = nodes.properties().max_branch_length();
|
||||||
auto hl_br_len = float(nodes.properties().max_branch_length()) / 2.f;
|
if (closest_it->dst_branching > max_br_len) {
|
||||||
Node new_node {{node.pos.x(), node.pos.y(), node.pos.z() - hl_br_len}, node.Rmin};
|
std::optional<Vec3f> avo = builder.suggest_avoidance(node, max_br_len);
|
||||||
new_node.id = int(nodes.next_junction_id());
|
if (!avo)
|
||||||
new_node.weight = nodes.get(node_id).weight + hl_br_len;
|
break;
|
||||||
new_node.left = node.id;
|
|
||||||
|
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))) {
|
if ((routed = builder.add_bridge(node, new_node))) {
|
||||||
size_t new_idx = nodes.insert_junction(new_node);
|
size_t new_idx = nodes.insert_junction(new_node);
|
||||||
ptsqueue.push(new_idx);
|
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))) {
|
} else if ((routed = builder.add_ground_bridge(node, closest_node))) {
|
||||||
closest_node.left = closest_node.right = node_id;
|
closest_node.left = closest_node.right = node_id;
|
||||||
nodes.get(closest_node_id) = closest_node;
|
nodes.get(closest_node_id) = closest_node;
|
||||||
|
@ -146,7 +146,7 @@ public:
|
|||||||
const branchingtree::Node &to) override;
|
const branchingtree::Node &to) override;
|
||||||
|
|
||||||
std::optional<Vec3f> suggest_avoidance(const branchingtree::Node &from,
|
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
|
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,
|
static std::optional<Vec3f> get_avoidance(const GroundConnection &conn,
|
||||||
float maxdist)
|
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(
|
std::optional<Vec3f> BranchingTreeBuilder::suggest_avoidance(
|
||||||
const branchingtree::Node &from, float max_bridge_len)
|
const branchingtree::Node &from, float max_bridge_len)
|
||||||
{
|
{
|
||||||
|
std::optional<Vec3f> ret;
|
||||||
|
|
||||||
double glvl = ground_level(m_sm);
|
double glvl = ground_level(m_sm);
|
||||||
branchingtree::Node dst = from;
|
branchingtree::Node dst = from;
|
||||||
dst.pos.z() = glvl;
|
dst.pos.z() = glvl;
|
||||||
dst.weight += from.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 conn = deepsearch_ground_connection(
|
||||||
auto it = m_gnd_connections.find(from.id);
|
beam_ex_policy , m_sm, j, get_radius(dst), sla::DOWN);
|
||||||
if (it != m_gnd_connections.end())
|
|
||||||
ret = get_avoidance(it->second, max_bridge_len);
|
ret = get_avoidance(conn, max_bridge_len);
|
||||||
}
|
|
||||||
|
|
||||||
return ret;
|
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,
|
inline void build_pillars(SupportTreeBuilder &builder,
|
||||||
|
Loading…
Reference in New Issue
Block a user