1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
|
// KD tree built upon external data set, referencing the external data by integer indices.
#ifndef slic3r_KDTreeIndirect_hpp_
#define slic3r_KDTreeIndirect_hpp_
#include <algorithm>
#include <limits>
#include <vector>
#include "Utils.hpp" // for next_highest_power_of_2()
namespace Slic3r {
// KD tree for N-dimensional closest point search.
template<size_t ANumDimensions, typename ACoordType, typename ACoordinateFn>
class KDTreeIndirect
{
public:
static constexpr size_t NumDimensions = ANumDimensions;
using CoordinateFn = ACoordinateFn;
using CoordType = ACoordType;
// Following could be static constexpr size_t, but that would not link in C++11
enum : size_t {
npos = size_t(-1)
};
KDTreeIndirect(CoordinateFn coordinate) : coordinate(coordinate) {}
KDTreeIndirect(CoordinateFn coordinate, std::vector<size_t> indices) : coordinate(coordinate) { this->build(std::move(indices)); }
KDTreeIndirect(CoordinateFn coordinate, std::vector<size_t> &&indices) : coordinate(coordinate) { this->build(std::move(indices)); }
KDTreeIndirect(CoordinateFn coordinate, size_t num_indices) : coordinate(coordinate) { this->build(num_indices); }
KDTreeIndirect(KDTreeIndirect &&rhs) : m_nodes(std::move(rhs.m_nodes)), coordinate(std::move(rhs.coordinate)) {}
KDTreeIndirect& operator=(KDTreeIndirect &&rhs) { m_nodes = std::move(rhs.m_nodes); coordinate = std::move(rhs.coordinate); return *this; }
void clear() { m_nodes.clear(); }
void build(size_t num_indices)
{
std::vector<size_t> indices;
indices.reserve(num_indices);
for (size_t i = 0; i < num_indices; ++ i)
indices.emplace_back(i);
this->build(std::move(indices));
}
void build(std::vector<size_t> &&indices)
{
if (indices.empty())
clear();
else {
// Allocate enough memory for a full binary tree.
m_nodes.assign(next_highest_power_of_2(indices.size() + 1), npos);
build_recursive(indices, 0, 0, 0, indices.size() - 1);
}
indices.clear();
}
enum class VisitorReturnMask : unsigned int
{
CONTINUE_LEFT = 1,
CONTINUE_RIGHT = 2,
STOP = 4,
};
template<typename CoordType>
unsigned int descent_mask(const CoordType &point_coord, const CoordType &search_radius, size_t idx, size_t dimension) const
{
CoordType dist = point_coord - this->coordinate(idx, dimension);
return (dist * dist < search_radius + CoordType(EPSILON)) ?
// The plane intersects a hypersphere centered at point_coord of search_radius.
((unsigned int)(VisitorReturnMask::CONTINUE_LEFT) | (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT)) :
// The plane does not intersect the hypersphere.
(dist > CoordType(0)) ? (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT) : (unsigned int)(VisitorReturnMask::CONTINUE_LEFT);
}
// Visitor is supposed to return a bit mask of VisitorReturnMask.
template<typename Visitor>
void visit(Visitor &visitor) const
{
visit_recursive(0, 0, visitor);
}
CoordinateFn coordinate;
private:
// Build a balanced tree by splitting the input sequence by an axis aligned plane at a dimension.
void build_recursive(std::vector<size_t> &input, size_t node, const size_t dimension, const size_t left, const size_t right)
{
if (left > right)
return;
assert(node < m_nodes.size());
if (left == right) {
// Insert a node into the balanced tree.
m_nodes[node] = input[left];
return;
}
// Partition the input to left / right pieces of the same length to produce a balanced tree.
size_t center = (left + right) / 2;
partition_input(input, dimension, left, right, center);
// Insert a node into the tree.
m_nodes[node] = input[center];
// Build up the left / right subtrees.
size_t next_dimension = dimension;
if (++ next_dimension == NumDimensions)
next_dimension = 0;
if (center > left)
build_recursive(input, node * 2 + 1, next_dimension, left, center - 1);
build_recursive(input, node * 2 + 2, next_dimension, center + 1, right);
}
// Partition the input m_nodes <left, right> at "k" and "dimension" using the QuickSelect method:
// https://en.wikipedia.org/wiki/Quickselect
// Items left of the k'th item are lower than the k'th item in the "dimension",
// items right of the k'th item are higher than the k'th item in the "dimension",
void partition_input(std::vector<size_t> &input, const size_t dimension, size_t left, size_t right, const size_t k) const
{
while (left < right) {
size_t center = (left + right) / 2;
CoordType pivot;
{
// Bubble sort the input[left], input[center], input[right], so that a median of the three values
// will end up in input[center].
CoordType left_value = this->coordinate(input[left], dimension);
CoordType center_value = this->coordinate(input[center], dimension);
CoordType right_value = this->coordinate(input[right], dimension);
if (left_value > center_value) {
std::swap(input[left], input[center]);
std::swap(left_value, center_value);
}
if (left_value > right_value) {
std::swap(input[left], input[right]);
right_value = left_value;
}
if (center_value > right_value) {
std::swap(input[center], input[right]);
center_value = right_value;
}
pivot = center_value;
}
if (right <= left + 2)
// The <left, right> interval is already sorted.
break;
size_t i = left;
size_t j = right - 1;
std::swap(input[center], input[j]);
// Partition the set based on the pivot.
for (;;) {
// Skip left points that are already at correct positions.
// Search will certainly stop at position (right - 1), which stores the pivot.
while (this->coordinate(input[++ i], dimension) < pivot) ;
// Skip right points that are already at correct positions.
while (this->coordinate(input[-- j], dimension) > pivot && i < j) ;
if (i >= j)
break;
std::swap(input[i], input[j]);
}
// Restore pivot to the center of the sequence.
std::swap(input[i], input[right - 1]);
// Which side the kth element is in?
if (k < i)
right = i - 1;
else if (k == i)
// Sequence is partitioned, kth element is at its place.
break;
else
left = i + 1;
}
}
template<typename Visitor>
void visit_recursive(size_t node, size_t dimension, Visitor &visitor) const
{
assert(! m_nodes.empty());
if (node >= m_nodes.size() || m_nodes[node] == npos)
return;
// Left / right child node index.
size_t left = node * 2 + 1;
size_t right = left + 1;
unsigned int mask = visitor(m_nodes[node], dimension);
if ((mask & (unsigned int)VisitorReturnMask::STOP) == 0) {
size_t next_dimension = (++ dimension == NumDimensions) ? 0 : dimension;
if (mask & (unsigned int)VisitorReturnMask::CONTINUE_LEFT)
visit_recursive(left, next_dimension, visitor);
if (mask & (unsigned int)VisitorReturnMask::CONTINUE_RIGHT)
visit_recursive(right, next_dimension, visitor);
}
}
std::vector<size_t> m_nodes;
};
// Find a closest point using Euclidian metrics.
// Returns npos if not found.
template<typename KDTreeIndirectType, typename PointType, typename FilterFn>
size_t find_closest_point(const KDTreeIndirectType &kdtree, const PointType &point, FilterFn filter)
{
using CoordType = typename KDTreeIndirectType::CoordType;
struct Visitor {
const KDTreeIndirectType &kdtree;
const PointType &point;
const FilterFn filter;
size_t min_idx = KDTreeIndirectType::npos;
CoordType min_dist = std::numeric_limits<CoordType>::max();
Visitor(const KDTreeIndirectType &kdtree, const PointType &point, FilterFn filter) : kdtree(kdtree), point(point), filter(filter) {}
unsigned int operator()(size_t idx, size_t dimension) {
if (this->filter(idx)) {
auto dist = CoordType(0);
for (size_t i = 0; i < KDTreeIndirectType::NumDimensions; ++ i) {
CoordType d = point[i] - kdtree.coordinate(idx, i);
dist += d * d;
}
if (dist < min_dist) {
min_dist = dist;
min_idx = idx;
}
}
return kdtree.descent_mask(point[dimension], min_dist, idx, dimension);
}
} visitor(kdtree, point, filter);
kdtree.visit(visitor);
return visitor.min_idx;
}
template<typename KDTreeIndirectType, typename PointType>
size_t find_closest_point(const KDTreeIndirectType& kdtree, const PointType& point)
{
return find_closest_point(kdtree, point, [](size_t) { return true; });
}
} // namespace Slic3r
#endif /* slic3r_KDTreeIndirect_hpp_ */
|