187 : points_(points), indices_(indices), leaf_size_(leaf_size), allocator_(allocator)
193 const int size = int(indices_.size());
198 if (size <= leaf_size_) {
199 return this->build_single_leaf(indices, r_bounds);
205 Node *root =
nullptr;
208 [&]() { root = this->build_nodes(indices, r_bounds,
true); });
211 root = this->build_nodes(indices, r_bounds,
false);
224 std::iota(indices_.
begin(), indices_.
end(), 0);
226 else if (indices.
data() != indices_.
data()) {
230 r_bounds =
Bounds<T>(points_[indices_.first()]);
231 for (
const int index : indices_) {
235 Node *root = allocator_.allocate<Node>();
236 root->children =
nullptr;
237 root->
leaf.start = 0;
238 root->
leaf.end = int(indices_.size());
242 Node *build_nodes(
const Span<int> indices, Bounds<T> &r_bounds,
const bool threaded)
244 const int size = int(indices_.size());
245 Array<BuildPoint, 256> build_points(size);
247 build_points_ = build_points.as_mutable_span();
248 const Bounds<BuildPoint> bounds = this->init_points(indices, threaded);
251 pool_.allocator = &allocator_;
253 thread_allocators_.emplace();
255 NodeAllocator &allocator = threaded ? thread_allocators_->local() : pool_;
256 Node *root = allocator.allocate(1);
257 this->build_node(root, 0, size, bounds, allocator, threaded);
261 if (thread_allocators_.has_value()) {
262 for (NodeAllocator &allocator : *thread_allocators_) {
263 allocator_.transfer_ownership_from(allocator.owned_allocator);
268 r_bounds.min[dim] = bounds.min[dim];
269 r_bounds.max[dim] = bounds.max[dim];
274 this->collect_indices(threaded);
284 Bounds<BuildPoint> init_points(
const Span<int> indices,
const bool threaded)
286 const int size = int(indices_.size());
287 const auto fill = [&](
const int start,
const int end) {
288 Bounds<BuildPoint> bounds(this->build_point(indices, start));
289 for (
const int i :
IndexRange(start, end - start)) {
290 const BuildPoint point = this->build_point(indices, i);
291 build_points_[i] = point;
292 bounds.min =
math::min(bounds.min, point);
293 bounds.max =
math::max(bounds.max, point);
298 const int blocks = threaded ? block_num(size) : 0;
300 return fill(0, size);
305 for (
const int block : range) {
306 block_bounds[block] =
fill(block_begin(size, blocks, block),
307 block_begin(size, blocks, block + 1));
310 return merge_bounds(block_bounds);
313 BuildPoint build_point(
const Span<int> indices,
const int i)
const
318 point[dim] = points_[
index][dim];
320 point[
dimensions] = std::bit_cast<value_type>(index);
325 void collect_indices(
const bool threaded)
327 const int size = int(indices_.size());
328 const auto fill = [&](
const int start,
const int end) {
329 for (
const int i :
IndexRange(start, end - start)) {
330 indices_[i] = std::bit_cast<int>(build_points_[i][
dimensions]);
334 const int blocks = threaded ? block_num(size) : 0;
340 for (
const int block : range) {
341 fill(block_begin(size, blocks, block), block_begin(size, blocks, block + 1));
351 void build_node(Node *
node,
354 const Bounds<BuildPoint> &bounds,
355 NodeAllocator &allocator,
358 if (end - start <= leaf_size_) {
359 node->children =
nullptr;
360 node->leaf.start = start;
361 node->leaf.end = end;
366 const BuildPoint extent = bounds.max - bounds.min;
369 if (extent[other] > extent[dim]) {
373 const value_type split_value = bounds.min[dim] + extent[dim] / 2;
381 int middle = split_blocks ? this->partition_points_in_blocks(start, end, dim, split_value) :
382 partition_points(build_points_, start, end, dim, split_value);
383 if (middle == start || middle == end) {
387 middle = start + (end - start) / 2;
390 Bounds<BuildPoint> left_bounds, right_bounds;
392 left_bounds = this->compute_bounds_in_blocks(start, middle);
393 right_bounds = this->compute_bounds_in_blocks(middle, end);
396 left_bounds = compute_bounds(build_points_, start, middle);
397 right_bounds = compute_bounds(build_points_, middle, end);
401 node->children = allocator.allocate(2);
406 &
node->children[0], start, middle, left_bounds, thread_allocators_->local(),
true);
410 &
node->children[1], middle, end, right_bounds, thread_allocators_->local(),
true);
414 this->build_node(&
node->children[0], start, middle, left_bounds, allocator,
false);
415 this->build_node(&
node->children[1], middle, end, right_bounds, allocator,
false);
418 node->split.low = left_bounds.max[dim];
419 node->split.high = right_bounds.min[dim];
428 static Bounds<BuildPoint> compute_bounds(
const Span<BuildPoint> points,
438 Bounds<BuildPoint> compute_bounds_in_blocks(
const int start,
const int end)
440 const int count = end - start;
441 const int blocks = block_num(count);
443 return compute_bounds(build_points_, start, end);
448 for (
const int block : range) {
449 block_bounds[block] = compute_bounds(build_points_,
450 start + block_begin(count, blocks, block),
451 start + block_begin(count, blocks, block + 1));
454 return merge_bounds(block_bounds);
457 static Bounds<BuildPoint> merge_bounds(
const Span<Bounds<BuildPoint>> block_bounds)
459 Bounds<BuildPoint> bounds = block_bounds.first();
460 for (
const Bounds<BuildPoint> &other : block_bounds.drop_front(1)) {
461 bounds.min =
math::min(bounds.min, other.min);
462 bounds.max =
math::max(bounds.max, other.max);
483 static int partition_points(
const MutableSpan<BuildPoint> points,
491 uint8_t offsets_l[block], offsets_r[block];
492 int num_l = 0, num_r = 0, first_l = 0, first_r = 0;
493 int l = start, r = end;
495 while (r -
l > 2 * block) {
502 num_l += int(!(points[
l + i][dim] < split_value));
509 num_r += int(points[r - 1 - i][dim] < split_value);
513 const int num = std::min(num_l, num_r);
515 std::swap(points[
l + offsets_l[first_l + i]], points[r - 1 - offsets_r[first_r + i]]);
531 return partition_points_small(points,
l, r, dim, split_value);
539 static int partition_points_small(
const MutableSpan<BuildPoint> points,
545 const int count = end - start;
548 std::array<BuildPoint, 2 * PARTITION_BLOCK_SIZE> buffer;
551 int l = 0, r = count;
552 for (
const int i :
IndexRange(start, count)) {
556 const bool less = points[i][dim] < split_value;
557 const BuildPoint point = points[i];
562 buffer[r - 1] = point;
567 MutableSpan<BuildPoint> range = points.slice(start, count);
569 range[i] = buffer[i];
585 int partition_points_in_blocks(
const int start,
590 const MutableSpan<BuildPoint> points = build_points_;
591 const int count = end - start;
592 const int blocks = block_num(count);
594 return partition_points(points, start, end, dim, split_value);
599 for (
const int block :
IndexRange(blocks + 1)) {
600 begin[block] = start + block_begin(count, blocks, block);
604 for (
const int block : range) {
605 middle[block] = partition_points(points,
begin[block],
begin[block + 1], dim, split_value);
618 int high_num = 0, low_num = 0;
620 if (middle[block] <
split) {
621 const int last = std::min(
begin[block + 1],
split);
622 if (middle[block] < last) {
626 else if (middle[block] >
split) {
627 const int first = std::max(
begin[block],
split);
628 if (first < middle[block]) {
643 for (
int h = 0,
l = 0, h_done = 0, l_done = 0; h < high_num &&
l < low_num;) {
644 const int num = std::min(
int(high[h].
size()) - h_done,
int(low[
l].
size()) - l_done);
645 swaps[swaps_num++] = {int(high[h].start()) + h_done, int(low[
l].start()) + l_done,
num};
648 if (h_done ==
int(high[h].
size())) {
652 if (l_done ==
int(low[
l].
size())) {
659 for (
const int i : range) {
662 std::swap(points[
swap.from + offset], points[
swap.to + offset]);
670 static int block_num(
const int count)
675 static int block_begin(
const int count,
const int blocks,
const int block)
677 return int(
int64_t(count) * block / blocks);
690void KDTreeNew<T>::build(
const Span<T> points,
697 points_ = points.
data();
705 mask->to_indices<
int>(tree_indices);
706 source = tree_indices;
712 TreeBuilder<T> builder(points, tree_indices, leaf_size, allocator);
713 root_ = builder.build(source, bounds_);
714 tree_indices_ = tree_indices.data();
720 for (
const int index : tree_indices) {
736 const int64_t alignment_slack = 2 *
alignof(std::max_align_t);
737 const int64_t rest = indices_size + nodes_size + alignment_slack;
751 if (owned_buffer_ !=
nullptr) {