Fix dangling pinheads

This commit is contained in:
tamasmeszaros 2022-06-13 18:20:00 +02:00
parent 2cb74013be
commit 064e9935d1
2 changed files with 58 additions and 22 deletions

View file

@ -16,13 +16,23 @@ void build_tree(PointCloud &nodes, Builder &builder)
auto ptsqueue = nodes.start_queue();
auto &properties = nodes.properties();
struct NodeDistance { size_t node_id = Node::ID_NONE; float distance = NaNf; };
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()) {
size_t node_id = ptsqueue.top();
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);
@ -30,44 +40,47 @@ void build_tree(PointCloud &nodes, Builder &builder)
distances.clear();
distances.reserve(K);
float dmax = 0.;
nodes.foreach_reachable(
node.pos,
[&distances](size_t id, float distance) {
distances.emplace_back(NodeDistance{id, distance});
[&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.distance < b.distance; });
[](auto &a, auto &b) { return a.dst_branching < b.dst_branching; });
if (distances.empty()) {
builder.report_unroutable(node);
ptsqueue.pop();
K = initK;
prev_dist_max = 0.;
routed = true;
continue;
}
prev_dist_max = distances.back().distance;
prev_dist_max = dmax;
K *= 2;
auto closest_it = distances.begin();
bool routed = false;
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->distance;
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;
if (closest_it->distance > nodes.properties().max_branch_length()) {
auto hl_br_len = float(nodes.properties().max_branch_length()) / 2.f;
if (closest_it->dst_branching > nodes.properties().max_branch_length()) {
auto hl_br_len = float(nodes.properties().max_branch_length()) / 2.;
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 + nodes.properties().max_branch_length();
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);
@ -147,11 +160,9 @@ void build_tree(PointCloud &nodes, Builder &builder)
}
if (routed) {
ptsqueue.pop();
prev_dist_max = 0.;
K = initK;
}
}
}

View file

@ -29,6 +29,19 @@ std::vector<Node> sample_bed(const ExPolygons &bed,
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();
double r = std::min(h * std::tan(phi), props.max_branch_length() * std::sin(phi));
Vec3f bb_min = {p.x() - r, p.y() - r, 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.
@ -199,20 +212,32 @@ public:
Output& operator *() { return *this; }
void operator=(const PointIndexEl &el) {
visitorfn(el.second, self->get_distance(p, el.second));
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,
m_props.ground_level() - EPSILON},
{pos.x() + brln, pos.y() + brln,
m_ktree.bounds().max_corner().get<Z>()}};
auto filter = bgi::satisfies(
[this, &pos, min_dist](const PointIndexEl &e) {
double d = get_distance(pos, e.second);
return m_searchable_indices[e.second] && !isinf(d) && d > min_dist;
});
// Extend upwards to find mergable junctions and support points
bb.max.z() = m_ktree.bounds().max_corner().get<Z>();
m_ktree.query(bgi::nearest(pos, k) && filter, Output{this, pos, visitor});
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()