Prevent merging with node being lower than closest point
This commit is contained in:
parent
91c0741f61
commit
688c9c644f
@ -96,7 +96,7 @@ bool build_tree(PointCloud &nodes, Builder &builder)
|
|||||||
float distsum = std::max(mergedist_closest, mergedist_node);
|
float distsum = std::max(mergedist_closest, mergedist_node);
|
||||||
w = Wsum + distsum;
|
w = Wsum + distsum;
|
||||||
|
|
||||||
if (mergedist_closest > EPSILON) {
|
if (mergedist_closest > EPSILON && mergedist_node > EPSILON) {
|
||||||
Node mergenode{*mergept, closest_node.Rmin};
|
Node mergenode{*mergept, closest_node.Rmin};
|
||||||
mergenode.weight = w;
|
mergenode.weight = w;
|
||||||
mergenode.id = int(nodes.next_junction_id());
|
mergenode.id = int(nodes.next_junction_id());
|
||||||
@ -109,9 +109,9 @@ bool build_tree(PointCloud &nodes, Builder &builder)
|
|||||||
ptsqueue.remove(nodes.get_queue_idx(closest_node_id));
|
ptsqueue.remove(nodes.get_queue_idx(closest_node_id));
|
||||||
nodes.mark_unreachable(closest_node_id);
|
nodes.mark_unreachable(closest_node_id);
|
||||||
}
|
}
|
||||||
} else if (closest_node.left == Node::ID_NONE ||
|
} else if (closest_node.pos.z() < node.pos.z() &&
|
||||||
closest_node.right == Node::ID_NONE)
|
(closest_node.left == Node::ID_NONE ||
|
||||||
{
|
closest_node.right == Node::ID_NONE)) {
|
||||||
closest_node.weight = w;
|
closest_node.weight = w;
|
||||||
if ((routed = builder.add_bridge(node, closest_node))) {
|
if ((routed = builder.add_bridge(node, closest_node))) {
|
||||||
if (closest_node.left == Node::ID_NONE)
|
if (closest_node.left == Node::ID_NONE)
|
||||||
|
Loading…
Reference in New Issue
Block a user