30 #define Magnitude(X) ((X) < 0 ? -(X) : (X)) 31 #define NodeFound(N,K,D) (( (N)->Key == (K) ) && ( (N)->Data == (D) )) 36 #define MINSEARCH -MAX_FLOAT32 37 #define MAXSEARCH MAX_FLOAT32 40 static int NextLevel(
KDTREE *tree,
int level) {
51 template<
typename Key,
typename Value>
54 MinK(Key max_key,
int k);
79 template<
typename Key,
typename Value>
81 max_key_(max_key), elements_count_(0), k_(k < 1 ? 1 : k), max_index_(0) {
82 elements_ =
new Element[k_];
85 template<
typename Key,
typename Value>
90 template<
typename Key,
typename Value>
92 if (elements_count_ < k_)
94 return elements_[max_index_].
key;
97 template<
typename Key,
typename Value>
99 if (elements_count_ < k_) {
100 elements_[elements_count_++] = Element(key, value);
101 if (key > elements_[max_index_].key)
102 max_index_ = elements_count_ - 1;
104 }
else if (key < elements_[max_index_].key) {
106 elements_[max_index_] = Element(key, value);
108 for (
int i = 0; i < elements_count_; i++) {
109 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),
161 for (
int i = 0; i < tree_->
KeySize; i++) {
167 *result_count =
count;
168 for (
int j = 0; j <
count; j++) {
171 results[j] = results_.
elements()[j].value;
185 for (
int i = 0; i < KeySize; i++) {
188 if (KeyDesc[i].Circular) {
225 Level = NextLevel(Tree, -1);
226 while (Node != NULL) {
228 PtrToNode = &(Node->
Left);
233 PtrToNode = &(Node->
Right);
237 Level = NextLevel(Tree, Level);
241 *PtrToNode =
MakeKDNode(Tree, Key, (
void *) Data, Level);
270 Father = &(Tree->
Root);
271 Current = Father->
Left;
272 Level = NextLevel(Tree, -1);
275 while ((Current != NULL) && (!
NodeFound (Current, Key, Data))) {
278 Current = Current->
Left;
280 Current = Current->
Right;
282 Level = NextLevel(Tree, Level);
285 if (Current != NULL) {
286 if (Current == Father->
Left) {
290 Father->
Right = NULL;
322 int *NumberOfResults,
void **NBuffer,
FLOAT32 DBuffer[]) {
324 search.
Search(NumberOfResults, DBuffer, NBuffer);
332 Walk(Tree, action, context, Tree->
Root.
Left, NextLevel(Tree, -1));
378 NewNode->
Data = Data;
382 NewNode->
Left = NULL;
383 NewNode->
Right = NULL;
401 void KDTreeSearch::SearchRec(
int level,
KDNODE *sub_tree) {
405 if (!BoxIntersectsSearch(sb_min_, sb_max_))
409 query_point_, sub_tree->
Key),
413 if (sub_tree->
Left != NULL) {
416 SearchRec(NextLevel(tree_, level), sub_tree->
Left);
417 sb_max_[level] = tmp;
419 if (sub_tree->
Right != NULL) {
422 SearchRec(NextLevel(tree_, level), sub_tree->
Right);
423 sb_min_[level] = tmp;
426 if (sub_tree->
Right != NULL) {
429 SearchRec(NextLevel(tree_, level), sub_tree->
Right);
430 sb_min_[level] = tmp;
432 if (sub_tree->
Left != NULL) {
435 SearchRec(NextLevel(tree_, level), sub_tree->
Left);
436 sb_max_[level] = tmp;
453 for (; k > 0; k--, p1++, p2++, dim++) {
457 FLOAT32 dimension_distance = *p1 - *p2;
461 dimension_distance =
Magnitude(dimension_distance);
462 FLOAT32 wrap_distance = dim->
Max - dim->
Min - dimension_distance;
463 dimension_distance =
MIN(dimension_distance, wrap_distance);
466 total_distance += dimension_distance * dimension_distance;
468 return total_distance;
480 bool KDTreeSearch::BoxIntersectsSearch(
FLOAT32 *lower,
FLOAT32 *upper) {
488 for (
int i = tree_->
KeySize; i > 0; i--, dim++, query++, lower++, upper++) {
494 dimension_distance = *lower - *query;
495 else if (*query > *upper)
496 dimension_distance = *query - *upper;
498 dimension_distance = 0;
504 wrap_distance = *query + dim->
Max - dim->
Min - *upper;
505 else if (*query > *upper)
506 wrap_distance = *lower - (*query - (dim->
Max - dim->
Min));
507 dimension_distance =
MIN(dimension_distance, wrap_distance);
510 total_distance += dimension_distance * dimension_distance;
511 if (total_distance >= radius_squared)
536 (*action)(context, sub_tree->
Data, level);
537 if (sub_tree->
Left != NULL)
538 Walk(tree, action, context, sub_tree->
Left, NextLevel(tree, level));
539 if (sub_tree->
Right != NULL)
540 Walk(tree, action, context, sub_tree->
Right, NextLevel(tree, level));
555 if (sub_tree != NULL) {
void KDWalk(KDTREE *Tree, void_proc action, void *context)
void KDStore(KDTREE *Tree, FLOAT32 *Key, void *Data)
void Search(int *result_count, FLOAT32 *distances, void **results)
KDTREE * MakeKDTree(inT16 KeySize, const PARAM_DESC KeyDesc[])
KDTreeSearch(KDTREE *tree, FLOAT32 *query_point, int k_closest)
const Element * elements()
#define NodeFound(N, K, D)
void KDNearestNeighborSearch(KDTREE *Tree, FLOAT32 Query[], int QuerySize, FLOAT32 MaxDistance, int *NumberOfResults, void **NBuffer, FLOAT32 DBuffer[])
const Key & max_insertable_key()
bool insert(Key k, Value v)
Element(const Key &k, const Value &v)
LIST search(LIST list, void *key, int_compare is_equal)
void FreeKDNode(KDNODE *Node)
void Walk(KDTREE *tree, void_proc action, void *context, KDNODE *sub_tree, inT32 level)
void InsertNodes(KDTREE *tree, KDNODE *nodes)
void FreeSubTree(KDNODE *sub_tree)
FLOAT32 ComputeDistance(int k, PARAM_DESC *dim, FLOAT32 p1[], FLOAT32 p2[])
void KDDelete(KDTREE *Tree, FLOAT32 Key[], void *Data)
void FreeKDTree(KDTREE *Tree)
FLOAT32 DistanceSquared(int k, PARAM_DESC *dim, FLOAT32 p1[], FLOAT32 p2[])
KDNODE * MakeKDNode(KDTREE *tree, FLOAT32 Key[], void *Data, int Index)