29 #define Magnitude(X) ((X) < 0 ? -(X) : (X))
30 #define NodeFound(N,K,D) (((N)->Key == (K)) && ((N)->Data == (D)))
35 #define MINSEARCH -FLT_MAX
36 #define MAXSEARCH FLT_MAX
39 static int NextLevel(
KDTREE *tree,
int level) {
50 template<
typename Key,
typename Value>
53 MinK(Key max_key,
int k);
64 bool insert(Key k, Value v);
68 const Element*
elements() {
return elements_; }
78 template<
typename Key,
typename Value>
80 max_key_(max_key), elements_count_(0), k_(k < 1 ? 1 : k), max_index_(0) {
81 elements_ =
new Element[k_];
84 template<
typename Key,
typename Value>
89 template<
typename Key,
typename Value>
91 if (elements_count_ < k_)
93 return elements_[max_index_].key;
96 template<
typename Key,
typename Value>
98 if (elements_count_ < k_) {
99 elements_[elements_count_++] = Element(key, value);
100 if (key > elements_[max_index_].key)
101 max_index_ = elements_count_ - 1;
103 }
else if (key < elements_[max_index_].key) {
105 elements_[max_index_] = Element(key, value);
107 for (
int i = 0; i < elements_count_; i++) {
108 if (elements_[i].key > elements_[max_index_].key)
126 void Search(
int *result_count,
float *distances,
void **results);
129 void SearchRec(
int Level,
KDNODE *SubTree);
130 bool BoxIntersectsSearch(
float *lower,
float *upper);
140 : tree_(tree), query_point_(query_point), results_(
MAXSEARCH, k_closest) {
141 sb_min_ =
new float[tree->
KeySize];
142 sb_max_ =
new float[tree->
KeySize];
158 for (
int i = 0; i < tree_->
KeySize; i++) {
164 *result_count =
count;
165 for (
int j = 0; j <
count; j++) {
168 distances[j] = static_cast<float>(sqrt(static_cast<double>(results_.
elements()[j].key)));
169 results[j] = results_.
elements()[j].value;
181 auto *KDTree = static_cast<KDTREE *>(
Emalloc(
183 for (
int i = 0; i < KeySize; i++) {
184 KDTree->KeyDesc[i].NonEssential = KeyDesc[i].
NonEssential;
185 KDTree->KeyDesc[i].Circular = KeyDesc[i].
Circular;
186 if (KeyDesc[i].Circular) {
187 KDTree->KeyDesc[i].Min = KeyDesc[i].
Min;
188 KDTree->KeyDesc[i].Max = KeyDesc[i].
Max;
189 KDTree->KeyDesc[i].Range = KeyDesc[i].
Max - KeyDesc[i].
Min;
190 KDTree->KeyDesc[i].HalfRange = KDTree->KeyDesc[i].Range / 2;
191 KDTree->KeyDesc[i].MidRange = (KeyDesc[i].
Max + KeyDesc[i].
Min) / 2;
197 KDTree->KeySize = KeySize;
198 KDTree->Root.Left =
nullptr;
199 KDTree->Root.Right =
nullptr;
219 Level = NextLevel(Tree, -1);
220 while (Node !=
nullptr) {
222 PtrToNode = &(Node->
Left);
227 PtrToNode = &(Node->
Right);
231 Level = NextLevel(Tree, Level);
235 *PtrToNode =
MakeKDNode(Tree, Key, Data, Level);
259 Father = &(Tree->
Root);
260 Current = Father->
Left;
261 Level = NextLevel(Tree, -1);
264 while ((Current !=
nullptr) && (!
NodeFound (Current, Key, Data))) {
267 Current = Current->
Left;
269 Current = Current->
Right;
271 Level = NextLevel(Tree, Level);
274 if (Current !=
nullptr) {
275 if (Current == Father->
Left) {
276 Father->
Left =
nullptr;
279 Father->
Right =
nullptr;
306 KDTREE *Tree,
float Query[],
int QuerySize,
float MaxDistance,
307 int *NumberOfResults,
void **NBuffer,
float DBuffer[]) {
309 search.Search(NumberOfResults, DBuffer, NBuffer);
358 NewNode->
Data = Data;
362 NewNode->
Left =
nullptr;
363 NewNode->
Right =
nullptr;
378 void KDTreeSearch::SearchRec(
int level,
KDNODE *sub_tree) {
382 if (!BoxIntersectsSearch(sb_min_, sb_max_))
390 if (sub_tree->
Left !=
nullptr) {
391 float tmp = sb_max_[level];
393 SearchRec(NextLevel(tree_, level), sub_tree->
Left);
394 sb_max_[level] = tmp;
396 if (sub_tree->
Right !=
nullptr) {
397 float tmp = sb_min_[level];
399 SearchRec(NextLevel(tree_, level), sub_tree->
Right);
400 sb_min_[level] = tmp;
403 if (sub_tree->
Right !=
nullptr) {
404 float tmp = sb_min_[level];
406 SearchRec(NextLevel(tree_, level), sub_tree->
Right);
407 sb_min_[level] = tmp;
409 if (sub_tree->
Left !=
nullptr) {
410 float tmp = sb_max_[level];
412 SearchRec(NextLevel(tree_, level), sub_tree->
Left);
413 sb_max_[level] = tmp;
428 float total_distance = 0;
430 for (; k > 0; k--, p1++, p2++, dim++) {
434 float dimension_distance = *p1 - *p2;
438 dimension_distance =
Magnitude(dimension_distance);
439 float wrap_distance = dim->
Max - dim->
Min - dimension_distance;
440 dimension_distance = std::min(dimension_distance, wrap_distance);
443 total_distance += dimension_distance * dimension_distance;
445 return total_distance;
457 bool KDTreeSearch::BoxIntersectsSearch(
float *lower,
float *upper) {
458 float *query = query_point_;
460 double total_distance = 0.0;
465 for (
int i = tree_->
KeySize; i > 0; i--, dim++, query++, lower++, upper++) {
469 float dimension_distance;
471 dimension_distance = *lower - *query;
472 else if (*query > *upper)
473 dimension_distance = *query - *upper;
475 dimension_distance = 0;
479 float wrap_distance = FLT_MAX;
481 wrap_distance = *query + dim->
Max - dim->
Min - *upper;
482 else if (*query > *upper)
483 wrap_distance = *lower - (*query - (dim->
Max - dim->
Min));
484 dimension_distance = std::min(dimension_distance, wrap_distance);
488 static_cast<double>(dimension_distance) * dimension_distance;
489 if (total_distance >= radius_squared)
513 KDNODE *sub_tree, int32_t level) {
514 (*action)(context, sub_tree->
Data, level);
515 if (sub_tree->
Left !=
nullptr)
516 Walk(tree,
action, context, sub_tree->
Left, NextLevel(tree, level));
517 if (sub_tree->
Right !=
nullptr)
523 if (nodes ==
nullptr)
533 if (sub_tree !=
nullptr) {