1 #include "Clustering.hpp"
2 #include "boost/geometry/index/rtree.hpp"
3
4 #include <libslic3r/SLA/SpatIndex.hpp>
5 #include <libslic3r/SLA/BoostAdapter.hpp>
6
7 namespace Slic3r { namespace sla {
8
9 namespace bgi = boost::geometry::index;
10 using Index3D = bgi::rtree< PointIndexEl, bgi::rstar<16, 4> /* ? */ >;
11
12 namespace {
13
cmp_ptidx_elements(const PointIndexEl & e1,const PointIndexEl & e2)14 bool cmp_ptidx_elements(const PointIndexEl& e1, const PointIndexEl& e2)
15 {
16 return e1.second < e2.second;
17 };
18
cluster(Index3D & sindex,unsigned max_points,std::function<std::vector<PointIndexEl> (const Index3D &,const PointIndexEl &)> qfn)19 ClusteredPoints cluster(Index3D &sindex,
20 unsigned max_points,
21 std::function<std::vector<PointIndexEl>(
22 const Index3D &, const PointIndexEl &)> qfn)
23 {
24 using Elems = std::vector<PointIndexEl>;
25
26 // Recursive function for visiting all the points in a given distance to
27 // each other
28 std::function<void(Elems&, Elems&)> group =
29 [&sindex, &group, max_points, qfn](Elems& pts, Elems& cluster)
30 {
31 for(auto& p : pts) {
32 std::vector<PointIndexEl> tmp = qfn(sindex, p);
33
34 std::sort(tmp.begin(), tmp.end(), cmp_ptidx_elements);
35
36 Elems newpts;
37 std::set_difference(tmp.begin(), tmp.end(),
38 cluster.begin(), cluster.end(),
39 std::back_inserter(newpts), cmp_ptidx_elements);
40
41 int c = max_points && newpts.size() + cluster.size() > max_points?
42 int(max_points - cluster.size()) : int(newpts.size());
43
44 cluster.insert(cluster.end(), newpts.begin(), newpts.begin() + c);
45 std::sort(cluster.begin(), cluster.end(), cmp_ptidx_elements);
46
47 if(!newpts.empty() && (!max_points || cluster.size() < max_points))
48 group(newpts, cluster);
49 }
50 };
51
52 std::vector<Elems> clusters;
53 for(auto it = sindex.begin(); it != sindex.end();) {
54 Elems cluster = {};
55 Elems pts = {*it};
56 group(pts, cluster);
57
58 for(auto& c : cluster) sindex.remove(c);
59 it = sindex.begin();
60
61 clusters.emplace_back(cluster);
62 }
63
64 ClusteredPoints result;
65 for(auto& cluster : clusters) {
66 result.emplace_back();
67 for(auto c : cluster) result.back().emplace_back(c.second);
68 }
69
70 return result;
71 }
72
distance_queryfn(const Index3D & sindex,const PointIndexEl & p,double dist,unsigned max_points)73 std::vector<PointIndexEl> distance_queryfn(const Index3D& sindex,
74 const PointIndexEl& p,
75 double dist,
76 unsigned max_points)
77 {
78 std::vector<PointIndexEl> tmp; tmp.reserve(max_points);
79 sindex.query(
80 bgi::nearest(p.first, max_points),
81 std::back_inserter(tmp)
82 );
83
84 for(auto it = tmp.begin(); it < tmp.end(); ++it)
85 if((p.first - it->first).norm() > dist) it = tmp.erase(it);
86
87 return tmp;
88 }
89
90 } // namespace
91
92 // Clustering a set of points by the given criteria
cluster(const std::vector<unsigned> & indices,std::function<Vec3d (unsigned)> pointfn,double dist,unsigned max_points)93 ClusteredPoints cluster(
94 const std::vector<unsigned>& indices,
95 std::function<Vec3d(unsigned)> pointfn,
96 double dist,
97 unsigned max_points)
98 {
99 // A spatial index for querying the nearest points
100 Index3D sindex;
101
102 // Build the index
103 for(auto idx : indices) sindex.insert( std::make_pair(pointfn(idx), idx));
104
105 return cluster(sindex, max_points,
106 [dist, max_points](const Index3D& sidx, const PointIndexEl& p)
107 {
108 return distance_queryfn(sidx, p, dist, max_points);
109 });
110 }
111
112 // Clustering a set of points by the given criteria
cluster(const std::vector<unsigned> & indices,std::function<Vec3d (unsigned)> pointfn,std::function<bool (const PointIndexEl &,const PointIndexEl &)> predicate,unsigned max_points)113 ClusteredPoints cluster(
114 const std::vector<unsigned>& indices,
115 std::function<Vec3d(unsigned)> pointfn,
116 std::function<bool(const PointIndexEl&, const PointIndexEl&)> predicate,
117 unsigned max_points)
118 {
119 // A spatial index for querying the nearest points
120 Index3D sindex;
121
122 // Build the index
123 for(auto idx : indices) sindex.insert( std::make_pair(pointfn(idx), idx));
124
125 return cluster(sindex, max_points,
126 [max_points, predicate](const Index3D& sidx, const PointIndexEl& p)
127 {
128 std::vector<PointIndexEl> tmp; tmp.reserve(max_points);
129 sidx.query(bgi::satisfies([p, predicate](const PointIndexEl& e){
130 return predicate(p, e);
131 }), std::back_inserter(tmp));
132 return tmp;
133 });
134 }
135
cluster(const Eigen::MatrixXd & pts,double dist,unsigned max_points)136 ClusteredPoints cluster(const Eigen::MatrixXd& pts, double dist, unsigned max_points)
137 {
138 // A spatial index for querying the nearest points
139 Index3D sindex;
140
141 // Build the index
142 for(Eigen::Index i = 0; i < pts.rows(); i++)
143 sindex.insert(std::make_pair(Vec3d(pts.row(i)), unsigned(i)));
144
145 return cluster(sindex, max_points,
146 [dist, max_points](const Index3D& sidx, const PointIndexEl& p)
147 {
148 return distance_queryfn(sidx, p, dist, max_points);
149 });
150 }
151
152 }} // namespace Slic3r::sla
153