Blender V5.3
BLI_kdtree_new.hh
Go to the documentation of this file.
1/* SPDX-FileCopyrightText: 2008-2009 Marius Muja, David G. Lowe.
2 * SPDX-FileCopyrightText: 2011-2026 Jose Luis Blanco-Claraco.
3 * SPDX-FileCopyrightText: 2026 Blender Authors.
4 *
5 * SPDX-License-Identifier: BSD-2-Clause */
6
7#pragma once
8
9#include <algorithm>
10#include <concepts>
11#include <limits>
12
13#include "BLI_bounds_types.hh"
14#include "BLI_index_mask_fwd.hh"
16#include "BLI_math_vector.hh"
17#include "BLI_span.hh"
18#include "BLI_utility_mixins.hh"
19#include "BLI_vector.hh"
20
21namespace blender {
22
23namespace kdtree {
24
26template<typename ValueT> struct Node {
31 union {
33 struct {
34 int start;
35 int end;
36 } leaf;
37
41 struct {
42 ValueT low;
43 ValueT high;
44 } split;
45 };
46
47 int dim;
48};
49
51template<typename Filter>
52concept FilterFn = std::predicate<Filter, int>;
53
58template<typename Query, typename ValueT>
59concept QueryType = requires(Query &query, const ValueT distance_sq, const int index) {
60 {
62 Query::inclusive
63 } -> std::convertible_to<bool>;
64 {
66 query.add_point(distance_sq, index)
67 } -> std::convertible_to<bool>;
68 {
70 query.max_distance_sq()
71 } -> std::convertible_to<ValueT>;
72};
73
74} // namespace kdtree
75
89template<typename T> class KDTreeNew : NonMovable, NonCopyable {
90 public:
91 using value_type = typename T::base_type;
92 static constexpr int dimensions = T::type_length;
93
101
110 static constexpr int LEAF_SIZE_DEFAULT = 32;
111
112 private:
113 using Node = kdtree::Node<value_type>;
114
120 const T *points_ = nullptr;
125 const int *tree_indices_ = nullptr;
127 Bounds<T> bounds_ = Bounds<T>(T(0));
128 Node *root_ = nullptr;
135 void *owned_buffer_ = nullptr;
136 int size_ = 0;
137
143 static LinearAllocator<> &make_allocator(int points_num, int leaf_size, void *&r_owned);
144
145 public:
150 explicit KDTreeNew(Span<T> points, int leaf_size = LEAF_SIZE_DEFAULT);
151
158
165
173 const IndexMask &mask,
174 LinearAllocator<> &allocator,
175 int leaf_size = LEAF_SIZE_DEFAULT);
177 Span<int> indices,
178 LinearAllocator<> &allocator,
179 int leaf_size = LEAF_SIZE_DEFAULT);
180
182
184 int size() const;
185
186 bool is_empty() const;
187
192 const T *points() const;
193
203
208 int find_nearest(const T &position, value_type *r_distance_sq = nullptr) const;
209
218 template<typename Filter>
220 Filter &&filter,
221 value_type *r_distance_sq = nullptr) const
222 requires kdtree::FilterFn<Filter>;
223
229 int find_nearest_n(const T &position, MutableSpan<Nearest> r_nearest) const;
230
232 template<typename Fn>
233 void foreach_in_radius(const T &position, const value_type radius, Fn &&fn) const;
234
241 int find_in_radius(const T &position, const value_type radius, Vector<Nearest> &r_nearest) const;
242
244 int count_in_radius(const T &position, const value_type radius) const;
245
251 template<typename Query>
252 void search(Query &query, const T &position) const
253 requires kdtree::QueryType<Query, value_type>;
254
255 private:
260 void build(Span<T> points,
261 Span<int> indices,
262 const IndexMask *mask,
263 int size,
264 int leaf_size,
265 LinearAllocator<> &allocator);
266
272 template<typename Query>
273 bool search_node(Query &query,
274 const T &position,
275 const Node *node,
276 const value_type distance_sq,
277 T &offsets) const
278 requires kdtree::QueryType<Query, value_type>;
279};
280
281namespace kdtree {
282
284template<typename ValueT> class NearestQuery {
285 private:
286 int index_ = -1;
287 ValueT distance_sq_ = std::numeric_limits<ValueT>::max();
288
289 public:
290 static constexpr bool inclusive = false;
291
292 int index() const
293 {
294 return index_;
295 }
296 ValueT distance_sq() const
297 {
298 return distance_sq_;
299 }
300
301 bool add_point(const ValueT distance_sq, const int index)
302 {
303 distance_sq_ = distance_sq;
304 index_ = index;
305 return true;
306 }
307
308 ValueT max_distance_sq() const
309 {
310 return distance_sq_;
311 }
312};
313
315template<typename ValueT, typename Filter> class FilteredNearestQuery {
316 private:
317 Filter filter_;
318 int index_ = -1;
319 ValueT distance_sq_ = std::numeric_limits<ValueT>::max();
320
321 public:
322 static constexpr bool inclusive = false;
323
324 explicit FilteredNearestQuery(Filter filter) : filter_(filter) {}
325
326 int index() const
327 {
328 return index_;
329 }
330 ValueT distance_sq() const
331 {
332 return distance_sq_;
333 }
334
335 bool add_point(const ValueT distance_sq, const int index)
336 {
337 if (filter_(index)) {
338 distance_sq_ = distance_sq;
339 index_ = index;
340 }
341 return true;
342 }
343
344 ValueT max_distance_sq() const
345 {
346 return distance_sq_;
347 }
348};
349
351template<typename NearestT, typename ValueT> class NearestNQuery {
352 private:
353 MutableSpan<NearestT> nearest_;
354 int count_ = 0;
355
356 public:
357 static constexpr bool inclusive = false;
358
359 explicit NearestNQuery(MutableSpan<NearestT> nearest) : nearest_(nearest) {}
360
361 int count() const
362 {
363 return count_;
364 }
365
366 bool add_point(const ValueT distance_sq, const int index)
367 {
368 /* Insert the new index at the sorted point in the list. */
369 const int capacity = int(nearest_.size());
370 int i;
371 for (i = count_; i > 0; i--) {
372 if (nearest_[i - 1].distance_sq <= distance_sq) {
373 break;
374 }
375 if (i < capacity) {
376 nearest_[i] = nearest_[i - 1];
377 }
378 }
379 if (i < capacity) {
380 nearest_[i] = {index, distance_sq};
381 }
382 if (count_ < capacity) {
383 count_++;
384 }
385 return true;
386 }
387
388 ValueT max_distance_sq() const
389 {
390 if (count_ < int(nearest_.size())) {
391 return std::numeric_limits<ValueT>::max();
392 }
393 return nearest_[count_ - 1].distance_sq;
394 }
395};
396
401template<typename ValueT, typename Fn> class RadiusQuery {
402 private:
403 ValueT radius_sq_;
404 Fn fn_;
405 int count_ = 0;
406
407 public:
408 static constexpr bool inclusive = true;
409
410 RadiusQuery(const ValueT radius_sq, Fn fn) : radius_sq_(radius_sq), fn_(fn) {}
411
412 int count() const
413 {
414 return count_;
415 }
416
417 bool add_point(const ValueT distance_sq, const int index)
418 {
419 count_++;
420 fn_(index, distance_sq);
421 return true;
422 }
423
425 ValueT max_distance_sq() const
426 {
427 return radius_sq_;
428 }
429};
430
432template<typename Query, typename ValueT>
433inline bool keep_distance(const ValueT distance_sq, const ValueT max_distance_sq)
434{
435 if constexpr (Query::inclusive) {
436 return distance_sq <= max_distance_sq;
437 }
438 else {
439 return distance_sq < max_distance_sq;
440 }
441}
442
465template<typename T>
466int calc_duplicates(const KDTreeNew<T> &tree,
467 typename T::base_type range,
468 Span<int> visit_order,
469 MutableSpan<int> duplicates);
470template<typename T>
471int calc_duplicates(const KDTreeNew<T> &tree,
472 typename T::base_type range,
473 const IndexMask &visit_order,
474 MutableSpan<int> duplicates);
475
483template<typename T>
484inline int calc_duplicates(const KDTreeNew<T> &tree,
485 const typename T::base_type range,
486 const MutableSpan<int> duplicates)
487{
488 return calc_duplicates(tree, range, tree.tree_indices(), duplicates);
489}
490
491} // namespace kdtree
492
493template<typename T> int KDTreeNew<T>::size() const
494{
495 return size_;
496}
497
498template<typename T> bool KDTreeNew<T>::is_empty() const
499{
500 return size_ == 0;
501}
502
503template<typename T> const T *KDTreeNew<T>::points() const
504{
505 return points_;
506}
507
508template<typename T> Span<int> KDTreeNew<T>::tree_indices() const
509{
510 return Span<int>(tree_indices_, size_);
511}
512
513template<typename T>
514int KDTreeNew<T>::find_nearest(const T &position, value_type *r_distance_sq) const
515{
516 if (this->is_empty()) {
517 return -1;
518 }
520 this->search(query, position);
521 if (query.index() != -1 && r_distance_sq) {
522 *r_distance_sq = query.distance_sq();
523 }
524 return query.index();
525}
526
527template<typename T>
528template<typename Filter>
530 Filter &&filter,
531 value_type *r_distance_sq) const
533{
534 if (this->is_empty()) {
535 return -1;
536 }
538 std::forward<Filter>(filter));
539 this->search(query, position);
540 if (query.index() != -1 && r_distance_sq) {
541 *r_distance_sq = query.distance_sq();
542 }
543 return query.index();
544}
545
546template<typename T>
548{
549 if (this->is_empty() || r_nearest.is_empty()) {
550 return 0;
551 }
553 this->search(query, position);
554 return query.count();
555}
556
557template<typename T>
558template<typename Fn>
559void KDTreeNew<T>::foreach_in_radius(const T &position, const value_type radius, Fn &&fn) const
560{
561 if (this->is_empty()) {
562 return;
563 }
564 kdtree::RadiusQuery<value_type, std::decay_t<Fn>> query(radius * radius, std::forward<Fn>(fn));
565 this->search(query, position);
566}
567
568template<typename T>
570 const value_type radius,
571 Vector<Nearest> &r_nearest) const
572{
573 r_nearest.clear();
574 this->foreach_in_radius(position, radius, [&](const int index, const value_type distance_sq) {
575 r_nearest.append({index, distance_sq});
576 });
577 std::sort(r_nearest.begin(), r_nearest.end(), [](const Nearest &a, const Nearest &b) {
578 return a.distance_sq < b.distance_sq || (a.distance_sq == b.distance_sq && a.index < b.index);
579 });
580 return int(r_nearest.size());
581}
582
583template<typename T>
584int KDTreeNew<T>::count_in_radius(const T &position, const value_type radius) const
585{
586 if (this->is_empty()) {
587 return 0;
588 }
589 kdtree::RadiusQuery<value_type, decltype([](int, value_type) {})> query(radius * radius, {});
590 this->search(query, position);
591 return query.count();
592}
593
594template<typename T>
595template<typename Query>
596void KDTreeNew<T>::search(Query &query, const T &position) const
598{
599 BLI_assert(!this->is_empty());
600 /* Squared distance to the bounds of the tree, per dimension, which the walk below replaces
601 * one dimension at a time rather than recomputing. */
602 T offsets(value_type(0));
603 value_type distance_sq = 0;
604 for (const int dim : IndexRange(dimensions)) {
605 if (position[dim] < bounds_.min[dim]) {
606 offsets[dim] = math::square(bounds_.min[dim] - position[dim]);
607 }
608 else if (position[dim] > bounds_.max[dim]) {
609 offsets[dim] = math::square(position[dim] - bounds_.max[dim]);
610 }
611 distance_sq += offsets[dim];
612 }
613 this->search_node(query, position, root_, distance_sq, offsets);
614}
615
616template<typename T>
617template<typename Query>
618bool KDTreeNew<T>::search_node(Query &query,
619 const T &position,
620 const Node *node,
621 const value_type distance_sq,
622 T &offsets) const
624{
625 if (node->children == nullptr) {
626 const int *indices = tree_indices_;
627 const T *points = points_;
628 for (int i = node->leaf.start; i < node->leaf.end; i++) {
629 const int index = indices[i];
630 const value_type point_distance_sq = math::distance_squared(position, points[index]);
631 if (kdtree::keep_distance<Query>(point_distance_sq, query.max_distance_sq())) {
632 if (!query.add_point(point_distance_sq, index)) {
633 return false;
634 }
635 }
636 }
637 return true;
638 }
639
640 /* Both children are loaded before it is known which one to visit, so that the loads do not
641 * wait on the comparison below. Picking one first makes the walk down the tree a chain of
642 * dependent loads. */
643 const Node *low_child = &node->children[0];
644 const Node *high_child = &node->children[1];
645
646 /* Visit the side of the split the position is on first, since what it finds there is what
647 * lets the other side be skipped. */
648 const int dim = node->dim;
649 const value_type value = position[dim];
650 const value_type to_low = value - node->split.low;
651 const value_type to_high = value - node->split.high;
652
653 /* Written as a branch rather than as a choice between two pointers, so that the processor
654 * predicts which child comes next and starts loading it before the comparison has resolved.
655 * Selecting the pointer instead makes the walk down the tree wait at every level. */
656 const Node *far_child;
657 value_type far_offset;
658 if ((to_low + to_high) < 0) {
659 if (!this->search_node(query, position, low_child, distance_sq, offsets)) {
660 return false;
661 }
662 far_child = high_child;
663 far_offset = math::square(to_high);
664 }
665 else {
666 if (!this->search_node(query, position, high_child, distance_sq, offsets)) {
667 return false;
668 }
669 far_child = low_child;
670 far_offset = math::square(to_low);
671 }
672
673 /* Swap in the distance to the far side for this dimension, keeping the others. */
674 const value_type previous = offsets[dim];
675 const value_type far_distance_sq = distance_sq + far_offset - previous;
676 if (!kdtree::keep_distance<Query>(far_distance_sq, query.max_distance_sq())) {
677 return true;
678 }
679 offsets[dim] = far_offset;
680 const bool keep_searching = this->search_node(
681 query, position, far_child, far_distance_sq, offsets);
682 offsets[dim] = previous;
683 return keep_searching;
684}
685
686} // namespace blender
constexpr IndexRange()=default
KDTreeNew(Span< T > points, const IndexMask &mask, LinearAllocator<> &allocator, int leaf_size=LEAF_SIZE_DEFAULT)
Definition kdtree.cc:764
int find_in_radius(const float3 &position, const value_type radius, Vector< Nearest > &r_nearest) const
void foreach_in_radius(const float3 &position, const value_type radius, Fn &&fn) const
int find_nearest(const T &position, value_type *r_distance_sq=nullptr) const
KDTreeNew(Span< T > points, LinearAllocator<> &allocator, int leaf_size=LEAF_SIZE_DEFAULT)
Definition kdtree.cc:758
KDTreeNew(Span< T > points, Span< int > indices, int leaf_size=LEAF_SIZE_DEFAULT)
Definition kdtree.cc:813
int count_in_radius(const float3 &position, const value_type radius) const
KDTreeNew(Span< T > points, Span< int > indices, LinearAllocator<> &allocator, int leaf_size=LEAF_SIZE_DEFAULT)
Definition kdtree.cc:781
Span< int > tree_indices() const
int find_nearest_filtered(const T &position, Filter &&filter, value_type *r_distance_sq=nullptr) const
KDTreeNew(Span< T > points, const IndexMask &mask, int leaf_size=LEAF_SIZE_DEFAULT)
Definition kdtree.cc:800
static constexpr int LEAF_SIZE_DEFAULT
const float3 * points() const
int find_nearest_n(const float3 &position, MutableSpan< Nearest > r_nearest) const
typename T::base_type value_type
static constexpr int dimensions
void search(Query &query, const float3 &position) const
KDTreeNew(Span< T > points, int leaf_size=LEAF_SIZE_DEFAULT)
Definition kdtree.cc:789
constexpr bool is_empty() const
Definition BLI_span.hh:515
int64_t size() const
void append(const T &value)
NonCopyable(const NonCopyable &other)=delete
NonMovable(NonMovable &&other)=delete
bool add_point(const ValueT distance_sq, const int index)
NearestNQuery(MutableSpan< NearestT > nearest)
static constexpr bool inclusive
bool add_point(const ValueT distance_sq, const int index)
bool add_point(const ValueT distance_sq, const int index)
static constexpr bool inclusive
bool add_point(const ValueT distance_sq, const int index)
static constexpr bool inclusive
RadiusQuery(const ValueT radius_sq, Fn fn)
#define position
#define capacity(const)
#define T
int calc_duplicates(const KDTreeNew< T > &tree, typename T::base_type range, Span< int > visit_order, MutableSpan< int > duplicates)
Definition kdtree.cc:867
bool keep_distance(const ValueT distance_sq, const ValueT max_distance_sq)
T square(const T &a)
T distance_squared(const VecBase< T, Size > &a, const VecBase< T, Size > &b)
BMesh Iterators.
BLI_assert(slot->slot_subtype.map==BMO_OP_SLOT_SUBTYPE_MAP_FLT)
Bounds()=default
struct blender::kdtree::Node::@136202335367076324045241233256012145133315376374::@052000000061374247324310024311347011327123106314 leaf
struct blender::kdtree::Node::@136202335367076324045241233256012145133315376374::@006376166267370336061007055145172370271056263274 split