13 #include "../stdafx.h"
36 template <
typename T,
typename TxyFunc,
typename CoordT,
typename DistT>
58 if (this->free_list.size() == 0) {
59 this->nodes.emplace_back(element);
60 return this->nodes.size() - 1;
62 size_t newidx = this->free_list.back();
63 this->free_list.pop_back();
64 this->nodes[newidx] =
node{ element };
70 template <
typename It>
73 It mid = begin + (end - begin) / 2;
74 std::nth_element(begin, mid, end, [&](T a, T b) {
return this->
xyfunc(a, level % 2) < this->
xyfunc(b, level % 2); });
75 return this->
xyfunc(*mid, level % 2);
79 template <
typename It>
82 ptrdiff_t count = end - begin;
86 }
else if (count == 1) {
88 }
else if (count > 1) {
90 It split = std::partition(begin, end, [&](T v) {
return this->
xyfunc(v, level % 2) < split_coord; });
91 size_t newidx = this->
AddNode(*split);
92 this->nodes[newidx].left = this->
BuildSubtree(begin, split, level + 1);
93 this->nodes[newidx].right = this->
BuildSubtree(split + 1, end, level + 1);
101 bool Rebuild(
const T *include_element,
const T *exclude_element)
103 size_t initial_count = this->
Count();
104 if (initial_count < 8)
return false;
106 T root_element = this->nodes[this->
root].element;
107 std::vector<T> elements = this->
FreeSubtree(this->root);
108 elements.push_back(root_element);
110 if (include_element !=
nullptr) {
111 elements.push_back(*include_element);
114 if (exclude_element !=
nullptr) {
115 typename std::vector<T>::iterator removed = std::remove(elements.begin(), elements.end(), *exclude_element);
116 elements.erase(removed, elements.end());
120 this->
Build(elements.begin(), elements.end());
121 assert(initial_count == this->
Count());
131 node &n = this->nodes[node_idx];
136 CoordT ec = this->
xyfunc(element, dim);
138 size_t &next = (ec < nc) ? n.
left : n.
right;
142 size_t newidx = this->
AddNode(element);
144 node &nn = this->nodes[node_idx];
145 if (ec < nc) nn.
left = newidx;
else nn.
right = newidx;
157 std::vector<T> subtree_elements;
158 node &n = this->nodes[node_idx];
161 size_t first_free = this->free_list.size();
168 for (
size_t i = first_free; i < this->free_list.size(); i++) {
169 node &fn = this->nodes[this->free_list[i]];
170 subtree_elements.push_back(fn.
element);
176 return subtree_elements;
189 node &n = this->nodes[node_idx];
193 this->free_list.push_back(node_idx);
199 std::vector<T> subtree_elements = this->
FreeSubtree(node_idx);
200 return this->
BuildSubtree(subtree_elements.begin(), subtree_elements.end(), level);;
209 CoordT ec = this->
xyfunc(element, dim);
211 size_t next = (ec < nc) ? n.
left : n.
right;
215 if (new_branch != next) {
217 node &nn = this->nodes[node_idx];
218 if (ec < nc) nn.
left = new_branch;
else nn.
right = new_branch;
225 DistT ManhattanDistance(
const T &element, CoordT x, CoordT y)
const
227 return abs((DistT)this->
xyfunc(element, 0) - (DistT)x) +
abs((DistT)this->
xyfunc(element, 1) - (DistT)y);
235 if (a.second < b.second)
return a;
236 if (b.second < a.second)
return b;
237 if (a.first < b.first)
return a;
238 if (b.first < a.first)
return b;
247 const node &n = this->nodes[node_idx];
252 DistT thisdist = ManhattanDistance(n.
element, xy[0], xy[1]);
257 size_t next = (xy[dim] < c) ? n.
left : n.
right;
263 limit = std::min(best.second, limit);
267 size_t opposite = (xy[dim] >= c) ? n.
left : n.
right;
276 template <
typename Outputter>
277 void FindContainedRecursive(CoordT p1[2], CoordT p2[2],
size_t node_idx,
int level,
const Outputter &outputter)
const
282 const node &n = this->nodes[node_idx];
285 CoordT ec = this->
xyfunc(n.element, dim);
287 CoordT oc = this->
xyfunc(n.element, 1 - dim);
290 if (ec >= p1[dim] && ec < p2[dim] && oc >= p1[1 - dim] && oc < p2[1 - dim]) outputter(n.element);
293 if (p1[dim] < ec && n.left !=
INVALID_NODE) this->FindContainedRecursive(p1, p2, n.left, level + 1, outputter);
296 if (p2[dim] > ec && n.right !=
INVALID_NODE) this->FindContainedRecursive(p1, p2, n.right, level + 1, outputter);
303 const node &n = this->nodes[node_idx];
307 void IncrementUnbalanced(
size_t amount = 1)
309 this->unbalanced += amount;
315 size_t count = this->
Count();
316 if (count < 8)
return false;
317 return this->unbalanced > this->
Count() / 4;
321 void CheckInvariant(
size_t node_idx,
int level, CoordT min_x, CoordT max_x, CoordT min_y, CoordT max_y)
325 const node &n = this->nodes[node_idx];
334 if (level % 2 == 0) {
349 CheckInvariant(this->root, 0, std::numeric_limits<CoordT>::min(), std::numeric_limits<CoordT>::max(), std::numeric_limits<CoordT>::min(), std::numeric_limits<CoordT>::max());
363 template <
typename It>
367 this->free_list.clear();
368 this->unbalanced = 0;
369 if (begin == end)
return;
370 this->nodes.reserve(end - begin);
382 this->free_list.clear();
383 this->unbalanced = 0;
392 this->
Rebuild(
nullptr,
nullptr);
402 if (this->
Count() == 0) {
403 this->root = this->
AddNode(element);
407 this->IncrementUnbalanced();
421 size_t count = this->
Count();
422 if (count == 0)
return;
426 this->IncrementUnbalanced();
434 assert(this->free_list.size() <= this->nodes.size());
435 return this->nodes.size() - this->free_list.size();
445 assert(this->
Count() > 0);
447 CoordT xy[2] = { x, y };
460 template <
typename Outputter>
461 void FindContained(CoordT x1, CoordT y1, CoordT x2, CoordT y2,
const Outputter &outputter)
const
466 if (this->
Count() == 0)
return;
468 CoordT p1[2] = { x1, y1 };
469 CoordT p2[2] = { x2, y2 };
470 this->FindContainedRecursive(p1, p2, this->root, 0, outputter);
477 std::vector<T>
FindContained(CoordT x1, CoordT y1, CoordT x2, CoordT y2)
const
479 std::vector<T> result;
480 this->
FindContained(x1, y1, x2, y2, [&result](T e) {result.push_back(e); });