247 lines
7.7 KiB
C++
247 lines
7.7 KiB
C++
#ifndef POINTCLOUD_HPP
|
|
#define POINTCLOUD_HPP
|
|
|
|
#include <optional>
|
|
|
|
#include "BranchingTree.hpp"
|
|
|
|
#include "libslic3r/Execution/Execution.hpp"
|
|
#include "libslic3r/MutablePriorityQueue.hpp"
|
|
|
|
#include "libslic3r/BoostAdapter.hpp"
|
|
#include "boost/geometry/index/rtree.hpp"
|
|
|
|
namespace Slic3r { namespace branchingtree {
|
|
|
|
std::optional<Vec3f> find_merge_pt(const Vec3f &A,
|
|
const Vec3f &B,
|
|
float max_slope);
|
|
|
|
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_bed(const ExPolygons &bed,
|
|
float z,
|
|
double radius = 1.);
|
|
|
|
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
|
|
// the other point types are established on creation and remain unchangeable.
|
|
class PointCloud {
|
|
std::vector<Node> m_leafs, m_junctions, m_meshpoints, m_bedpoints;
|
|
|
|
const branchingtree::Properties &m_props;
|
|
|
|
const double cos2bridge_slope;
|
|
const size_t MESHPTS_BEGIN, LEAFS_BEGIN, JUNCTIONS_BEGIN;
|
|
|
|
private:
|
|
|
|
// These vectors have the same size as there are indices for nodes to keep
|
|
// access complexity constant. WARN: there might be cache non-locality costs
|
|
std::vector<bool> m_searchable_indices; // searchable flag value of a node
|
|
std::vector<size_t> m_queue_indices; // queue id of a node if queued
|
|
|
|
size_t m_reachable_cnt;
|
|
|
|
struct CoordFn
|
|
{
|
|
const PointCloud *self;
|
|
CoordFn(const PointCloud *s) : self{s} {}
|
|
float operator()(size_t nodeid, size_t dim) const
|
|
{
|
|
return self->get(nodeid).pos(int(dim));
|
|
}
|
|
};
|
|
|
|
using PointIndexEl = std::pair<Vec3f, unsigned>;
|
|
|
|
boost::geometry::index::
|
|
rtree<PointIndexEl, boost::geometry::index::rstar<16, 4> /* ? */>
|
|
m_ktree;
|
|
|
|
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());
|
|
|
|
return dot_sq < D.squaredNorm() * cos2bridge_slope;
|
|
}
|
|
|
|
template<class PC>
|
|
static auto *get_node(PC &&pc, size_t id)
|
|
{
|
|
auto *ret = decltype(pc.m_bedpoints.data())(nullptr);
|
|
|
|
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 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);
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
public:
|
|
|
|
static constexpr auto Unqueued = size_t(-1);
|
|
|
|
struct ZCompareFn
|
|
{
|
|
const PointCloud *self;
|
|
ZCompareFn(const PointCloud *s) : self{s} {}
|
|
bool operator()(size_t node_a, size_t node_b) const
|
|
{
|
|
return self->get(node_a).pos.z() > self->get(node_b).pos.z();
|
|
}
|
|
};
|
|
|
|
PointCloud(const indexed_triangle_set &M,
|
|
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 < 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;
|
|
}
|
|
|
|
const Node &get(size_t node_id) const
|
|
{
|
|
return *get_node(*this, node_id);
|
|
}
|
|
|
|
Node &get(size_t node_id)
|
|
{
|
|
return *get_node(*this, node_id);
|
|
}
|
|
|
|
const Node *find(size_t node_id) const { return get_node(*this, node_id); }
|
|
Node *find(size_t node_id) { return get_node(*this, node_id); }
|
|
|
|
// Return the original index of a leaf in the input array, if the given
|
|
// node id is indeed of type SUPP
|
|
int get_leaf_id(size_t node_id) const
|
|
{
|
|
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) const;
|
|
|
|
size_t next_junction_id() const
|
|
{
|
|
return JUNCTIONS_BEGIN + m_junctions.size();
|
|
}
|
|
|
|
size_t insert_junction(const Node &p)
|
|
{
|
|
size_t new_id = next_junction_id();
|
|
m_junctions.emplace_back(p);
|
|
m_junctions.back().id = int(new_id);
|
|
m_ktree.insert({m_junctions.back().pos, new_id});
|
|
m_searchable_indices.emplace_back(true);
|
|
m_queue_indices.emplace_back(Unqueued);
|
|
++m_reachable_cnt;
|
|
|
|
return new_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)
|
|
{
|
|
assert(node_id < m_searchable_indices.size());
|
|
|
|
m_searchable_indices[node_id] = false;
|
|
m_queue_indices[node_id] = Unqueued;
|
|
--m_reachable_cnt;
|
|
}
|
|
|
|
size_t reachable_count() const { return m_reachable_cnt; }
|
|
|
|
template<class Fn>
|
|
void foreach_reachable(const Vec3f &pos,
|
|
Fn &&visitor,
|
|
size_t k,
|
|
double min_dist = 0.)
|
|
{
|
|
// Fake output iterator
|
|
struct Output {
|
|
const PointCloud *self;
|
|
Vec3f p;
|
|
Fn &visitorfn;
|
|
|
|
Output& operator *() { return *this; }
|
|
void operator=(const PointIndexEl &el) {
|
|
visitorfn(el.second, self->get_distance(p, el.second));
|
|
}
|
|
Output& operator++() { return *this; }
|
|
};
|
|
|
|
namespace bgi = boost::geometry::index;
|
|
|
|
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;
|
|
});
|
|
|
|
m_ktree.query(bgi::nearest(pos, k) && filter, Output{this, pos, visitor});
|
|
}
|
|
|
|
auto start_queue()
|
|
{
|
|
auto ptsqueue = make_mutable_priority_queue<size_t, true>(
|
|
[this](size_t el, size_t idx) { m_queue_indices[el] = idx; },
|
|
ZCompareFn{this});
|
|
|
|
ptsqueue.reserve(m_leafs.size());
|
|
size_t iend = LEAFS_BEGIN + m_leafs.size();
|
|
for (size_t i = LEAFS_BEGIN; i < iend; ++i)
|
|
ptsqueue.push(i);
|
|
|
|
return ptsqueue;
|
|
}
|
|
};
|
|
|
|
template<class PC, class Fn> void traverse(PC &&pc, size_t root, Fn &&fn)
|
|
{
|
|
if (auto nodeptr = pc.find(root); nodeptr != nullptr) {
|
|
auto &nroot = *nodeptr;
|
|
fn(nroot);
|
|
if (nroot.left >= 0) traverse(pc, nroot.left, fn);
|
|
if (nroot.right >= 0) traverse(pc, nroot.right, fn);
|
|
}
|
|
}
|
|
|
|
void build_tree(PointCloud &pcloud, Builder &builder);
|
|
|
|
}} // namespace Slic3r::branchingtree
|
|
|
|
#endif // POINTCLOUD_HPP
|