31 #define Magnitude(X) ((X) < 0 ? -(X) : (X))
32 #define NodeFound(N,K,D) (( (N)->Key == (K) ) && ( (N)->Data == (D) ))
37 #define MINSEARCH -MAX_FLOAT32
38 #define MAXSEARCH MAX_FLOAT32
41 static int NextLevel(
KDTREE *tree,
int level) {
52 template<
typename Key,
typename Value>
55 MinK(Key max_key,
int k);
66 bool insert(Key k, Value v);
70 const Element*
elements() {
return elements_; }
80 template<
typename Key,
typename Value>
82 max_key_(max_key), elements_count_(0), k_(k < 1 ? 1 : k), max_index_(0) {
86 template<
typename Key,
typename Value>
91 template<
typename Key,
typename Value>
93 if (elements_count_ < k_)
95 return elements_[max_index_].key;
98 template<
typename Key,
typename Value>
100 if (elements_count_ < k_) {
101 elements_[elements_count_++] =
Element(key, value);
102 if (key > elements_[max_index_].key)
103 max_index_ = elements_count_ - 1;
105 }
else if (key < elements_[max_index_].key) {
107 elements_[max_index_] =
Element(key, value);
109 for (
int i = 0; i < elements_count_; i++) {
110 if (elements_[i].key > elements_[max_index_].key)
127 void Search(
int *result_count,
FLOAT32 *distances,
void **results);
130 void SearchRec(
int Level,
KDNODE *SubTree);
142 query_point_(query_point) {
162 for (
int i = 0; i < tree_->
KeySize; i++) {
168 *result_count =
count;
169 for (
int j = 0; j <
count; j++) {
171 results[j] = results_->
elements()[j].value;
187 for (
int i = 0; i < KeySize; i++) {
190 if (KeyDesc[i].Circular) {
228 Level = NextLevel(Tree, -1);
229 while (Node !=
NULL) {
231 PtrToNode = &(Node->
Left);
236 PtrToNode = &(Node->
Right);
240 Level = NextLevel(Tree, Level);
244 *PtrToNode =
MakeKDNode(Tree, Key, (
void *) Data, Level);
275 Father = &(Tree->
Root);
276 Current = Father->
Left;
277 Level = NextLevel(Tree, -1);
280 while ((Current !=
NULL) && (!
NodeFound (Current, Key, Data))) {
283 Current = Current->
Left;
285 Current = Current->
Right;
287 Level = NextLevel(Tree, Level);
290 if (Current !=
NULL) {
291 if (Current == Father->
Left) {
309 int *NumberOfResults,
void **NBuffer,
FLOAT32 DBuffer[]) {
333 search.
Search(NumberOfResults, DBuffer, NBuffer);
341 Walk(Tree, action, context, Tree->
Root.
Left, NextLevel(Tree, -1));
395 NewNode->
Data = Data;
417 void KDTreeSearch::SearchRec(
int level,
KDNODE *sub_tree) {
421 if (!BoxIntersectsSearch(sb_min_, sb_max_))
425 query_point_, sub_tree->
Key),
432 SearchRec(NextLevel(tree_, level), sub_tree->
Left);
433 sb_max_[level] = tmp;
438 SearchRec(NextLevel(tree_, level), sub_tree->
Right);
439 sb_min_[level] = tmp;
445 SearchRec(NextLevel(tree_, level), sub_tree->
Right);
446 sb_min_[level] = tmp;
451 SearchRec(NextLevel(tree_, level), sub_tree->
Left);
452 sb_max_[level] = tmp;
468 for (; k > 0; k--, p1++, p2++, dim++) {
472 FLOAT32 dimension_distance = *p1 - *p2;
476 dimension_distance =
Magnitude(dimension_distance);
477 FLOAT32 wrap_distance = dim->
Max - dim->
Min - dimension_distance;
478 dimension_distance =
MIN(dimension_distance, wrap_distance);
481 total_distance += dimension_distance * dimension_distance;
483 return total_distance;
495 bool KDTreeSearch::BoxIntersectsSearch(
FLOAT32 *lower,
FLOAT32 *upper) {
502 for (
int i = tree_->
KeySize; i > 0; i--, dim++, query++, lower++, upper++) {
508 dimension_distance = *lower - *query;
509 else if (*query > *upper)
510 dimension_distance = *query - *upper;
512 dimension_distance = 0;
518 wrap_distance = *query + dim->
Max - dim->
Min - *upper;
519 else if (*query > *upper)
520 wrap_distance = *lower - (*query - (dim->
Max - dim->
Min));
521 dimension_distance =
MIN(dimension_distance, wrap_distance);
524 total_distance += dimension_distance * dimension_distance;
525 if (total_distance >= radius_squared)
549 (*action)(context, sub_tree->
Data, level);
551 Walk(tree, action, context, sub_tree->
Left, NextLevel(tree, level));
553 Walk(tree, action, context, sub_tree->
Right, NextLevel(tree, level));
569 if (sub_tree !=
NULL) {