9#include <unordered_map>
69template<u
int8_t Dim,
typename Component>
73 using Point = std::array<Component, Dim>;
85 for (uint8_t d = 0; d < Dim; ++d)
89 size_t size()
const {
return n; }
93 for (uint8_t d = 0; d < Dim; ++d)
100 for (uint8_t d = 0; d < Dim; ++d)
101 point[d] =
begin(d)[i];
107 for (uint8_t d = 0; d < Dim; ++d)
108 begin(d)[i] = point[d];
113 const Component *
begin(uint8_t d)
const {
return coords.get() + d *
n; }
114 const Component *
end(uint8_t d)
const {
return begin(d) +
n; }
138 for (uint8_t d = 0; d < Dim; ++d) {
139 for (
size_t i = 0; i < n; ++i) {
159 const auto mid =
begin + left_n;
162 for (
auto it =
begin; it != mid; ++it)
170 for (uint8_t d = 0; d < Dim; ++d) {
180 *(right_ptr++) = *it;
186 for (
auto it =
begin; it != mid; ++it)
187 markers[*it] =
false;
189 return SplitResult{std::move(left), std::move(right), *mid};
201template<u
int8_t Dim,
class Component>
211 points.setPoint(0, point);
218 for (uint8_t d = 0; d < Dim; ++d) {
219 const auto coord =
points.begin(d);
221 return coord[i] < coord[j];
230 const auto n =
points.size();
232 for (uint8_t d = 0; d < Dim; ++d) {
235 const auto coord =
points.begin(d);
236 auto a_ptr = a.
indices.begin(d);
237 auto b_ptr = b.
indices.begin(d);
238 auto dst_ptr =
indices.begin(d);
240 const auto i = *a_ptr;
241 const auto j = *b_ptr + a.
size();
242 if (coord[i] <= coord[j]) {
250 while (a_ptr != a.
indices.end(d))
251 *(dst_ptr++) = *(a_ptr++);
252 while (b_ptr != b.
indices.end(d))
253 *(dst_ptr++) = a.
size() + *(b_ptr++);
268template<u
int8_t Dim,
class Component,
class Id>
272 using Point = std::array<Component, Dim>;
285 ,
ids(std::make_unique<Id[]>(1))
286 ,
tree(std::make_unique<
Idx[]>(1))
294 KdTree(
size_t n, Id
const *
ids, std::array<Component const *, Dim> pts)
296 ,
ids(std::make_unique<Id[]>(n))
297 ,
tree(std::make_unique<
Idx[]>(n))
300 std::copy(
ids,
ids + n, this->ids.get());
308 tree = std::make_unique<Idx[]>(
cap());
309 ids = std::make_unique<Id[]>(
cap());
310 std::copy(a.
ids.get(), a.
ids.get() + a.
cap(),
ids.get());
334 void foreach(F cb)
const
336 for (
size_t i = 0; i <
cap(); ++i) {
338 cb(i,
items.points.getPoint(i),
ids[i]);
352 const auto next_axis = (axis + 1) % Dim;
353 if (!
split.left.empty())
354 init(2 * root + 1, next_axis,
split.left);
355 if (!
split.right.empty())
356 init(2 * root + 2, next_axis,
split.right);
367 const auto ptid =
tree[root];
368 const auto coord =
items.points.begin(
split)[ptid];
369 const auto leftChild = 2*root + 1;
370 const auto rightChild = 2*root + 2;
371 const auto nextSplit = (
split + 1) % Dim;
372 if (min[
split] > coord) {
373 rangeQuery(rightChild, nextSplit, min, max, cb);
374 }
else if (max[
split] < coord) {
375 rangeQuery(leftChild, nextSplit, min, max, cb);
377 rangeQuery(rightChild, nextSplit, min, max, cb);
378 rangeQuery(leftChild, nextSplit, min, max, cb);
381 const auto point =
items.points.getPoint(ptid);
382 for (uint8_t d = 0; d < Dim; ++d)
383 if (point[d] < min[d] || point[d] > max[d])
385 cb(point,
ids[ptid]);
389 std::unique_ptr<Id[]>
ids;
394template<u
int8_t Dim,
class Component,
class Id>
402 void insert(
const std::array<Component, Dim> &point, Id
id)
404 Tree tree(point,
id);
405 for (uint8_t tree_idx = 0;; ++tree_idx) {
406 if (tree_idx ==
trees.size()) {
407 trees.push_back(std::move(tree));
412 if (
trees[tree_idx].cap() == 0) {
413 trees[tree_idx] = std::move(tree);
428 const auto &entry = node.mapped();
429 trees.at(entry.tree_idx).remove(entry.in_tree);
446 for (
const auto &tree :
trees)
447 tree.rangeQuery(min, max, cb);
459 trees[tree_idx].foreach([&](
Idx in_tree_idx,
auto _, Id
id) {
475 const auto live_ids = std::make_unique<Id[]>(
n_entries);
478 for (
const auto &tree :
trees) {
479 tree.foreach([&](
Idx _,
auto point, Id
id) {
490 auto id_ptr = live_ids.get();
491 std::array<Component const *, Dim> point_ptrs;
493 for (uint8_t d = 0; d < Dim; ++d)
494 point_ptrs[d] = live_points.
begin(d);
495 for (uint8_t tree_idx = 0; tree_idx <
trees.size() - 1; ++tree_idx, n *= 2) {
499 if (
trees[tree_idx+1].cap() > 0) {
500 tree = std::move(
Tree(n, id_ptr, point_ptrs));
502 for (uint8_t d = 0; d < Dim; ++d)
505 trees[tree_idx] = std::move(tree);
Definition k_d_tree.h:396
void updateDelEntries(uint8_t tree_idx)
Definition k_d_tree.h:457
void rangeQuery(const Point &min, const Point &max, const F &cb) const
Definition k_d_tree.h:443
std::vector< Tree > trees
Definition k_d_tree.h:512
void remove(Id id)
Definition k_d_tree.h:423
std::unordered_map< Id, DelEntry > del_entries
Definition k_d_tree.h:517
size_t n_entries
Definition k_d_tree.h:518
typename Tree::Point Point
Definition k_d_tree.h:400
KdTree< Dim, Component, Id > Tree
Definition k_d_tree.h:397
size_t size() const
Definition k_d_tree.h:450
void update(const Point &newPos, Id id)
Definition k_d_tree.h:436
size_t deleted
Definition k_d_tree.h:519
void insert(const std::array< Component, Dim > &point, Id id)
Definition k_d_tree.h:402
void shrink_to_half()
Definition k_d_tree.h:465
Definition k_d_tree.h:270
void remove(Idx internalIdx)
Definition k_d_tree.h:327
void init(Idx root, uint8_t axis, const SortedIndices< Dim > &sorted)
Definition k_d_tree.h:347
KdTree(const KdTree &a, const KdTree &b)
Merge two trees. Both trees are assumed to have a power of two size.
Definition k_d_tree.h:305
std::array< Component, Dim > Point
Definition k_d_tree.h:272
std::unique_ptr< Idx[]> tree
Definition k_d_tree.h:390
void rangeQuery(const Point &min, const Point &max, const F &cb) const
Definition k_d_tree.h:321
KdTree()
Empty tree.
Definition k_d_tree.h:275
KdTree(const Point &point, const Id &id)
Build a tree containing just a single point.
Definition k_d_tree.h:283
std::unique_ptr< Id[]> ids
Definition k_d_tree.h:389
size_t cap() const
Capacity, not size, since some items may be marked as deleted.
Definition k_d_tree.h:344
KdTree(size_t n, Id const *ids, std::array< Component const *, Dim > pts)
Build a tree.
Definition k_d_tree.h:294
std::vector< bool > deleted
Definition k_d_tree.h:391
SortedPoints< Dim, Component > items
Definition k_d_tree.h:388
void rangeQuery(size_t root, uint8_t split, const Point &min, const Point &max, const F &cb) const
Definition k_d_tree.h:361
size_t n
Definition k_d_tree.h:117
Points()
Empty.
Definition k_d_tree.h:75
Point getPoint(Idx i) const
Definition k_d_tree.h:97
Component * end(uint8_t d)
Definition k_d_tree.h:112
std::unique_ptr< Component[]> coords
Definition k_d_tree.h:118
std::array< Component, Dim > Point
Definition k_d_tree.h:73
Component * begin(uint8_t d)
Definition k_d_tree.h:111
Points(size_t n)
Allocating constructor; leaves coords uninitialized!
Definition k_d_tree.h:77
void setPoint(Idx i, const Point &point)
Definition k_d_tree.h:105
void assign(Idx start, const Points &from)
Definition k_d_tree.h:91
size_t size() const
Definition k_d_tree.h:89
Points(size_t n, const std::array< Component const *, Dim > &coords)
Copying constructor.
Definition k_d_tree.h:82
const Component * end(uint8_t d) const
Definition k_d_tree.h:114
const Component * begin(uint8_t d) const
Definition k_d_tree.h:113
Definition k_d_tree.h:123
static SortedIndices newUninitialized(size_t n)
uninitialized indices
Definition k_d_tree.h:129
Idx * end(uint8_t d)
Definition k_d_tree.h:193
Points< Dim, Idx > indices
Definition k_d_tree.h:198
SortedIndices(Points< Dim, Idx > &&indices)
Definition k_d_tree.h:197
Idx * begin(uint8_t d)
Definition k_d_tree.h:192
bool empty() const
Definition k_d_tree.h:146
const Idx * end(uint8_t d) const
Definition k_d_tree.h:195
const Idx * begin(uint8_t d) const
Definition k_d_tree.h:194
size_t size() const
Definition k_d_tree.h:145
SortedIndices()
empty
Definition k_d_tree.h:126
SplitResult split(uint8_t axis, std::vector< bool > &markers) const
Definition k_d_tree.h:155
SortedIndices(size_t n)
Identity permutation on all axes.
Definition k_d_tree.h:135
Definition k_d_tree.h:203
size_t size() const
Definition k_d_tree.h:257
SortedPoints(size_t n, const std::array< Component const *, Dim > ptrs)
Sort points.
Definition k_d_tree.h:215
Points< Dim, Component > points
Definition k_d_tree.h:264
SortedPoints()
Definition k_d_tree.h:205
SortedPoints(const SortedPoints &a, const SortedPoints &b)
Merge two sets of sorted points.
Definition k_d_tree.h:227
SortedIndices< Dim > indices
Definition k_d_tree.h:265
SortedPoints(const std::array< Component, Dim > &point)
Single point.
Definition k_d_tree.h:208
#define sanity_check(expr)
Definition debug.h:74
#define _(String)
Definition gettext.h:28
uint16_t Idx
Definition k_d_tree.h:61
constexpr size_t IDX_MAX
Definition k_d_tree.h:62
std::vector< std::basic_string< T > > split(std::basic_string_view< T > s, T delim, T escape=static_cast< T >('\\'))
Splits a string into its component parts separated by the character delimiter.
Definition string.h:681
Definition k_d_tree.h:513
Idx in_tree
Definition k_d_tree.h:515
uint8_t tree_idx
Definition k_d_tree.h:514
Definition k_d_tree.h:148
SortedIndices right
Definition k_d_tree.h:149
SortedIndices left
Definition k_d_tree.h:149
Idx pivot
Definition k_d_tree.h:150