1 /******************************************************************************
2 ** Filename: kdtree.c
3 ** Purpose: Routines for managing K-D search trees
4 ** Author: Dan Johnson
5 ** History: 3/10/89, DSJ, Created.
6 ** 5/23/89, DSJ, Added circular feature capability.
7 ** 7/13/89, DSJ, Made tree nodes invisible to outside.
8 **
9 ** (c) Copyright Hewlett-Packard Company, 1988.
10 ** Licensed under the Apache License, Version 2.0 (the "License");
11 ** you may not use this file except in compliance with the License.
12 ** You may obtain a copy of the License at
13 ** http://www.apache.org/licenses/LICENSE-2.0
14 ** Unless required by applicable law or agreed to in writing, software
15 ** distributed under the License is distributed on an "AS IS" BASIS,
16 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17 ** See the License for the specific language governing permissions and
18 ** limitations under the License.
19 ******************************************************************************/
20
21 /**----------------------------------------------------------------------------
22 Include Files and Type Defines
23 ----------------------------------------------------------------------------**/
24 #include "kdtree.h"
25 #include "const.h"
26 #include "emalloc.h"
27 #include "freelist.h"
28 #include <stdio.h>
29 #include <math.h>
30 #include <setjmp.h>
31
32 #define Magnitude(X) ((X) < 0 ? -(X) : (X))
33 #define MIN(A,B) ((A) < (B) ? (A) : (B))
34 #define NodeFound(N,K,D) (( (N)->Key == (K) ) && ( (N)->Data == (D) ))
35
36 /**----------------------------------------------------------------------------
37 Global Data Definitions and Declarations
38 ----------------------------------------------------------------------------**/
39 #define MINSEARCH -MAX_FLOAT32
40 #define MAXSEARCH MAX_FLOAT32
41
42 static int NumberOfNeighbors;
43 static inT16 N; /* number of dimensions in the kd tree */
44
45 static FLOAT32 *QueryPoint;
46 static int MaxNeighbors;
47 static FLOAT32 Radius;
48 static int Furthest;
49 static char **Neighbor;
50 static FLOAT32 *Distance;
51
52 static int MaxDimension = 0;
53 static FLOAT32 *SBMin;
54 static FLOAT32 *SBMax;
55 static FLOAT32 *LBMin;
56 static FLOAT32 *LBMax;
57
58 static PARAM_DESC *KeyDesc;
59
60 static jmp_buf QuickExit;
61
62 static void_proc WalkAction;
63
64 // Helper function to find the next essential dimension in a cycle.
NextLevel(int level)65 static int NextLevel(int level) {
66 do {
67 ++level;
68 if (level >= N)
69 level = 0;
70 } while (KeyDesc[level].NonEssential);
71 return level;
72 }
73
74 // Helper function to find the previous essential dimension in a cycle.
PrevLevel(int level)75 static int PrevLevel(int level) {
76 do {
77 --level;
78 if (level < 0)
79 level = N - 1;
80 } while (KeyDesc[level].NonEssential);
81 return level;
82 }
83
84 /**----------------------------------------------------------------------------
85 Public Code
86 ----------------------------------------------------------------------------**/
87 /*---------------------------------------------------------------------------*/
88 KDTREE *
MakeKDTree(inT16 KeySize,PARAM_DESC KeyDesc[])89 MakeKDTree (inT16 KeySize, PARAM_DESC KeyDesc[]) {
90 /*
91 ** Parameters:
92 ** KeySize # of dimensions in the K-D tree
93 ** KeyDesc array of params to describe key dimensions
94 ** Globals:
95 ** MaxDimension largest # of dimensions in any K-D tree
96 ** SBMin small search region box
97 ** SBMax
98 ** LBMin large search region box
99 ** LBMax
100 ** Key description of key dimensions
101 ** Operation:
102 ** This routine allocates and returns a new K-D tree data
103 ** structure. It also reallocates the small and large
104 ** search region boxes if they are not large enough to
105 ** accomodate the size of the new K-D tree. KeyDesc is
106 ** an array of key descriptors that indicate which dimensions
107 ** are circular and, if they are circular, what the range is.
108 ** Return:
109 ** Pointer to new K-D tree
110 ** Exceptions:
111 ** None
112 ** History:
113 ** 3/13/89, DSJ, Created.
114 */
115 int i;
116 void *NewMemory;
117 KDTREE *KDTree;
118
119 if (KeySize > MaxDimension) {
120 NewMemory = Emalloc (KeySize * 4 * sizeof (FLOAT32));
121 if (MaxDimension > 0) {
122 memfree ((char *) SBMin);
123 memfree ((char *) SBMax);
124 memfree ((char *) LBMin);
125 memfree ((char *) LBMax);
126 }
127 SBMin = (FLOAT32 *) NewMemory;
128 SBMax = SBMin + KeySize;
129 LBMin = SBMax + KeySize;
130 LBMax = LBMin + KeySize;
131 }
132
133 KDTree =
134 (KDTREE *) Emalloc (sizeof (KDTREE) +
135 (KeySize - 1) * sizeof (PARAM_DESC));
136 for (i = 0; i < KeySize; i++) {
137 KDTree->KeyDesc[i].NonEssential = KeyDesc[i].NonEssential;
138 KDTree->KeyDesc[i].Circular = KeyDesc[i].Circular;
139 if (KeyDesc[i].Circular) {
140 KDTree->KeyDesc[i].Min = KeyDesc[i].Min;
141 KDTree->KeyDesc[i].Max = KeyDesc[i].Max;
142 KDTree->KeyDesc[i].Range = KeyDesc[i].Max - KeyDesc[i].Min;
143 KDTree->KeyDesc[i].HalfRange = KDTree->KeyDesc[i].Range / 2;
144 KDTree->KeyDesc[i].MidRange = (KeyDesc[i].Max + KeyDesc[i].Min) / 2;
145 }
146 else {
147 KDTree->KeyDesc[i].Min = MINSEARCH;
148 KDTree->KeyDesc[i].Max = MAXSEARCH;
149 }
150 }
151 KDTree->KeySize = KeySize;
152 KDTree->Root.Left = NULL;
153 KDTree->Root.Right = NULL;
154 return (KDTree);
155 } /* MakeKDTree */
156
157
158 /*---------------------------------------------------------------------------*/
KDStore(KDTREE * Tree,FLOAT32 * Key,void * Data)159 void KDStore(KDTREE *Tree, FLOAT32 *Key, void *Data) {
160 /*
161 ** Parameters:
162 ** Tree K-D tree in which data is to be stored
163 ** Key ptr to key by which data can be retrieved
164 ** Data ptr to data to be stored in the tree
165 ** Globals:
166 ** N dimension of the K-D tree
167 ** KeyDesc descriptions of tree dimensions
168 ** StoreCount debug variables for performance tests
169 ** StoreUniqueCount
170 ** StoreProbeCount
171 ** Operation:
172 ** This routine stores Data in the K-D tree specified by Tree
173 ** using Key as an access key.
174 ** Return: none
175 ** Exceptions: none
176 ** History: 3/10/89, DSJ, Created.
177 ** 7/13/89, DSJ, Changed return to void.
178 */
179 int Level;
180 KDNODE *Node;
181 KDNODE **PtrToNode;
182
183 N = Tree->KeySize;
184 KeyDesc = &(Tree->KeyDesc[0]);
185 PtrToNode = &(Tree->Root.Left);
186 Node = *PtrToNode;
187 Level = NextLevel(-1);
188 while (Node != NULL) {
189 if (Key[Level] < Node->BranchPoint) {
190 PtrToNode = &(Node->Left);
191 if (Key[Level] > Node->LeftBranch)
192 Node->LeftBranch = Key[Level];
193 }
194 else {
195 PtrToNode = &(Node->Right);
196 if (Key[Level] < Node->RightBranch)
197 Node->RightBranch = Key[Level];
198 }
199 Level = NextLevel(Level);
200 Node = *PtrToNode;
201 }
202
203 *PtrToNode = MakeKDNode (Key, (char *) Data, Level);
204 } /* KDStore */
205
206
207 /*---------------------------------------------------------------------------*/
208 void
KDDelete(KDTREE * Tree,FLOAT32 Key[],void * Data)209 KDDelete (KDTREE * Tree, FLOAT32 Key[], void *Data) {
210 /*
211 ** Parameters:
212 ** Tree K-D tree to delete node from
213 ** Key key of node to be deleted
214 ** Data data contents of node to be deleted
215 ** Globals:
216 ** N dimension of the K-D tree
217 ** KeyDesc description of each dimension
218 ** DeleteCount debug variables for performance tests
219 ** DeleteProbeCount
220 ** Operation:
221 ** This routine deletes a node from Tree. The node to be
222 ** deleted is specified by the Key for the node and the Data
223 ** contents of the node. These two pointers must be identical
224 ** to the pointers that were used for the node when it was
225 ** originally stored in the tree. A node will be deleted from
226 ** the tree only if its key and data pointers are identical
227 ** to Key and Data respectively. The empty space left in the tree
228 ** is filled by pulling a leaf up from the bottom of one of
229 ** the subtrees of the node being deleted. The leaf node will
230 ** be pulled from left subtrees whenever possible (this was
231 ** an arbitrary decision). No attempt is made to pull the leaf
232 ** from the deepest subtree (to minimize length). The branch
233 ** point for the replacement node is changed to be the same as
234 ** the branch point of the deleted node. This keeps us from
235 ** having to rearrange the tree every time we delete a node.
236 ** Also, the LeftBranch and RightBranch numbers of the
237 ** replacement node are set to be the same as the deleted node.
238 ** The makes the delete easier and more efficient, but it may
239 ** make searches in the tree less efficient after many nodes are
240 ** deleted. If the node specified by Key and Data does not
241 ** exist in the tree, then nothing is done.
242 ** Return: none
243 ** None
244 ** Exceptions: none
245 ** None
246 ** History: 3/13/89, DSJ, Created.
247 ** 7/13/89, DSJ, Specify node indirectly by key and data.
248 */
249 int Level;
250 KDNODE *Current;
251 KDNODE *Father;
252 KDNODE *Replacement;
253 KDNODE *FatherReplacement;
254
255 /* initialize search at root of tree */
256 N = Tree->KeySize;
257 KeyDesc = &(Tree->KeyDesc[0]);
258 Father = &(Tree->Root);
259 Current = Father->Left;
260 Level = NextLevel(-1);
261
262 /* search tree for node to be deleted */
263 while ((Current != NULL) && (!NodeFound (Current, Key, Data))) {
264 Father = Current;
265 if (Key[Level] < Current->BranchPoint)
266 Current = Current->Left;
267 else
268 Current = Current->Right;
269
270 Level = NextLevel(Level);
271 }
272
273 if (Current != NULL) { /* if node to be deleted was found */
274 Replacement = Current;
275 FatherReplacement = Father;
276
277 /* search for replacement node (a leaf under node to be deleted */
278 while (TRUE) {
279 if (Replacement->Left != NULL) {
280 FatherReplacement = Replacement;
281 Replacement = Replacement->Left;
282 }
283 else if (Replacement->Right != NULL) {
284 FatherReplacement = Replacement;
285 Replacement = Replacement->Right;
286 }
287 else
288 break;
289
290 Level = NextLevel(Level);
291 }
292
293 /* compute level of replacement node's father */
294 Level = PrevLevel(Level);
295
296 /* disconnect replacement node from it's father */
297 if (FatherReplacement->Left == Replacement) {
298 FatherReplacement->Left = NULL;
299 FatherReplacement->LeftBranch = KeyDesc[Level].Min;
300 }
301 else {
302 FatherReplacement->Right = NULL;
303 FatherReplacement->RightBranch = KeyDesc[Level].Max;
304 }
305
306 /* replace deleted node with replacement (unless they are the same) */
307 if (Replacement != Current) {
308 Replacement->BranchPoint = Current->BranchPoint;
309 Replacement->LeftBranch = Current->LeftBranch;
310 Replacement->RightBranch = Current->RightBranch;
311 Replacement->Left = Current->Left;
312 Replacement->Right = Current->Right;
313
314 if (Father->Left == Current)
315 Father->Left = Replacement;
316 else
317 Father->Right = Replacement;
318 }
319 FreeKDNode(Current);
320 }
321 } /* KDDelete */
322
323
324 /*---------------------------------------------------------------------------*/
325 int
KDNearestNeighborSearch(KDTREE * Tree,FLOAT32 Query[],int QuerySize,FLOAT32 MaxDistance,void * NBuffer,FLOAT32 DBuffer[])326 KDNearestNeighborSearch (KDTREE * Tree,
327 FLOAT32 Query[],
328 int QuerySize,
329 FLOAT32 MaxDistance,
330 void *NBuffer, FLOAT32 DBuffer[]) {
331 /*
332 ** Parameters:
333 ** Tree ptr to K-D tree to be searched
334 ** Query ptr to query key (point in D-space)
335 ** QuerySize number of nearest neighbors to be found
336 ** MaxDistance all neighbors must be within this distance
337 ** NBuffer ptr to QuerySize buffer to hold nearest neighbors
338 ** DBuffer ptr to QuerySize buffer to hold distances
339 ** from nearest neighbor to query point
340 ** Globals:
341 ** NumberOfNeighbors # of neighbors found so far
342 ** N # of features in each key
343 ** KeyDesc description of tree dimensions
344 ** QueryPoint point in D-space to find neighbors of
345 ** MaxNeighbors maximum # of neighbors to find
346 ** Radius current distance of furthest neighbor
347 ** Furthest index of furthest neighbor
348 ** Neighbor buffer of current neighbors
349 ** Distance buffer of neighbor distances
350 ** SBMin lower extent of small search region
351 ** SBMax upper extent of small search region
352 ** LBMin lower extent of large search region
353 ** LBMax upper extent of large search region
354 ** QuickExit quick exit from recursive search
355 ** Operation:
356 ** This routine searches the K-D tree specified by Tree and
357 ** finds the QuerySize nearest neighbors of Query. All neighbors
358 ** must be within MaxDistance of Query. The data contents of
359 ** the nearest neighbors
360 ** are placed in NBuffer and their distances from Query are
361 ** placed in DBuffer.
362 ** Return: Number of nearest neighbors actually found
363 ** Exceptions: none
364 ** History:
365 ** 3/10/89, DSJ, Created.
366 ** 7/13/89, DSJ, Return contents of node instead of node itself.
367 */
368 int i;
369
370 NumberOfNeighbors = 0;
371 N = Tree->KeySize;
372 KeyDesc = &(Tree->KeyDesc[0]);
373 QueryPoint = Query;
374 MaxNeighbors = QuerySize;
375 Radius = MaxDistance;
376 Furthest = 0;
377 Neighbor = (char **) NBuffer;
378 Distance = DBuffer;
379
380 for (i = 0; i < N; i++) {
381 SBMin[i] = KeyDesc[i].Min;
382 SBMax[i] = KeyDesc[i].Max;
383 LBMin[i] = KeyDesc[i].Min;
384 LBMax[i] = KeyDesc[i].Max;
385 }
386
387 if (Tree->Root.Left != NULL) {
388 if (setjmp (QuickExit) == 0)
389 Search (0, Tree->Root.Left);
390 }
391 return (NumberOfNeighbors);
392 } /* KDNearestNeighborSearch */
393
394
395 /*---------------------------------------------------------------------------*/
KDWalk(KDTREE * Tree,void_proc Action)396 void KDWalk(KDTREE *Tree, void_proc Action) {
397 /*
398 ** Parameters:
399 ** Tree ptr to K-D tree to be walked
400 ** Action ptr to function to be executed at each node
401 ** Globals:
402 ** WalkAction action to be performed at every node
403 ** Operation:
404 ** This routine stores the desired action in a global
405 ** variable and starts a recursive walk of Tree. The walk
406 ** is started at the root node.
407 ** Return:
408 ** None
409 ** Exceptions:
410 ** None
411 ** History:
412 ** 3/13/89, DSJ, Created.
413 */
414 WalkAction = Action;
415 if (Tree->Root.Left != NULL)
416 Walk (Tree->Root.Left, NextLevel(-1));
417 } /* KDWalk */
418
419
420 /*---------------------------------------------------------------------------*/
FreeKDTree(KDTREE * Tree)421 void FreeKDTree(KDTREE *Tree) {
422 /*
423 ** Parameters:
424 ** Tree tree data structure to be released
425 ** Globals: none
426 ** Operation:
427 ** This routine frees all memory which is allocated to the
428 ** specified KD-tree. This includes the data structure for
429 ** the kd-tree itself plus the data structures for each node
430 ** in the tree. It does not include the Key and Data items
431 ** which are pointed to by the nodes. This memory is left
432 ** untouched.
433 ** Return: none
434 ** Exceptions: none
435 ** History:
436 ** 5/26/89, DSJ, Created.
437 */
438 FreeSubTree (Tree->Root.Left);
439 memfree(Tree);
440 } /* FreeKDTree */
441
442
443 /**----------------------------------------------------------------------------
444 Private Code
445 ----------------------------------------------------------------------------**/
446 /*---------------------------------------------------------------------------*/
447 int
Equal(FLOAT32 Key1[],FLOAT32 Key2[])448 Equal (FLOAT32 Key1[], FLOAT32 Key2[]) {
449 /*
450 ** Parameters:
451 ** Key1,Key2 search keys to be compared for equality
452 ** Globals:
453 ** N number of parameters per key
454 ** Operation:
455 ** This routine returns TRUE if Key1 = Key2.
456 ** Return:
457 ** TRUE if Key1 = Key2, else FALSE.
458 ** Exceptions:
459 ** None
460 ** History:
461 ** 3/11/89, DSJ, Created.
462 */
463 int i;
464
465 for (i = N; i > 0; i--, Key1++, Key2++)
466 if (*Key1 != *Key2)
467 return (FALSE);
468 return (TRUE);
469 } /* Equal */
470
471
472 /*---------------------------------------------------------------------------*/
473 KDNODE *
MakeKDNode(FLOAT32 Key[],char * Data,int Index)474 MakeKDNode (FLOAT32 Key[], char *Data, int Index) {
475 /*
476 ** Parameters:
477 ** Key Access key for new node in KD tree
478 ** Data ptr to data to be stored in new node
479 ** Index index of Key to branch on
480 ** Globals:
481 ** KeyDesc descriptions of key dimensions
482 ** Operation:
483 ** This routine allocates memory for a new K-D tree node
484 ** and places the specified Key and Data into it. The
485 ** left and right subtree pointers for the node are
486 ** initialized to empty subtrees.
487 ** Return:
488 ** pointer to new K-D tree node
489 ** Exceptions:
490 ** None
491 ** History:
492 ** 3/11/89, DSJ, Created.
493 */
494 KDNODE *NewNode;
495
496 NewNode = (KDNODE *) Emalloc (sizeof (KDNODE));
497
498 NewNode->Key = Key;
499 NewNode->Data = Data;
500 NewNode->BranchPoint = Key[Index];
501 NewNode->LeftBranch = KeyDesc[Index].Min;
502 NewNode->RightBranch = KeyDesc[Index].Max;
503 NewNode->Left = NULL;
504 NewNode->Right = NULL;
505
506 return (NewNode);
507 } /* MakeKDNode */
508
509
510 /*---------------------------------------------------------------------------*/
FreeKDNode(KDNODE * Node)511 void FreeKDNode(KDNODE *Node) {
512 /*
513 ** Parameters:
514 ** Node ptr to node data structure to be freed
515 ** Globals:
516 ** None
517 ** Operation:
518 ** This routine frees up the memory allocated to Node.
519 ** Return:
520 ** None
521 ** Exceptions:
522 ** None
523 ** History:
524 ** 3/13/89, DSJ, Created.
525 */
526 memfree ((char *) Node);
527 } /* FreeKDNode */
528
529
530 /*---------------------------------------------------------------------------*/
Search(int Level,KDNODE * SubTree)531 void Search(int Level, KDNODE *SubTree) {
532 /*
533 ** Parameters:
534 ** Level level in tree of sub-tree to be searched
535 ** SubTree sub-tree to be searched
536 ** Globals:
537 ** NumberOfNeighbors # of neighbors found so far
538 ** N # of features in each key
539 ** KeyDesc description of key dimensions
540 ** QueryPoint point in D-space to find neighbors of
541 ** MaxNeighbors maximum # of neighbors to find
542 ** Radius current distance of furthest neighbor
543 ** Furthest index of furthest neighbor
544 ** Neighbor buffer of current neighbors
545 ** Distance buffer of neighbor distances
546 ** SBMin lower extent of small search region
547 ** SBMax upper extent of small search region
548 ** LBMin lower extent of large search region
549 ** LBMax upper extent of large search region
550 ** QuickExit quick exit from recursive search
551 ** Operation:
552 ** This routine searches SubTree for those entries which are
553 ** possibly among the MaxNeighbors nearest neighbors of the
554 ** QueryPoint and places their data in the Neighbor buffer and
555 ** their distances from QueryPoint in the Distance buffer.
556 ** Return: none
557 ** Exceptions: none
558 ** History:
559 ** 3/11/89, DSJ, Created.
560 ** 7/13/89, DSJ, Save node contents, not node, in neighbor buffer
561 */
562 FLOAT32 d;
563 FLOAT32 OldSBoxEdge;
564 FLOAT32 OldLBoxEdge;
565
566 if (Level >= N)
567 Level = 0;
568
569 d = ComputeDistance (N, KeyDesc, QueryPoint, SubTree->Key);
570 if (d < Radius) {
571 if (NumberOfNeighbors < MaxNeighbors) {
572 Neighbor[NumberOfNeighbors] = SubTree->Data;
573 Distance[NumberOfNeighbors] = d;
574 NumberOfNeighbors++;
575 if (NumberOfNeighbors == MaxNeighbors)
576 FindMaxDistance();
577 }
578 else {
579 Neighbor[Furthest] = SubTree->Data;
580 Distance[Furthest] = d;
581 FindMaxDistance();
582 }
583 }
584 if (QueryPoint[Level] < SubTree->BranchPoint) {
585 OldSBoxEdge = SBMax[Level];
586 SBMax[Level] = SubTree->LeftBranch;
587 OldLBoxEdge = LBMax[Level];
588 LBMax[Level] = SubTree->RightBranch;
589 if (SubTree->Left != NULL)
590 Search (NextLevel(Level), SubTree->Left);
591 SBMax[Level] = OldSBoxEdge;
592 LBMax[Level] = OldLBoxEdge;
593 OldSBoxEdge = SBMin[Level];
594 SBMin[Level] = SubTree->RightBranch;
595 OldLBoxEdge = LBMin[Level];
596 LBMin[Level] = SubTree->LeftBranch;
597 if ((SubTree->Right != NULL) && QueryIntersectsSearch ())
598 Search (NextLevel(Level), SubTree->Right);
599 SBMin[Level] = OldSBoxEdge;
600 LBMin[Level] = OldLBoxEdge;
601 }
602 else {
603 OldSBoxEdge = SBMin[Level];
604 SBMin[Level] = SubTree->RightBranch;
605 OldLBoxEdge = LBMin[Level];
606 LBMin[Level] = SubTree->LeftBranch;
607 if (SubTree->Right != NULL)
608 Search (NextLevel(Level), SubTree->Right);
609 SBMin[Level] = OldSBoxEdge;
610 LBMin[Level] = OldLBoxEdge;
611 OldSBoxEdge = SBMax[Level];
612 SBMax[Level] = SubTree->LeftBranch;
613 OldLBoxEdge = LBMax[Level];
614 LBMax[Level] = SubTree->RightBranch;
615 if ((SubTree->Left != NULL) && QueryIntersectsSearch ())
616 Search (NextLevel(Level), SubTree->Left);
617 SBMax[Level] = OldSBoxEdge;
618 LBMax[Level] = OldLBoxEdge;
619 }
620 if (QueryInSearch ())
621 longjmp (QuickExit, 1);
622 } /* Search */
623
624
625 /*---------------------------------------------------------------------------*/
626 FLOAT32
ComputeDistance(register int N,register PARAM_DESC Dim[],register FLOAT32 p1[],register FLOAT32 p2[])627 ComputeDistance (register int N,
628 register PARAM_DESC Dim[],
629 register FLOAT32 p1[], register FLOAT32 p2[]) {
630 /*
631 ** Parameters:
632 ** N number of dimensions in K-D space
633 ** Dim descriptions of each dimension
634 ** p1,p2 two different points in K-D space
635 ** Globals:
636 ** None
637 ** Operation:
638 ** This routine computes the euclidian distance
639 ** between p1 and p2 in K-D space (an N dimensional space).
640 ** Return:
641 ** Distance between p1 and p2.
642 ** Exceptions:
643 ** None
644 ** History:
645 ** 3/11/89, DSJ, Created.
646 */
647 register FLOAT32 TotalDistance;
648 register FLOAT32 DimensionDistance;
649 FLOAT32 WrapDistance;
650
651 TotalDistance = 0;
652 for (; N > 0; N--, p1++, p2++, Dim++) {
653 if (Dim->NonEssential)
654 continue;
655
656 DimensionDistance = *p1 - *p2;
657
658 /* if this dimension is circular - check wraparound distance */
659 if (Dim->Circular) {
660 DimensionDistance = Magnitude (DimensionDistance);
661 WrapDistance = Dim->Max - Dim->Min - DimensionDistance;
662 DimensionDistance = MIN (DimensionDistance, WrapDistance);
663 }
664
665 TotalDistance += DimensionDistance * DimensionDistance;
666 }
667 return ((FLOAT32) sqrt ((FLOAT64) TotalDistance));
668 } /* ComputeDistance */
669
670
671 /*---------------------------------------------------------------------------*/
FindMaxDistance()672 void FindMaxDistance() {
673 /*
674 ** Parameters:
675 ** None
676 ** Globals:
677 ** MaxNeighbors maximum # of neighbors to find
678 ** Radius current distance of furthest neighbor
679 ** Furthest index of furthest neighbor
680 ** Distance buffer of neighbor distances
681 ** Operation:
682 ** This routine searches the Distance buffer for the maximum
683 ** distance, places this distance in Radius, and places the
684 ** index of this distance in Furthest.
685 ** Return:
686 ** None
687 ** Exceptions:
688 ** None
689 ** History:
690 ** 3/11/89, DSJ, Created.
691 */
692 int i;
693
694 Radius = Distance[Furthest];
695 for (i = 0; i < MaxNeighbors; i++) {
696 if (Distance[i] > Radius) {
697 Radius = Distance[i];
698 Furthest = i;
699 }
700 }
701 } /* FindMaxDistance */
702
703
704 /*---------------------------------------------------------------------------*/
QueryIntersectsSearch()705 int QueryIntersectsSearch() {
706 /*
707 ** Parameters:
708 ** None
709 ** Globals:
710 ** N # of features in each key
711 ** KeyDesc descriptions of each dimension
712 ** QueryPoint point in D-space to find neighbors of
713 ** Radius current distance of furthest neighbor
714 ** SBMin lower extent of small search region
715 ** SBMax upper extent of small search region
716 ** Operation:
717 ** This routine returns TRUE if the query region intersects
718 ** the current smallest search region. The query region is
719 ** the circle of radius Radius centered at QueryPoint.
720 ** The smallest search region is the box (in N dimensions)
721 ** whose edges in each dimension are specified by SBMin and SBMax.
722 ** In the case of circular dimensions, we must also check the
723 ** point which is one wrap-distance away from the query to
724 ** see if it would intersect the search region.
725 ** Return:
726 ** TRUE if query region intersects search region, else FALSE
727 ** Exceptions:
728 ** None
729 ** History:
730 ** 3/11/89, DSJ, Created.
731 */
732 register int i;
733 register FLOAT32 *Query;
734 register FLOAT32 *Lower;
735 register FLOAT32 *Upper;
736 register FLOAT64 TotalDistance;
737 register FLOAT32 DimensionDistance;
738 register FLOAT64 RadiusSquared;
739 register PARAM_DESC *Dim;
740 register FLOAT32 WrapDistance;
741
742 RadiusSquared = Radius * Radius;
743 Query = QueryPoint;
744 Lower = SBMin;
745 Upper = SBMax;
746 TotalDistance = 0.0;
747 Dim = KeyDesc;
748 for (i = N; i > 0; i--, Dim++, Query++, Lower++, Upper++) {
749 if (Dim->NonEssential)
750 continue;
751
752 if (*Query < *Lower)
753 DimensionDistance = *Lower - *Query;
754 else if (*Query > *Upper)
755 DimensionDistance = *Query - *Upper;
756 else
757 DimensionDistance = 0;
758
759 /* if this dimension is circular - check wraparound distance */
760 if (Dim->Circular) {
761 if (*Query < *Lower)
762 WrapDistance = *Query + Dim->Max - Dim->Min - *Upper;
763 else if (*Query > *Upper)
764 WrapDistance = *Lower - (*Query - (Dim->Max - Dim->Min));
765 else
766 WrapDistance = MAX_FLOAT32;
767
768 DimensionDistance = MIN (DimensionDistance, WrapDistance);
769 }
770
771 TotalDistance += DimensionDistance * DimensionDistance;
772 if (TotalDistance >= RadiusSquared)
773 return (FALSE);
774 }
775 return (TRUE);
776 } /* QueryIntersectsSearch */
777
778
779 /*---------------------------------------------------------------------------*/
QueryInSearch()780 int QueryInSearch() {
781 /*
782 ** Parameters:
783 ** None
784 ** Globals:
785 ** N # of features in each key
786 ** KeyDesc descriptions of each dimension
787 ** QueryPoint point in D-space to find neighbors of
788 ** Radius current distance of furthest neighbor
789 ** LBMin lower extent of large search region
790 ** LBMax upper extent of large search region
791 ** Operation:
792 ** This routine returns TRUE if the current query region is
793 ** totally contained in the current largest search region.
794 ** The query region is the circle of
795 ** radius Radius centered at QueryPoint. The search region is
796 ** the box (in N dimensions) whose edges in each
797 ** dimension are specified by LBMin and LBMax.
798 ** Return:
799 ** TRUE if query region is inside search region, else FALSE
800 ** Exceptions:
801 ** None
802 ** History:
803 ** 3/11/89, DSJ, Created.
804 */
805 register int i;
806 register FLOAT32 *Query;
807 register FLOAT32 *Lower;
808 register FLOAT32 *Upper;
809 register PARAM_DESC *Dim;
810
811 Query = QueryPoint;
812 Lower = LBMin;
813 Upper = LBMax;
814 Dim = KeyDesc;
815
816 for (i = N - 1; i >= 0; i--, Dim++, Query++, Lower++, Upper++) {
817 if (Dim->NonEssential)
818 continue;
819
820 if ((*Query < *Lower + Radius) || (*Query > *Upper - Radius))
821 return (FALSE);
822 }
823 return (TRUE);
824 } /* QueryInSearch */
825
826
827 /*---------------------------------------------------------------------------*/
Walk(KDNODE * SubTree,inT32 Level)828 void Walk(KDNODE *SubTree, inT32 Level) {
829 /*
830 ** Parameters:
831 ** SubTree ptr to root of subtree to be walked
832 ** Level current level in the tree for this node
833 ** Globals:
834 ** WalkAction action to be performed at every node
835 ** Operation:
836 ** This routine walks thru the specified SubTree and invokes
837 ** WalkAction at each node. WalkAction is invoked with three
838 ** arguments as follows:
839 ** WalkAction( NodeData, Order, Level )
840 ** Data is the data contents of the node being visited,
841 ** Order is either preorder,
842 ** postorder, endorder, or leaf depending on whether this is
843 ** the 1st, 2nd, or 3rd time a node has been visited, or
844 ** whether the node is a leaf. Level is the level of the node in
845 ** the tree with the root being level 0.
846 ** Return: none
847 ** Exceptions: none
848 ** History:
849 ** 3/13/89, DSJ, Created.
850 ** 7/13/89, DSJ, Pass node contents, not node, to WalkAction().
851 */
852 if ((SubTree->Left == NULL) && (SubTree->Right == NULL))
853 (*WalkAction) (SubTree->Data, leaf, Level);
854 else {
855 (*WalkAction) (SubTree->Data, preorder, Level);
856 if (SubTree->Left != NULL)
857 Walk (SubTree->Left, NextLevel(Level));
858 (*WalkAction) (SubTree->Data, postorder, Level);
859 if (SubTree->Right != NULL)
860 Walk (SubTree->Right, NextLevel(Level));
861 (*WalkAction) (SubTree->Data, endorder, Level);
862 }
863 } /* Walk */
864
865
866 /*---------------------------------------------------------------------------*/
FreeSubTree(KDNODE * SubTree)867 void FreeSubTree(KDNODE *SubTree) {
868 /*
869 ** Parameters:
870 ** SubTree ptr to root node of sub-tree to be freed
871 ** Globals: none
872 ** Operation:
873 ** This routine recursively frees the memory allocated to
874 ** to the specified subtree.
875 ** Return: none
876 ** Exceptions: none
877 ** History: 7/13/89, DSJ, Created.
878 */
879 if (SubTree != NULL) {
880 FreeSubTree (SubTree->Left);
881 FreeSubTree (SubTree->Right);
882 memfree(SubTree);
883 }
884 } /* FreeSubTree */
885