34template <
typename T,
typename TxyFunc,
typename CoordT,
typename DistT>
56 if (this->free_list.empty()) {
57 this->nodes.emplace_back(element);
58 return this->nodes.size() - 1;
60 size_t newidx = this->free_list.back();
61 this->free_list.pop_back();
62 this->nodes[newidx] =
node{ element };
68 template <
typename It>
71 It mid = begin + (end - begin) / 2;
72 std::nth_element(begin, mid, end, [&](T a, T b) {
return TxyFunc()(a, level % 2) < TxyFunc()(b, level % 2); });
73 return TxyFunc()(*mid, level % 2);
77 template <
typename It>
80 ptrdiff_t count = end - begin;
84 }
else if (count == 1) {
86 }
else if (count > 1) {
88 It split = std::partition(begin, end, [&](T v) {
return TxyFunc()(v, level % 2) < split_coord; });
89 size_t newidx = this->
AddNode(*split);
90 this->nodes[newidx].left = this->
BuildSubtree(begin, split, level + 1);
91 this->nodes[newidx].right = this->
BuildSubtree(split + 1, end, level + 1);
99 bool Rebuild(
const T *include_element,
const T *exclude_element)
101 size_t initial_count = this->
Count();
104 T root_element = this->nodes[this->
root].element;
105 std::vector<T> elements = this->
FreeSubtree(this->root);
106 elements.push_back(root_element);
108 if (include_element !=
nullptr) {
109 elements.push_back(*include_element);
112 if (exclude_element !=
nullptr) {
113 typename std::vector<T>::iterator removed = std::remove(elements.begin(), elements.end(), *exclude_element);
114 elements.erase(removed, elements.end());
118 this->
Build(elements.begin(), elements.end());
119 assert(initial_count == this->
Count());
129 node &n = this->nodes[node_idx];
132 CoordT nc = TxyFunc()(n.
element, dim);
134 CoordT ec = TxyFunc()(element, dim);
136 size_t &next = (ec < nc) ? n.
left : n.
right;
140 size_t newidx = this->
AddNode(element);
142 node &nn = this->nodes[node_idx];
143 if (ec < nc) nn.
left = newidx;
else nn.
right = newidx;
155 std::vector<T> subtree_elements;
156 node &n = this->nodes[node_idx];
159 size_t first_free = this->free_list.size();
166 for (
size_t i = first_free; i < this->free_list.size(); i++) {
167 node &fn = this->nodes[this->free_list[i]];
168 subtree_elements.push_back(fn.
element);
174 return subtree_elements;
187 node &n = this->nodes[node_idx];
191 this->free_list.push_back(node_idx);
197 std::vector<T> subtree_elements = this->
FreeSubtree(node_idx);
198 return this->
BuildSubtree(subtree_elements.begin(), subtree_elements.end(), level);;
205 CoordT nc = TxyFunc()(n.
element, dim);
207 CoordT ec = TxyFunc()(element, dim);
209 size_t next = (ec < nc) ? n.
left : n.
right;
213 if (new_branch != next) {
215 node &nn = this->nodes[node_idx];
216 if (ec < nc) nn.
left = new_branch;
else nn.
right = new_branch;
223 DistT ManhattanDistance(
const T &element, CoordT x, CoordT y)
const
225 return abs((DistT)TxyFunc()(element, 0) - (DistT)x) +
abs((DistT)TxyFunc()(element, 1) - (DistT)y);
233 if (a.second < b.second)
return a;
234 if (b.second < a.second)
return b;
235 if (a.first < b.first)
return a;
236 if (b.first < a.first)
return b;
245 const node &n = this->nodes[node_idx];
248 CoordT c = TxyFunc()(n.
element, dim);
250 DistT thisdist = this->ManhattanDistance(n.
element, xy[0], xy[1]);
255 size_t next = (xy[dim] < c) ? n.
left : n.
right;
261 limit = std::min(best.second, limit);
265 size_t opposite = (xy[dim] >= c) ? n.
left : n.
right;
274 template <
typename Outputter>
275 void FindContainedRecursive(CoordT p1[2], CoordT p2[2],
size_t node_idx,
int level,
const Outputter &outputter)
const
280 const node &n = this->nodes[node_idx];
283 CoordT ec = TxyFunc()(n.element, dim);
285 CoordT oc = TxyFunc()(n.element, 1 - dim);
288 if (ec >= p1[dim] && ec < p2[dim] && oc >= p1[1 - dim] && oc < p2[1 - dim]) outputter(n.element);
291 if (p1[dim] < ec && n.left !=
INVALID_NODE) this->FindContainedRecursive(p1, p2, n.left, level + 1, outputter);
294 if (p2[dim] > ec && n.right !=
INVALID_NODE) this->FindContainedRecursive(p1, p2, n.right, level + 1, outputter);
301 const node &n = this->nodes[node_idx];
305 void IncrementUnbalanced(
size_t amount = 1)
307 this->unbalanced += amount;
313 size_t count = this->
Count();
315 return this->unbalanced > count / 4;
319 void CheckInvariant(
size_t node_idx,
int level, CoordT min_x, CoordT max_x, CoordT min_y, CoordT max_y)
const
323 const node &n = this->nodes[node_idx];
324 CoordT cx = TxyFunc()(n.
element, 0);
325 CoordT cy = TxyFunc()(n.
element, 1);
332 if (level % 2 == 0) {
347 this->
CheckInvariant(this->root, 0, std::numeric_limits<CoordT>::min(), std::numeric_limits<CoordT>::max(), std::numeric_limits<CoordT>::min(), std::numeric_limits<CoordT>::max());
361 template <
typename It>
365 this->free_list.clear();
366 this->unbalanced = 0;
367 if (begin == end)
return;
368 this->nodes.reserve(end - begin);
380 this->free_list.clear();
381 this->unbalanced = 0;
390 this->
Rebuild(
nullptr,
nullptr);
400 if (this->
Count() == 0) {
401 this->root = this->
AddNode(element);
405 this->IncrementUnbalanced();
419 size_t count = this->
Count();
420 if (count == 0)
return;
424 this->IncrementUnbalanced();
432 assert(this->free_list.size() <= this->nodes.size());
433 return this->nodes.size() - this->free_list.size();
443 assert(this->
Count() > 0);
445 CoordT xy[2] = { x, y };
458 template <
typename Outputter>
459 void FindContained(CoordT x1, CoordT y1, CoordT x2, CoordT y2,
const Outputter &outputter)
const
464 if (this->
Count() == 0)
return;
466 CoordT p1[2] = { x1, y1 };
467 CoordT p2[2] = { x2, y2 };
468 this->FindContainedRecursive(p1, p2, this->root, 0, outputter);
475 std::vector<T>
FindContained(CoordT x1, CoordT y1, CoordT x2, CoordT y2)
const
477 std::vector<T> result;
478 this->
FindContained(x1, y1, x2, y2, [&result](T e) {result.push_back(e); });
K-dimensional tree, specialised for 2-dimensional space.
static const size_t MIN_REBALANCE_THRESHOLD
Arbitrary value for "not worth rebalancing".
void Build(It begin, It end)
Clear and rebuild the tree from a new sequence of elements,.
size_t Count() const
Get number of elements stored in tree.
bool IsUnbalanced() const
Check if the entire tree is in need of rebuilding.
void FindContained(CoordT x1, CoordT y1, CoordT x2, CoordT y2, const Outputter &outputter) const
Find all items contained within the given rectangle.
bool Rebuild(const T *include_element, const T *exclude_element)
Rebuild the tree with all existing elements, optionally adding or removing one more.
std::vector< size_t > free_list
List of dead indices in the nodes vector.
node_distance FindNearestRecursive(CoordT xy[2], size_t node_idx, int level, DistT limit=std::numeric_limits< DistT >::max()) const
Search a sub-tree for the element nearest to a given point.
void InsertRecursive(const T &element, size_t node_idx, int level)
Insert one element in the tree somewhere below node_idx.
void Rebuild()
Reconstruct the tree with the same elements, letting it be fully balanced.
std::pair< T, DistT > node_distance
A data element and its distance to a searched-for point.
void Insert(const T &element)
Insert a single element in the tree.
Kdtree()
Construct a new Kdtree with the given xyfunc.
std::vector< node > nodes
Pool of all nodes in the tree.
std::vector< T > FindContained(CoordT x1, CoordT y1, CoordT x2, CoordT y2) const
Find all items contained within the given rectangle.
static node_distance SelectNearestNodeDistance(const node_distance &a, const node_distance &b)
Ordering function for node_distance objects, elements with equal distance are ordered by less-than co...
void CheckInvariant(size_t node_idx, int level, CoordT min_x, CoordT max_x, CoordT min_y, CoordT max_y) const
Verify that the invariant is true for a sub-tree, assert if not.
void Remove(const T &element)
Remove a single element from the tree, if it exists.
static const size_t INVALID_NODE
Index value indicating no-such-node.
size_t root
Index of root node.
size_t RemoveRecursive(const T &element, size_t node_idx, int level)
Find and remove one element from the tree.
size_t BuildSubtree(It begin, It end, int level)
Construct a subtree from elements between begin and end iterators, return index of root.
size_t AddNode(const T &element)
Create one new node in the tree, return its index in the pool.
void CheckInvariant() const
Verify the invariant for the entire tree, does nothing unless KDTREE_DEBUG is defined.
T FindNearest(CoordT x, CoordT y) const
Find the element closest to given coordinate, in Manhattan distance.
CoordT SelectSplitCoord(It begin, It end, int level)
Find a coordinate value to split a range of elements at.
std::vector< T > FreeSubtree(size_t node_idx)
Free all children of the given node.
void Clear()
Clear the tree.
size_t CountValue(const T &element, size_t node_idx) const
Debugging function, counts number of occurrences of an element regardless of its correct position in ...
size_t unbalanced
Number approximating how unbalanced the tree might be.
constexpr T abs(const T a)
Returns the absolute value of (scalar) variable.
Type of a node in the tree.
T element
Element stored at node.
size_t left
Index of node to the left, INVALID_NODE if none.
size_t right
Index of node to the right, INVALID_NODE if none.