Luanti 5.19.0-dev
Loading...
Searching...
No Matches
k_d_tree.h
Go to the documentation of this file.
1// Copyright (C) 2024 Lars Müller
2// SPDX-License-Identifier: LGPL-2.1-or-later
3
4#pragma once
5
6#include <algorithm>
7#include <cassert>
8#include <cstdint>
9#include <unordered_map>
10#include <vector>
11#include <memory>
12#include "debug.h"
13
14/*
15This implements a dynamic forest of static k-d-trees.
16
17A k-d-tree is a k-dimensional binary search tree.
18On the i-th level of the tree, you split by the (i mod k)-th coordinate.
19
20Building a balanced k-d-tree for n points is done in O(n log n) time:
21Points are stored in a matrix, identified by indices.
22These indices are presorted by all k axes.
23To split, you simply pick the pivot index in the appropriate index array,
24and mark all points left to it by index in a bitset.
25This lets you then split the indices sorted by the other axes,
26while preserving the sorted order.
27
28This however only gives us a static spatial index.
29To make it dynamic, we keep a "forest" of k-d-trees of sizes of successive powers of two.
30When we insert a new tree, we check whether there already is a k-d-tree of the same size.
31If this is the case, we merge with that tree, giving us a tree of twice the size,
32and so on, until we find a free size.
33
34This means our "forest" corresponds to a bit pattern,
35where a set bit means a non-empty tree.
36Inserting a point is equivalent to incrementing this bit pattern.
37
38To handle deletions, we simply mark the appropriate point as deleted using another bitset.
39When more than half the points have been deleted,
40we shrink the structure by removing all deleted points.
41This is equivalent to shifting down the "bit pattern" by one.
42
43There are plenty variations that could be explored:
44
45* Keeping a small amount of points in a small pool to make updates faster -
46 avoid building and querying small k-d-trees.
47 This might be useful if the overhead for small sizes hurts performance.
48* Keeping fewer trees to make queries faster, at the expense of updates.
49* More eagerly removing entries marked as deleted (for example, on merge).
50* Replacing the array-backed structure with a structure of dynamically allocated nodes.
51 This would make it possible to "let trees get out of shape".
52* Shrinking the structure currently sorts the live points by all axes,
53 not leveraging the existing presorting of the subsets.
54 Cleverly done filtering followed by sorted merges should enable linear time.
55* A special ray proximity query could be implemented. This is tricky however.
56*/
57
58namespace k_d_tree
59{
60
61using Idx = uint16_t;
62constexpr size_t IDX_MAX = UINT16_MAX;
63
64// We use size_t for sizes (but not for indices)
65// to make sure there are no wraparounds when we approach the limit.
66// This hardly affects performance or memory usage;
67// the core arrays still only store indices.
68
69template<uint8_t Dim, typename Component>
70class Points
71{
72public:
73 using Point = std::array<Component, Dim>;
75 Points() : n(0), coords(nullptr) {}
77 Points(size_t n) : n(n), coords(new Component[Dim * n])
78 {
79 assert(n <= IDX_MAX);
80 }
81
82 Points(size_t n, const std::array<Component const *, Dim> &coords)
83 : Points(n)
84 {
85 for (uint8_t d = 0; d < Dim; ++d)
86 std::copy(coords[d], coords[d] + n, begin(d));
87 }
88
89 size_t size() const { return n; }
90
91 void assign(Idx start, const Points &from)
92 {
93 for (uint8_t d = 0; d < Dim; ++d)
94 std::copy(from.begin(d), from.end(d), begin(d) + start);
95 }
96
98 {
99 Point point;
100 for (uint8_t d = 0; d < Dim; ++d)
101 point[d] = begin(d)[i];
102 return point;
103 }
104
105 void setPoint(Idx i, const Point &point)
106 {
107 for (uint8_t d = 0; d < Dim; ++d)
108 begin(d)[i] = point[d];
109 }
110
111 Component *begin(uint8_t d) { return coords.get() + d * n; }
112 Component *end(uint8_t d) { return begin(d) + n; }
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; }
115
116private:
117 size_t n;
118 std::unique_ptr<Component[]> coords;
119};
120
121template<uint8_t Dim>
123{
124public:
127
130 {
132 }
133
136 : indices(n)
137 {
138 for (uint8_t d = 0; d < Dim; ++d) {
139 for (size_t i = 0; i < n; ++i) {
140 indices.begin(d)[i] = i;
141 }
142 }
143 }
144
145 size_t size() const { return indices.size(); }
146 bool empty() const { return size() == 0; }
147
152
155 SplitResult split(uint8_t axis, std::vector<bool> &markers) const
156 {
157 const auto begin = indices.begin(axis);
158 Idx left_n = indices.size() / 2;
159 const auto mid = begin + left_n;
160
161 // Mark all points to be partitioned left
162 for (auto it = begin; it != mid; ++it)
163 markers[*it] = true;
164
165 SortedIndices left(left_n);
166 std::copy(begin, mid, left.indices.begin(axis));
167 SortedIndices right(indices.size() - left_n - 1);
168 std::copy(mid + 1, indices.end(axis), right.indices.begin(axis));
169
170 for (uint8_t d = 0; d < Dim; ++d) {
171 if (d == axis)
172 continue;
173 auto left_ptr = left.indices.begin(d);
174 auto right_ptr = right.indices.begin(d);
175 for (auto it = indices.begin(d); it != indices.end(d); ++it) {
176 if (*it != *mid) { // ignore pivot
177 if (markers[*it])
178 *(left_ptr++) = *it;
179 else
180 *(right_ptr++) = *it;
181 }
182 }
183 }
184
185 // Unmark points, since we want to reuse the storage for markers
186 for (auto it = begin; it != mid; ++it)
187 markers[*it] = false;
188
189 return SplitResult{std::move(left), std::move(right), *mid};
190 }
191
192 Idx *begin(uint8_t d) { return indices.begin(d); }
193 Idx *end(uint8_t d) { return indices.end(d); }
194 const Idx *begin(uint8_t d) const { return indices.begin(d); }
195 const Idx *end(uint8_t d) const { return indices.end(d); }
196private:
199};
200
201template<uint8_t Dim, class Component>
203{
204public:
206
208 SortedPoints(const std::array<Component, Dim> &point)
209 : points(1), indices(1)
210 {
211 points.setPoint(0, point);
212 }
213
215 SortedPoints(size_t n, const std::array<Component const *, Dim> ptrs)
216 : points(n, ptrs), indices(n)
217 {
218 for (uint8_t d = 0; d < Dim; ++d) {
219 const auto coord = points.begin(d);
220 std::sort(indices.begin(d), indices.end(d), [&](auto i, auto j) {
221 return coord[i] < coord[j];
222 });
223 }
224 }
225
228 : points(a.size() + b.size())
229 {
230 const auto n = points.size();
232 for (uint8_t d = 0; d < Dim; ++d) {
233 points.assign(0, a.points);
234 points.assign(a.points.size(), b.points);
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);
239 while (a_ptr != a.indices.end(d) && b_ptr != b.indices.end(d)) {
240 const auto i = *a_ptr;
241 const auto j = *b_ptr + a.size();
242 if (coord[i] <= coord[j]) {
243 *(dst_ptr++) = i;
244 ++a_ptr;
245 } else {
246 *(dst_ptr++) = j;
247 ++b_ptr;
248 }
249 }
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++);
254 }
255 }
256
257 size_t size() const
258 {
259 // technically redundant with indices.size(),
260 // but that is irrelevant
261 return points.size();
262 }
263
266};
267
268template<uint8_t Dim, class Component, class Id>
270{
271public:
272 using Point = std::array<Component, Dim>;
273
276 : items()
277 , ids(nullptr)
278 , tree(nullptr)
279 , deleted()
280 {}
281
283 KdTree(const Point &point, const Id &id)
284 : items(point)
285 , ids(std::make_unique<Id[]>(1))
286 , tree(std::make_unique<Idx[]>(1))
287 , deleted(1)
288 {
289 tree[0] = 0;
290 ids[0] = id;
291 }
292
294 KdTree(size_t n, Id const *ids, std::array<Component const *, Dim> pts)
295 : items(n, pts)
296 , ids(std::make_unique<Id[]>(n))
297 , tree(std::make_unique<Idx[]>(n))
298 , deleted(n)
299 {
300 std::copy(ids, ids + n, this->ids.get());
301 init(0, 0, items.indices);
302 }
303
305 KdTree(const KdTree &a, const KdTree &b)
306 : items(a.items, b.items)
307 {
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());
311 std::copy(b.ids.get(), b.ids.get() + b.cap(), ids.get() + a.cap());
312 // Note: Initialize `deleted` *before* calling `init`,
313 // since `init` abuses the `deleted` marks as left/right marks.
314 deleted = std::vector<bool>(cap());
315 init(0, 0, items.indices);
316 std::copy(a.deleted.begin(), a.deleted.end(), deleted.begin());
317 std::copy(b.deleted.begin(), b.deleted.end(), deleted.begin() + a.items.size());
318 }
319
320 template<typename F>
321 void rangeQuery(const Point &min, const Point &max,
322 const F &cb) const
323 {
324 rangeQuery(0, 0, min, max, cb);
325 }
326
327 void remove(Idx internalIdx)
328 {
329 assert(!deleted[internalIdx]);
330 deleted[internalIdx] = true;
331 }
332
333 template<class F>
334 void foreach(F cb) const
335 {
336 for (size_t i = 0; i < cap(); ++i) {
337 if (!deleted[i]) {
338 cb(i, items.points.getPoint(i), ids[i]);
339 }
340 }
341 }
342
344 size_t cap() const { return items.size(); }
345
346private:
347 void init(Idx root, uint8_t axis, const SortedIndices<Dim> &sorted)
348 {
349 // Temporarily abuse "deleted" marks as left/right marks
350 const auto split = sorted.split(axis, deleted);
351 tree[root] = split.pivot;
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);
357 }
358
359 template<typename F>
360 // Note: root is of type size_t to avoid issues with wraparound
361 void rangeQuery(size_t root, uint8_t split,
362 const Point &min, const Point &max,
363 const F &cb) const
364 {
365 if (root >= cap())
366 return;
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);
376 } else {
377 rangeQuery(rightChild, nextSplit, min, max, cb);
378 rangeQuery(leftChild, nextSplit, min, max, cb);
379 if (deleted[ptid])
380 return;
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])
384 return;
385 cb(point, ids[ptid]);
386 }
387 }
389 std::unique_ptr<Id[]> ids;
390 std::unique_ptr<Idx[]> tree;
391 std::vector<bool> deleted;
392};
393
394template<uint8_t Dim, class Component, class Id>
396{
398
399public:
400 using Point = typename Tree::Point;
401
402 void insert(const std::array<Component, Dim> &point, Id id)
403 {
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));
408 updateDelEntries(tree_idx);
409 break;
410 }
411 // Can we use a free slot to "plant" the tree?
412 if (trees[tree_idx].cap() == 0) {
413 trees[tree_idx] = std::move(tree);
414 updateDelEntries(tree_idx);
415 break;
416 }
417 tree = Tree(tree, trees[tree_idx]);
418 trees[tree_idx] = std::move(Tree());
419 }
420 ++n_entries;
421 }
422
423 void remove(Id id)
424 {
425 {
426 auto node = del_entries.extract(id);
427 sanity_check(node);
428 const auto &entry = node.mapped();
429 trees.at(entry.tree_idx).remove(entry.in_tree);
430 }
431 ++deleted;
432 if (deleted >= (n_entries+1)/2) // "shift out" the last tree
434 }
435
436 void update(const Point &newPos, Id id)
437 {
438 remove(id);
439 insert(newPos, id);
440 }
441
442 template<typename F>
443 void rangeQuery(const Point &min, const Point &max,
444 const F &cb) const
445 {
446 for (const auto &tree : trees)
447 tree.rangeQuery(min, max, cb);
448 }
449
450 size_t size() const
451 {
452 return n_entries - deleted;
453 }
454
455private:
456
457 void updateDelEntries(uint8_t tree_idx)
458 {
459 trees[tree_idx].foreach([&](Idx in_tree_idx, auto _, Id id) {
460 del_entries[id] = {tree_idx, in_tree_idx};
461 });
462 }
463
464 // Shrink to half the size, equivalent to shifting down the "bit pattern".
466 {
467 assert(n_entries >= deleted);
468 assert(n_entries - deleted == (n_entries >> 1));
470 deleted = 0;
471 // Reset map, freeing memory (instead of clearing)
472 del_entries = std::unordered_map<Id, DelEntry>();
473
474 // Collect all live points and corresponding IDs.
475 const auto live_ids = std::make_unique<Id[]>(n_entries);
477 size_t i = 0;
478 for (const auto &tree : trees) {
479 tree.foreach([&](Idx _, auto point, Id id) {
480 assert(i < n_entries);
481 live_points.setPoint(static_cast<Idx>(i), point);
482 live_ids[i] = id;
483 ++i;
484 });
485 }
486 assert(i == n_entries);
487
488 // Construct a new forest.
489 // The "tree pattern" will effectively just be shifted down by one.
490 auto id_ptr = live_ids.get();
491 std::array<Component const *, Dim> point_ptrs;
492 size_t n = 1;
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) {
496 Tree tree;
497 // If there was a tree at the next position, there should be
498 // a tree at this position after shifting the pattern.
499 if (trees[tree_idx+1].cap() > 0) {
500 tree = std::move(Tree(n, id_ptr, point_ptrs));
501 id_ptr += n;
502 for (uint8_t d = 0; d < Dim; ++d)
503 point_ptrs[d] += n;
504 }
505 trees[tree_idx] = std::move(tree);
506 updateDelEntries(tree_idx);
507 }
508 trees.pop_back(); // "shift out" tree with the most elements
509 }
510 // This could even use an array instead of a vector,
511 // since the number of trees is guaranteed to be logarithmic in the max of Idx
512 std::vector<Tree> trees;
513 struct DelEntry {
514 uint8_t tree_idx;
516 };
517 std::unordered_map<Id, DelEntry> del_entries;
518 size_t n_entries = 0;
519 size_t deleted = 0;
520};
521
522} // end namespace k_d_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
Definition k_d_tree.h:71
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
Definition k_d_tree.h:59
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