Improvements to widening strategy and branch length limitations
Also some refactoring
This commit is contained in:
parent
ae23adff10
commit
fd8fd77077
@ -22,7 +22,7 @@ bool build_tree(PointCloud &nodes, Builder &builder)
|
||||
ptsqueue.pop();
|
||||
|
||||
Node node = nodes.get(node_id);
|
||||
nodes.remove_node(node_id);
|
||||
nodes.mark_unreachable(node_id);
|
||||
|
||||
distances.clear();
|
||||
distances.reserve(nodes.reachable_count());
|
||||
@ -53,10 +53,21 @@ bool build_tree(PointCloud &nodes, Builder &builder)
|
||||
switch (type) {
|
||||
case BED: {
|
||||
closest_node.weight = w;
|
||||
if ((routed = builder.add_ground_bridge(node, closest_node))) {
|
||||
if (closest_it->distance > 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 + nodes.properties().max_branch_length();
|
||||
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;
|
||||
nodes.remove_node(closest_node_id);
|
||||
nodes.mark_unreachable(closest_node_id);
|
||||
}
|
||||
|
||||
break;
|
||||
@ -66,12 +77,12 @@ bool build_tree(PointCloud &nodes, Builder &builder)
|
||||
if ((routed = builder.add_mesh_bridge(node, closest_node))) {
|
||||
closest_node.left = closest_node.right = node_id;
|
||||
nodes.get(closest_node_id) = closest_node;
|
||||
nodes.remove_node(closest_node_id);
|
||||
nodes.mark_unreachable(closest_node_id);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case SUPP:
|
||||
case LEAF:
|
||||
case JUNCTION: {
|
||||
auto max_slope = float(properties.max_slope());
|
||||
|
||||
@ -96,7 +107,7 @@ bool build_tree(PointCloud &nodes, Builder &builder)
|
||||
size_t new_idx = nodes.insert_junction(mergenode);
|
||||
ptsqueue.push(new_idx);
|
||||
ptsqueue.remove(nodes.get_queue_idx(closest_node_id));
|
||||
nodes.remove_node(closest_node_id);
|
||||
nodes.mark_unreachable(closest_node_id);
|
||||
}
|
||||
} else if (closest_node.left == Node::ID_NONE ||
|
||||
closest_node.right == Node::ID_NONE)
|
||||
@ -138,4 +149,19 @@ bool build_tree(const indexed_triangle_set & its,
|
||||
return build_tree(nodes, builder);
|
||||
}
|
||||
|
||||
ExPolygon make_bed_poly(const indexed_triangle_set &its)
|
||||
{
|
||||
auto bb = bounding_box(its);
|
||||
|
||||
BoundingBox bbcrd{scaled(to_2d(bb.min)), scaled(to_2d(bb.max))};
|
||||
bbcrd.offset(scaled(10.));
|
||||
Point min = bbcrd.min, max = bbcrd.max;
|
||||
ExPolygon ret = {{min.x(), min.y()},
|
||||
{max.x(), min.y()},
|
||||
{max.x(), max.y()},
|
||||
{min.x(), max.y()}};
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
}} // namespace Slic3r::branchingtree
|
||||
|
@ -16,7 +16,7 @@ class Properties
|
||||
double m_max_slope = PI / 4.;
|
||||
double m_ground_level = 0.;
|
||||
double m_sampling_radius = .5;
|
||||
double m_max_branch_len = 20.;
|
||||
double m_max_branch_len = 10.;
|
||||
|
||||
ExPolygons m_bed_shape;
|
||||
|
||||
@ -67,11 +67,11 @@ struct Node
|
||||
int id = ID_NONE, left = ID_NONE, right = ID_NONE;
|
||||
|
||||
Vec3f pos;
|
||||
float Rmin;
|
||||
float Rmin = 0.f;
|
||||
|
||||
// Tracking the weight of each junction, which is essentially the sum of
|
||||
// the lenghts of all branches emanating from this junction.
|
||||
float weight;
|
||||
float weight = 0.f;
|
||||
|
||||
Node(const Vec3f &p, float r_min = .0f) : pos{p}, Rmin{r_min}, weight{0.f}
|
||||
{}
|
||||
@ -137,20 +137,7 @@ inline bool build_tree(const indexed_triangle_set & its,
|
||||
//bool build_tree(PointCloud &pcloud, Builder &builder);
|
||||
|
||||
// Helper function to derive a bed polygon only from the model bounding box.
|
||||
inline ExPolygon make_bed_poly(const indexed_triangle_set &its)
|
||||
{
|
||||
auto bb = bounding_box(its);
|
||||
|
||||
BoundingBox bbcrd{scaled(to_2d(bb.min)), scaled(to_2d(bb.max))};
|
||||
bbcrd.offset(scaled(10.));
|
||||
Point min = bbcrd.min, max = bbcrd.max;
|
||||
ExPolygon ret = {{min.x(), min.y()},
|
||||
{max.x(), min.y()},
|
||||
{max.x(), max.y()},
|
||||
{min.x(), max.y()}};
|
||||
|
||||
return ret;
|
||||
}
|
||||
ExPolygon make_bed_poly(const indexed_triangle_set &its);
|
||||
|
||||
}} // namespace Slic3r::branchingtree
|
||||
|
||||
|
@ -109,72 +109,82 @@ std::vector<Node> sample_bed(const ExPolygons &bed, float z, double radius)
|
||||
}
|
||||
|
||||
PointCloud::PointCloud(const indexed_triangle_set &M,
|
||||
std::vector<Node> support_leafs,
|
||||
std::vector<Node> support_leafs,
|
||||
const Properties &props)
|
||||
: PointCloud{sample_mesh(M, props.sampling_radius()),
|
||||
sample_bed(props.bed_shape(),
|
||||
props.ground_level(),
|
||||
props.sampling_radius()),
|
||||
std::move(support_leafs), props}
|
||||
{}
|
||||
|
||||
PointCloud::PointCloud(std::vector<Node> meshpts,
|
||||
std::vector<Node> bedpts,
|
||||
std::vector<Node> support_leafs,
|
||||
const Properties &props)
|
||||
: m_leafs{std::move(support_leafs)}
|
||||
, m_meshpoints{sample_mesh(M, props.sampling_radius())}
|
||||
, m_bedpoints{sample_bed(props.bed_shape(),
|
||||
props.ground_level(),
|
||||
props.sampling_radius())}
|
||||
, m_meshpoints{std::move(meshpts)}
|
||||
, m_bedpoints{std::move(bedpts)}
|
||||
, m_props{props}
|
||||
, cos2bridge_slope{std::cos(props.max_slope()) *
|
||||
std::abs(std::cos(props.max_slope()))}
|
||||
, MESHPTS_BEGIN{m_bedpoints.size()}
|
||||
, SUPP_BEGIN{MESHPTS_BEGIN + m_meshpoints.size()}
|
||||
, JUNCTIONS_BEGIN{SUPP_BEGIN + m_leafs.size()}
|
||||
, LEAFS_BEGIN{MESHPTS_BEGIN + m_meshpoints.size()}
|
||||
, JUNCTIONS_BEGIN{LEAFS_BEGIN + m_leafs.size()}
|
||||
, m_searchable_indices(JUNCTIONS_BEGIN, true)
|
||||
, m_queue_indices(JUNCTIONS_BEGIN, UNQUEUED)
|
||||
, m_reachable_cnt{JUNCTIONS_BEGIN}
|
||||
, m_ktree{CoordFn{this}, SUPP_BEGIN} // Only for bed and mesh points
|
||||
, m_ktree{CoordFn{this}, LEAFS_BEGIN} // Only for bed and mesh points
|
||||
{
|
||||
for (size_t i = 0; i < m_bedpoints.size(); ++i)
|
||||
m_bedpoints[i].id = int(i);
|
||||
|
||||
|
||||
for (size_t i = 0; i < m_meshpoints.size(); ++i)
|
||||
m_meshpoints[i].id = int(MESHPTS_BEGIN + i);
|
||||
|
||||
|
||||
for (size_t i = 0; i < m_leafs.size(); ++i)
|
||||
m_leafs[i].id = int(SUPP_BEGIN + i);
|
||||
m_leafs[i].id = int(LEAFS_BEGIN + i);
|
||||
}
|
||||
|
||||
float PointCloud::get_distance(const Vec3f &p, size_t node)
|
||||
float PointCloud::get_distance(const Vec3f &p, size_t node_id) const
|
||||
{
|
||||
auto t = get_type(node);
|
||||
auto t = get_type(node_id);
|
||||
auto ret = std::numeric_limits<float>::infinity();
|
||||
|
||||
const auto &node = get(node_id);
|
||||
|
||||
switch (t) {
|
||||
case MESH:
|
||||
case BED: {
|
||||
// Points of mesh or bed which are outside of the support cone of
|
||||
// 'pos' must be discarded.
|
||||
if (is_outside_support_cone(p, get(node).pos))
|
||||
if (is_outside_support_cone(p, node.pos))
|
||||
ret = std::numeric_limits<float>::infinity();
|
||||
else
|
||||
ret = (get(node).pos - p).norm();
|
||||
|
||||
ret = (node.pos - p).norm();
|
||||
|
||||
break;
|
||||
}
|
||||
case SUPP:
|
||||
case LEAF:
|
||||
case JUNCTION:{
|
||||
auto mergept = find_merge_pt(p, get(node).pos, m_props.max_slope());
|
||||
if (!mergept || mergept->z() < (m_props.ground_level() + 2 * get(node).Rmin))
|
||||
auto mergept = find_merge_pt(p, node.pos, m_props.max_slope());
|
||||
if (!mergept || mergept->z() < (m_props.ground_level() + 2 * node.Rmin))
|
||||
ret = std::numeric_limits<float>::infinity();
|
||||
else
|
||||
else if ( (node.pos - *mergept).norm() < m_props.max_branch_length())
|
||||
ret = (p - *mergept).norm();
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
case NONE:
|
||||
;
|
||||
}
|
||||
|
||||
// Setting the ret val to infinity will effectively discard this
|
||||
// connection of nodes. max_branch_length property if used here
|
||||
// to discard node=>node and node=>mesh connections longer than this
|
||||
// property.
|
||||
|
||||
// Setting the ret val to infinity will effectively discard this
|
||||
// connection of nodes. max_branch_length property if used here
|
||||
// to discard node=>node and node=>mesh connections longer than this
|
||||
// property.
|
||||
if (t != BED && ret > m_props.max_branch_length())
|
||||
ret = std::numeric_limits<float>::infinity();
|
||||
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
@ -1,8 +1,11 @@
|
||||
#ifndef POINTCLOUD_HPP
|
||||
#define POINTCLOUD_HPP
|
||||
|
||||
#include <optional>
|
||||
|
||||
#include "BranchingTree.hpp"
|
||||
|
||||
#include "libslic3r/Execution/Execution.hpp"
|
||||
#include "libslic3r/KDTreeIndirect.hpp"
|
||||
#include "libslic3r/MutablePriorityQueue.hpp"
|
||||
|
||||
@ -16,14 +19,13 @@ void to_eigen_mesh(const indexed_triangle_set &its,
|
||||
Eigen::MatrixXd &V,
|
||||
Eigen::MatrixXi &F);
|
||||
|
||||
std::vector<Node> sample_mesh(const indexed_triangle_set &its,
|
||||
double radius);
|
||||
std::vector<Node> sample_mesh(const indexed_triangle_set &its, double radius);
|
||||
|
||||
std::vector<Node> sample_bed(const ExPolygons &bed,
|
||||
float z,
|
||||
double radius = 1.);
|
||||
|
||||
enum PtType { SUPP, MESH, BED, JUNCTION, NONE };
|
||||
enum PtType { LEAF, MESH, BED, JUNCTION, NONE };
|
||||
|
||||
// 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
|
||||
@ -34,7 +36,7 @@ class PointCloud {
|
||||
const branchingtree::Properties &m_props;
|
||||
|
||||
const double cos2bridge_slope;
|
||||
const size_t MESHPTS_BEGIN, SUPP_BEGIN, JUNCTIONS_BEGIN;
|
||||
const size_t MESHPTS_BEGIN, LEAFS_BEGIN, JUNCTIONS_BEGIN;
|
||||
|
||||
private:
|
||||
|
||||
@ -57,7 +59,7 @@ private:
|
||||
|
||||
KDTreeIndirect<3, float, CoordFn> m_ktree;
|
||||
|
||||
bool is_outside_support_cone(const Vec3f &supp, const Vec3f &pt)
|
||||
bool is_outside_support_cone(const Vec3f &supp, const Vec3f &pt) const
|
||||
{
|
||||
Vec3d D = (pt - supp).cast<double>();
|
||||
double dot_sq = -D.z() * std::abs(-D.z());
|
||||
@ -75,7 +77,7 @@ private:
|
||||
switch(pc.get_type(id)) {
|
||||
case BED: ret = &pc.m_bedpoints[id]; break;
|
||||
case MESH: ret = &pc.m_meshpoints[id - pc.MESHPTS_BEGIN]; break;
|
||||
case SUPP: ret = &pc.m_leafs [id - pc.SUPP_BEGIN]; break;
|
||||
case LEAF: ret = &pc.m_leafs [id - pc.LEAFS_BEGIN]; break;
|
||||
case JUNCTION: ret = &pc.m_junctions[id - pc.JUNCTIONS_BEGIN]; break;
|
||||
case NONE: assert(false);
|
||||
}
|
||||
@ -99,13 +101,18 @@ public:
|
||||
std::vector<Node> support_leafs,
|
||||
const Properties & props);
|
||||
|
||||
PointCloud(std::vector<Node> meshpts,
|
||||
std::vector<Node> bedpts,
|
||||
std::vector<Node> support_leafs,
|
||||
const Properties &props);
|
||||
|
||||
PtType get_type(size_t node_id) const
|
||||
{
|
||||
PtType ret = NONE;
|
||||
|
||||
if (node_id < MESHPTS_BEGIN && !m_bedpoints.empty()) ret = BED;
|
||||
else if (node_id < SUPP_BEGIN && !m_meshpoints.empty()) ret = MESH;
|
||||
else if (node_id < JUNCTIONS_BEGIN && !m_leafs.empty()) ret = SUPP;
|
||||
else if (node_id < LEAFS_BEGIN && !m_meshpoints.empty()) ret = MESH;
|
||||
else if (node_id < JUNCTIONS_BEGIN && !m_leafs.empty()) ret = LEAF;
|
||||
else if (node_id >= JUNCTIONS_BEGIN && !m_junctions.empty()) ret = JUNCTION;
|
||||
|
||||
return ret;
|
||||
@ -128,14 +135,14 @@ public:
|
||||
// node id is indeed of type SUPP
|
||||
int get_leaf_id(size_t node_id) const
|
||||
{
|
||||
return node_id >= SUPP_BEGIN && node_id < JUNCTIONS_BEGIN ?
|
||||
node_id - SUPP_BEGIN :
|
||||
return node_id >= LEAFS_BEGIN && node_id < JUNCTIONS_BEGIN ?
|
||||
node_id - LEAFS_BEGIN :
|
||||
Node::ID_NONE;
|
||||
}
|
||||
|
||||
size_t get_queue_idx(size_t node_id) const { return m_queue_indices[node_id]; }
|
||||
|
||||
float get_distance(const Vec3f &p, size_t node);
|
||||
float get_distance(const Vec3f &p, size_t node) const;
|
||||
|
||||
size_t next_junction_id() const
|
||||
{
|
||||
@ -154,7 +161,13 @@ public:
|
||||
return new_id;
|
||||
}
|
||||
|
||||
void remove_node(size_t node_id)
|
||||
const std::vector<Node> &get_junctions() const noexcept { return m_junctions; }
|
||||
const std::vector<Node> &get_bedpoints() const noexcept { return m_bedpoints; }
|
||||
const std::vector<Node> &get_meshpoints() const noexcept { return m_meshpoints; }
|
||||
const std::vector<Node> &get_leafs() const noexcept { return m_leafs; }
|
||||
const Properties & properties() const noexcept { return m_props; }
|
||||
|
||||
void mark_unreachable(size_t node_id)
|
||||
{
|
||||
m_searchable_indices[node_id] = false;
|
||||
m_queue_indices[node_id] = UNQUEUED;
|
||||
@ -175,7 +188,7 @@ public:
|
||||
if (anchor != m_ktree.npos)
|
||||
visitor(anchor, get_distance(pos, anchor));
|
||||
|
||||
for (size_t i = SUPP_BEGIN; i < m_searchable_indices.size(); ++i)
|
||||
for (size_t i = LEAFS_BEGIN; i < m_searchable_indices.size(); ++i)
|
||||
if (m_searchable_indices[i])
|
||||
visitor(i, get_distance(pos, i));
|
||||
}
|
||||
@ -187,14 +200,12 @@ public:
|
||||
ZCompareFn{this});
|
||||
|
||||
ptsqueue.reserve(m_leafs.size());
|
||||
size_t iend = SUPP_BEGIN + m_leafs.size();
|
||||
for (size_t i = SUPP_BEGIN; i < iend; ++i)
|
||||
size_t iend = LEAFS_BEGIN + m_leafs.size();
|
||||
for (size_t i = LEAFS_BEGIN; i < iend; ++i)
|
||||
ptsqueue.push(i);
|
||||
|
||||
return ptsqueue;
|
||||
}
|
||||
|
||||
const Properties & properties() const { return m_props; }
|
||||
};
|
||||
|
||||
template<class PC, class Fn> void traverse(PC &&pc, size_t root, Fn &&fn)
|
||||
|
@ -26,7 +26,7 @@ class BranchingTreeBuilder: public branchingtree::Builder {
|
||||
{
|
||||
double w = WIDENING_SCALE * m_sm.cfg.pillar_widening_factor * j.weight;
|
||||
|
||||
return std::max(double(j.Rmin), std::min(m_sm.cfg.base_radius_mm, w));
|
||||
return std::min(m_sm.cfg.base_radius_mm, double(j.Rmin) + w);
|
||||
}
|
||||
|
||||
std::vector<size_t> m_unroutable_pinheads;
|
||||
@ -51,10 +51,13 @@ class BranchingTreeBuilder: public branchingtree::Builder {
|
||||
} else if (int child = node.left + node.right + 1; child >= 0) {
|
||||
auto from = m_cloud.get(child);
|
||||
auto to = m_cloud.get(node.id);
|
||||
auto tod = to.pos.cast<double>();
|
||||
double toR = get_radius(to);
|
||||
m_builder.add_diffbridge(from.pos.cast<double>(),
|
||||
to.pos.cast<double>(),
|
||||
tod,
|
||||
get_radius(from),
|
||||
get_radius(to));
|
||||
toR);
|
||||
m_builder.add_junction(tod, toR);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user