Fix dangling pinheads
This commit is contained in:
parent
2cb74013be
commit
064e9935d1
2 changed files with 58 additions and 22 deletions
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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()
|
||||
|
|
Loading…
Reference in a new issue