00001
00023
00024
00025
00026 #include "kdtree.h"
00027 #include "const.h"
00028 #include "emalloc.h"
00029 #include "freelist.h"
00030 #include <stdio.h>
00031 #include <math.h>
00032 #include <setjmp.h>
00033
00034 #define Magnitude(X) ((X) < 0 ? -(X) : (X))
00035 #define MIN(A,B) ((A) < (B) ? (A) : (B))
00036 #define NodeFound(N,K,D) (( (N)->Key == (K) ) && ( (N)->Data == (D) ))
00037
00038
00039
00040
00041 #define MINSEARCH -MAX_FLOAT32
00042 #define MAXSEARCH MAX_FLOAT32
00043
00044 static int NumberOfNeighbors;
00046 static INT16 N;
00047
00048 static FLOAT32 *QueryPoint;
00049 static int MaxNeighbors;
00050 static FLOAT32 Radius;
00051 static int Furthest;
00052 static char **Neighbor;
00053 static FLOAT32 *Distance;
00054
00055 static int MaxDimension = 0;
00056 static FLOAT32 *SBMin;
00057 static FLOAT32 *SBMax;
00058 static FLOAT32 *LBMin;
00059 static FLOAT32 *LBMax;
00060
00061 static PARAM_DESC *KeyDesc;
00062
00063 static jmp_buf QuickExit;
00064
00065 static void_proc WalkAction;
00066
00067
00068
00069
00070
00093 KDTREE *
00094 MakeKDTree (INT16 KeySize, PARAM_DESC KeyDesc[]) {
00095 int i;
00096 void *NewMemory;
00097 KDTREE *KDTree;
00098
00099 if (KeySize > MaxDimension) {
00100 NewMemory = Emalloc (KeySize * 4 * sizeof (FLOAT32));
00101 if (MaxDimension > 0) {
00102 memfree ((char *) SBMin);
00103 memfree ((char *) SBMax);
00104 memfree ((char *) LBMin);
00105 memfree ((char *) LBMax);
00106 }
00107 SBMin = (FLOAT32 *) NewMemory;
00108 SBMax = SBMin + KeySize;
00109 LBMin = SBMax + KeySize;
00110 LBMax = LBMin + KeySize;
00111 }
00112
00113 KDTree =
00114 (KDTREE *) Emalloc (sizeof (KDTREE) +
00115 (KeySize - 1) * sizeof (PARAM_DESC));
00116 for (i = 0; i < KeySize; i++) {
00117 KDTree->KeyDesc[i].NonEssential = KeyDesc[i].NonEssential;
00118 KDTree->KeyDesc[i].Circular = KeyDesc[i].Circular;
00119 if (KeyDesc[i].Circular) {
00120 KDTree->KeyDesc[i].Min = KeyDesc[i].Min;
00121 KDTree->KeyDesc[i].Max = KeyDesc[i].Max;
00122 KDTree->KeyDesc[i].Range = KeyDesc[i].Max - KeyDesc[i].Min;
00123 KDTree->KeyDesc[i].HalfRange = KDTree->KeyDesc[i].Range / 2;
00124 KDTree->KeyDesc[i].MidRange = (KeyDesc[i].Max + KeyDesc[i].Min) / 2;
00125 }
00126 else {
00127 KDTree->KeyDesc[i].Min = MINSEARCH;
00128 KDTree->KeyDesc[i].Max = MAXSEARCH;
00129 }
00130 }
00131 KDTree->KeySize = KeySize;
00132 KDTree->Root.Left = NULL;
00133 KDTree->Root.Right = NULL;
00134 return (KDTree);
00135 }
00136
00137
00138
00156 void KDStore(KDTREE *Tree, FLOAT32 *Key, void *Data) {
00157 int Level;
00158 KDNODE *Node;
00159 KDNODE **PtrToNode;
00160
00161 N = Tree->KeySize;
00162 KeyDesc = &(Tree->KeyDesc[0]);
00163 PtrToNode = &(Tree->Root.Left);
00164 Node = *PtrToNode;
00165 Level = 0;
00166 while (Node != NULL) {
00167 if (Key[Level] < Node->BranchPoint) {
00168 PtrToNode = &(Node->Left);
00169 if (Key[Level] > Node->LeftBranch)
00170 Node->LeftBranch = Key[Level];
00171 }
00172 else {
00173 PtrToNode = &(Node->Right);
00174 if (Key[Level] < Node->RightBranch)
00175 Node->RightBranch = Key[Level];
00176 }
00177 Level++;
00178 if (Level >= N)
00179 Level = 0;
00180 Node = *PtrToNode;
00181 }
00182
00183 *PtrToNode = MakeKDNode (Key, (char *) Data, Level);
00184 }
00185
00186
00187
00231 void
00232 KDDelete (KDTREE * Tree, FLOAT32 Key[], void *Data) {
00233 int Level;
00234 KDNODE *Current;
00235 KDNODE *Father;
00236 KDNODE *Replacement;
00237 KDNODE *FatherReplacement;
00238
00239
00240 N = Tree->KeySize;
00241 KeyDesc = &(Tree->KeyDesc[0]);
00242 Father = &(Tree->Root);
00243 Current = Father->Left;
00244 Level = 0;
00245
00246
00247 while ((Current != NULL) && (!NodeFound (Current, Key, Data))) {
00248 Father = Current;
00249 if (Key[Level] < Current->BranchPoint)
00250 Current = Current->Left;
00251 else
00252 Current = Current->Right;
00253
00254 Level++;
00255 if (Level >= N)
00256 Level = 0;
00257 }
00258
00259 if (Current != NULL) {
00260 Replacement = Current;
00261 FatherReplacement = Father;
00262
00263
00264 while (TRUE) {
00265 if (Replacement->Left != NULL) {
00266 FatherReplacement = Replacement;
00267 Replacement = Replacement->Left;
00268 }
00269 else if (Replacement->Right != NULL) {
00270 FatherReplacement = Replacement;
00271 Replacement = Replacement->Right;
00272 }
00273 else
00274 break;
00275
00276 Level++;
00277 if (Level >= N)
00278 Level = 0;
00279 }
00280
00281
00282 Level--;
00283 if (Level < 0)
00284 Level = N - 1;
00285
00286
00287 if (FatherReplacement->Left == Replacement) {
00288 FatherReplacement->Left = NULL;
00289 FatherReplacement->LeftBranch = KeyDesc[Level].Min;
00290 }
00291 else {
00292 FatherReplacement->Right = NULL;
00293 FatherReplacement->RightBranch = KeyDesc[Level].Max;
00294 }
00295
00296
00297 if (Replacement != Current) {
00298 Replacement->BranchPoint = Current->BranchPoint;
00299 Replacement->LeftBranch = Current->LeftBranch;
00300 Replacement->RightBranch = Current->RightBranch;
00301 Replacement->Left = Current->Left;
00302 Replacement->Right = Current->Right;
00303
00304 if (Father->Left == Current)
00305 Father->Left = Replacement;
00306 else
00307 Father->Right = Replacement;
00308 }
00309 FreeKDNode(Current);
00310 }
00311 }
00312
00313
00314
00350 int
00351 KDNearestNeighborSearch (KDTREE * Tree,
00352 FLOAT32 Query[],
00353 int QuerySize,
00354 FLOAT32 MaxDistance,
00355 void *NBuffer, FLOAT32 DBuffer[]) {
00356 int i;
00357
00358 NumberOfNeighbors = 0;
00359 N = Tree->KeySize;
00360 KeyDesc = &(Tree->KeyDesc[0]);
00361 QueryPoint = Query;
00362 MaxNeighbors = QuerySize;
00363 Radius = MaxDistance;
00364 Furthest = 0;
00365 Neighbor = (char **) NBuffer;
00366 Distance = DBuffer;
00367
00368 for (i = 0; i < N; i++) {
00369 SBMin[i] = KeyDesc[i].Min;
00370 SBMax[i] = KeyDesc[i].Max;
00371 LBMin[i] = KeyDesc[i].Min;
00372 LBMax[i] = KeyDesc[i].Max;
00373 }
00374
00375 if (Tree->Root.Left != NULL) {
00376 if (setjmp (QuickExit) == 0)
00377 Search (0, Tree->Root.Left);
00378 }
00379 return (NumberOfNeighbors);
00380 }
00381
00382
00383
00395 void KDWalk(KDTREE *Tree, void_proc Action) {
00396 WalkAction = Action;
00397 if (Tree->Root.Left != NULL)
00398 Walk (Tree->Root.Left, 0);
00399 }
00400
00401
00402
00418 void FreeKDTree(KDTREE *Tree) {
00419 FreeSubTree (Tree->Root.Left);
00420 memfree(Tree);
00421 }
00422
00423
00424
00425
00426
00437 int
00438 Equal (FLOAT32 Key1[], FLOAT32 Key2[]) {
00439 int i;
00440
00441 for (i = N; i > 0; i--, Key1++, Key2++)
00442 if (*Key1 != *Key2)
00443 return (FALSE);
00444 return (TRUE);
00445 }
00446
00447
00448
00464 KDNODE *
00465 MakeKDNode (FLOAT32 Key[], char *Data, int Index) {
00466 KDNODE *NewNode;
00467
00468 NewNode = (KDNODE *) Emalloc (sizeof (KDNODE));
00469
00470 NewNode->Key = Key;
00471 NewNode->Data = Data;
00472 NewNode->BranchPoint = Key[Index];
00473 NewNode->LeftBranch = KeyDesc[Index].Min;
00474 NewNode->RightBranch = KeyDesc[Index].Max;
00475 NewNode->Left = NULL;
00476 NewNode->Right = NULL;
00477
00478 return (NewNode);
00479 }
00480
00481
00482
00491 void FreeKDNode(KDNODE *Node) {
00492 memfree ((char *) Node);
00493 }
00494
00495
00496
00525 void Search(int Level, KDNODE *SubTree) {
00526 FLOAT32 d;
00527 FLOAT32 OldSBoxEdge;
00528 FLOAT32 OldLBoxEdge;
00529
00530 if (Level >= N)
00531 Level = 0;
00532
00533 d = ComputeDistance (N, KeyDesc, QueryPoint, SubTree->Key);
00534 if (d < Radius) {
00535 if (NumberOfNeighbors < MaxNeighbors) {
00536 Neighbor[NumberOfNeighbors] = SubTree->Data;
00537 Distance[NumberOfNeighbors] = d;
00538 NumberOfNeighbors++;
00539 if (NumberOfNeighbors == MaxNeighbors)
00540 FindMaxDistance();
00541 }
00542 else {
00543 Neighbor[Furthest] = SubTree->Data;
00544 Distance[Furthest] = d;
00545 FindMaxDistance();
00546 }
00547 }
00548 if (QueryPoint[Level] < SubTree->BranchPoint) {
00549 OldSBoxEdge = SBMax[Level];
00550 SBMax[Level] = SubTree->LeftBranch;
00551 OldLBoxEdge = LBMax[Level];
00552 LBMax[Level] = SubTree->RightBranch;
00553 if (SubTree->Left != NULL)
00554 Search (Level + 1, SubTree->Left);
00555 SBMax[Level] = OldSBoxEdge;
00556 LBMax[Level] = OldLBoxEdge;
00557 OldSBoxEdge = SBMin[Level];
00558 SBMin[Level] = SubTree->RightBranch;
00559 OldLBoxEdge = LBMin[Level];
00560 LBMin[Level] = SubTree->LeftBranch;
00561 if ((SubTree->Right != NULL) && QueryIntersectsSearch ())
00562 Search (Level + 1, SubTree->Right);
00563 SBMin[Level] = OldSBoxEdge;
00564 LBMin[Level] = OldLBoxEdge;
00565 }
00566 else {
00567 OldSBoxEdge = SBMin[Level];
00568 SBMin[Level] = SubTree->RightBranch;
00569 OldLBoxEdge = LBMin[Level];
00570 LBMin[Level] = SubTree->LeftBranch;
00571 if (SubTree->Right != NULL)
00572 Search (Level + 1, SubTree->Right);
00573 SBMin[Level] = OldSBoxEdge;
00574 LBMin[Level] = OldLBoxEdge;
00575 OldSBoxEdge = SBMax[Level];
00576 SBMax[Level] = SubTree->LeftBranch;
00577 OldLBoxEdge = LBMax[Level];
00578 LBMax[Level] = SubTree->RightBranch;
00579 if ((SubTree->Left != NULL) && QueryIntersectsSearch ())
00580 Search (Level + 1, SubTree->Left);
00581 SBMax[Level] = OldSBoxEdge;
00582 LBMax[Level] = OldLBoxEdge;
00583 }
00584 if (QueryInSearch ())
00585 longjmp (QuickExit, 1);
00586 }
00587
00588
00589
00602 FLOAT32
00603 ComputeDistance (register int N,
00604 register PARAM_DESC Dim[],
00605 register FLOAT32 p1[], register FLOAT32 p2[]) {
00606 register FLOAT32 TotalDistance;
00607 register FLOAT32 DimensionDistance;
00608 FLOAT32 WrapDistance;
00609
00610 TotalDistance = 0;
00611 for (; N > 0; N--, p1++, p2++, Dim++) {
00612 if (Dim->NonEssential)
00613 continue;
00614
00615 DimensionDistance = *p1 - *p2;
00616
00617
00618 if (Dim->Circular) {
00619 DimensionDistance = Magnitude (DimensionDistance);
00620 WrapDistance = Dim->Max - Dim->Min - DimensionDistance;
00621 DimensionDistance = MIN (DimensionDistance, WrapDistance);
00622 }
00623
00624 TotalDistance += DimensionDistance * DimensionDistance;
00625 }
00626 return ((FLOAT32) sqrt ((FLOAT64) TotalDistance));
00627 }
00628
00629
00630
00646 void FindMaxDistance() {
00647 int i;
00648
00649 Radius = Distance[Furthest];
00650 for (i = 0; i < MaxNeighbors; i++) {
00651 if (Distance[i] > Radius) {
00652 Radius = Distance[i];
00653 Furthest = i;
00654 }
00655 }
00656 }
00657
00658
00659
00685 int QueryIntersectsSearch() {
00686 register int i;
00687 register FLOAT32 *Query;
00688 register FLOAT32 *Lower;
00689 register FLOAT32 *Upper;
00690 register FLOAT64 TotalDistance;
00691 register FLOAT32 DimensionDistance;
00692 register FLOAT64 RadiusSquared;
00693 register PARAM_DESC *Dim;
00694 register FLOAT32 WrapDistance;
00695
00696 RadiusSquared = Radius * Radius;
00697 Query = QueryPoint;
00698 Lower = SBMin;
00699 Upper = SBMax;
00700 TotalDistance = 0.0;
00701 Dim = KeyDesc;
00702 for (i = N; i > 0; i--, Dim++, Query++, Lower++, Upper++) {
00703 if (Dim->NonEssential)
00704 continue;
00705
00706 if (*Query < *Lower)
00707 DimensionDistance = *Lower - *Query;
00708 else if (*Query > *Upper)
00709 DimensionDistance = *Query - *Upper;
00710 else
00711 DimensionDistance = 0;
00712
00714 if (Dim->Circular) {
00715 if (*Query < *Lower)
00716 WrapDistance = *Query + Dim->Max - Dim->Min - *Upper;
00717 else if (*Query > *Upper)
00718 WrapDistance = *Lower - (*Query - (Dim->Max - Dim->Min));
00719 else
00720 WrapDistance = MAX_FLOAT32;
00721
00722 DimensionDistance = MIN (DimensionDistance, WrapDistance);
00723 }
00724
00725 TotalDistance += DimensionDistance * DimensionDistance;
00726 if (TotalDistance >= RadiusSquared)
00727 return (FALSE);
00728 }
00729 return (TRUE);
00730 }
00731
00732
00733
00758 int QueryInSearch() {
00759 register int i;
00760 register FLOAT32 *Query;
00761 register FLOAT32 *Lower;
00762 register FLOAT32 *Upper;
00763 register PARAM_DESC *Dim;
00764
00765 Query = QueryPoint;
00766 Lower = LBMin;
00767 Upper = LBMax;
00768 Dim = KeyDesc;
00769
00770 for (i = N - 1; i >= 0; i--, Dim++, Query++, Lower++, Upper++) {
00771 if (Dim->NonEssential)
00772 continue;
00773
00774 if ((*Query < *Lower + Radius) || (*Query > *Upper - Radius))
00775 return (FALSE);
00776 }
00777 return (TRUE);
00778 }
00779
00780
00781
00802 void Walk(KDNODE *SubTree, INT32 Level) {
00803 if ((SubTree->Left == NULL) && (SubTree->Right == NULL))
00804 (*WalkAction) (SubTree->Data, leaf, Level);
00805 else {
00806 (*WalkAction) (SubTree->Data, preorder, Level);
00807 if (SubTree->Left != NULL)
00808 Walk (SubTree->Left, Level + 1);
00809 (*WalkAction) (SubTree->Data, postorder, Level);
00810 if (SubTree->Right != NULL)
00811 Walk (SubTree->Right, Level + 1);
00812 (*WalkAction) (SubTree->Data, endorder, Level);
00813 }
00814 }
00815
00816
00817
00827 void FreeSubTree(KDNODE *SubTree) {
00828 if (SubTree != NULL) {
00829 FreeSubTree (SubTree->Left);
00830 FreeSubTree (SubTree->Right);
00831 memfree(SubTree);
00832 }
00833 }