2007-03-08 04:03:40 +08:00
|
|
|
/******************************************************************************
|
|
|
|
** Filename: kdtree.c
|
|
|
|
** Purpose: Routines for managing K-D search trees
|
|
|
|
** Author: Dan Johnson
|
|
|
|
** History: 3/10/89, DSJ, Created.
|
|
|
|
** 5/23/89, DSJ, Added circular feature capability.
|
|
|
|
** 7/13/89, DSJ, Made tree nodes invisible to outside.
|
|
|
|
**
|
|
|
|
** (c) Copyright Hewlett-Packard Company, 1988.
|
|
|
|
** Licensed under the Apache License, Version 2.0 (the "License");
|
|
|
|
** you may not use this file except in compliance with the License.
|
|
|
|
** You may obtain a copy of the License at
|
|
|
|
** http://www.apache.org/licenses/LICENSE-2.0
|
|
|
|
** Unless required by applicable law or agreed to in writing, software
|
|
|
|
** distributed under the License is distributed on an "AS IS" BASIS,
|
|
|
|
** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
|
|
** See the License for the specific language governing permissions and
|
|
|
|
** limitations under the License.
|
|
|
|
******************************************************************************/
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------------
|
|
|
|
Include Files and Type Defines
|
|
|
|
----------------------------------------------------------------------------**/
|
|
|
|
#include "kdtree.h"
|
|
|
|
#include "const.h"
|
|
|
|
#include "emalloc.h"
|
|
|
|
#include "freelist.h"
|
|
|
|
#include <stdio.h>
|
|
|
|
#include <math.h>
|
|
|
|
#include <setjmp.h>
|
|
|
|
|
|
|
|
#define Magnitude(X) ((X) < 0 ? -(X) : (X))
|
|
|
|
#define MIN(A,B) ((A) < (B) ? (A) : (B))
|
|
|
|
#define NodeFound(N,K,D) (( (N)->Key == (K) ) && ( (N)->Data == (D) ))
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------------
|
|
|
|
Global Data Definitions and Declarations
|
|
|
|
----------------------------------------------------------------------------**/
|
|
|
|
#define MINSEARCH -MAX_FLOAT32
|
|
|
|
#define MAXSEARCH MAX_FLOAT32
|
|
|
|
|
|
|
|
static int NumberOfNeighbors;
|
2008-04-22 08:23:04 +08:00
|
|
|
static inT16 N; /* number of dimensions in the kd tree */
|
2007-03-08 04:03:40 +08:00
|
|
|
|
|
|
|
static FLOAT32 *QueryPoint;
|
|
|
|
static int MaxNeighbors;
|
|
|
|
static FLOAT32 Radius;
|
|
|
|
static int Furthest;
|
|
|
|
static char **Neighbor;
|
|
|
|
static FLOAT32 *Distance;
|
|
|
|
|
|
|
|
static int MaxDimension = 0;
|
|
|
|
static FLOAT32 *SBMin;
|
|
|
|
static FLOAT32 *SBMax;
|
|
|
|
static FLOAT32 *LBMin;
|
|
|
|
static FLOAT32 *LBMax;
|
|
|
|
|
|
|
|
static PARAM_DESC *KeyDesc;
|
|
|
|
|
|
|
|
static jmp_buf QuickExit;
|
|
|
|
|
|
|
|
static void_proc WalkAction;
|
|
|
|
|
2008-02-01 08:07:59 +08:00
|
|
|
// Helper function to find the next essential dimension in a cycle.
|
|
|
|
static int NextLevel(int level) {
|
|
|
|
do {
|
|
|
|
++level;
|
|
|
|
if (level >= N)
|
|
|
|
level = 0;
|
|
|
|
} while (KeyDesc[level].NonEssential);
|
|
|
|
return level;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Helper function to find the previous essential dimension in a cycle.
|
|
|
|
static int PrevLevel(int level) {
|
|
|
|
do {
|
|
|
|
--level;
|
|
|
|
if (level < 0)
|
|
|
|
level = N - 1;
|
|
|
|
} while (KeyDesc[level].NonEssential);
|
|
|
|
return level;
|
|
|
|
}
|
|
|
|
|
2007-03-08 04:03:40 +08:00
|
|
|
/**----------------------------------------------------------------------------
|
|
|
|
Public Code
|
|
|
|
----------------------------------------------------------------------------**/
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
KDTREE *
|
2008-04-22 08:23:04 +08:00
|
|
|
MakeKDTree (inT16 KeySize, PARAM_DESC KeyDesc[]) {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** KeySize # of dimensions in the K-D tree
|
|
|
|
** KeyDesc array of params to describe key dimensions
|
|
|
|
** Globals:
|
|
|
|
** MaxDimension largest # of dimensions in any K-D tree
|
|
|
|
** SBMin small search region box
|
|
|
|
** SBMax
|
|
|
|
** LBMin large search region box
|
|
|
|
** LBMax
|
|
|
|
** Key description of key dimensions
|
|
|
|
** Operation:
|
|
|
|
** This routine allocates and returns a new K-D tree data
|
|
|
|
** structure. It also reallocates the small and large
|
|
|
|
** search region boxes if they are not large enough to
|
|
|
|
** accomodate the size of the new K-D tree. KeyDesc is
|
|
|
|
** an array of key descriptors that indicate which dimensions
|
|
|
|
** are circular and, if they are circular, what the range is.
|
|
|
|
** Return:
|
|
|
|
** Pointer to new K-D tree
|
|
|
|
** Exceptions:
|
|
|
|
** None
|
|
|
|
** History:
|
|
|
|
** 3/13/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
int i;
|
|
|
|
void *NewMemory;
|
|
|
|
KDTREE *KDTree;
|
|
|
|
|
|
|
|
if (KeySize > MaxDimension) {
|
|
|
|
NewMemory = Emalloc (KeySize * 4 * sizeof (FLOAT32));
|
|
|
|
if (MaxDimension > 0) {
|
|
|
|
memfree ((char *) SBMin);
|
|
|
|
memfree ((char *) SBMax);
|
|
|
|
memfree ((char *) LBMin);
|
|
|
|
memfree ((char *) LBMax);
|
|
|
|
}
|
|
|
|
SBMin = (FLOAT32 *) NewMemory;
|
|
|
|
SBMax = SBMin + KeySize;
|
|
|
|
LBMin = SBMax + KeySize;
|
|
|
|
LBMax = LBMin + KeySize;
|
|
|
|
}
|
|
|
|
|
|
|
|
KDTree =
|
|
|
|
(KDTREE *) Emalloc (sizeof (KDTREE) +
|
|
|
|
(KeySize - 1) * sizeof (PARAM_DESC));
|
|
|
|
for (i = 0; i < KeySize; i++) {
|
|
|
|
KDTree->KeyDesc[i].NonEssential = KeyDesc[i].NonEssential;
|
|
|
|
KDTree->KeyDesc[i].Circular = KeyDesc[i].Circular;
|
|
|
|
if (KeyDesc[i].Circular) {
|
|
|
|
KDTree->KeyDesc[i].Min = KeyDesc[i].Min;
|
|
|
|
KDTree->KeyDesc[i].Max = KeyDesc[i].Max;
|
|
|
|
KDTree->KeyDesc[i].Range = KeyDesc[i].Max - KeyDesc[i].Min;
|
|
|
|
KDTree->KeyDesc[i].HalfRange = KDTree->KeyDesc[i].Range / 2;
|
|
|
|
KDTree->KeyDesc[i].MidRange = (KeyDesc[i].Max + KeyDesc[i].Min) / 2;
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
KDTree->KeyDesc[i].Min = MINSEARCH;
|
|
|
|
KDTree->KeyDesc[i].Max = MAXSEARCH;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
KDTree->KeySize = KeySize;
|
|
|
|
KDTree->Root.Left = NULL;
|
|
|
|
KDTree->Root.Right = NULL;
|
|
|
|
return (KDTree);
|
|
|
|
} /* MakeKDTree */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
2008-02-01 08:07:59 +08:00
|
|
|
void KDStore(KDTREE *Tree, FLOAT32 *Key, void *Data) {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** Tree K-D tree in which data is to be stored
|
|
|
|
** Key ptr to key by which data can be retrieved
|
|
|
|
** Data ptr to data to be stored in the tree
|
|
|
|
** Globals:
|
|
|
|
** N dimension of the K-D tree
|
|
|
|
** KeyDesc descriptions of tree dimensions
|
|
|
|
** StoreCount debug variables for performance tests
|
|
|
|
** StoreUniqueCount
|
|
|
|
** StoreProbeCount
|
|
|
|
** Operation:
|
|
|
|
** This routine stores Data in the K-D tree specified by Tree
|
|
|
|
** using Key as an access key.
|
|
|
|
** Return: none
|
|
|
|
** Exceptions: none
|
|
|
|
** History: 3/10/89, DSJ, Created.
|
|
|
|
** 7/13/89, DSJ, Changed return to void.
|
|
|
|
*/
|
|
|
|
int Level;
|
|
|
|
KDNODE *Node;
|
|
|
|
KDNODE **PtrToNode;
|
|
|
|
|
|
|
|
N = Tree->KeySize;
|
|
|
|
KeyDesc = &(Tree->KeyDesc[0]);
|
|
|
|
PtrToNode = &(Tree->Root.Left);
|
|
|
|
Node = *PtrToNode;
|
2008-02-01 08:07:59 +08:00
|
|
|
Level = NextLevel(-1);
|
2007-03-08 04:03:40 +08:00
|
|
|
while (Node != NULL) {
|
|
|
|
if (Key[Level] < Node->BranchPoint) {
|
|
|
|
PtrToNode = &(Node->Left);
|
|
|
|
if (Key[Level] > Node->LeftBranch)
|
|
|
|
Node->LeftBranch = Key[Level];
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
PtrToNode = &(Node->Right);
|
|
|
|
if (Key[Level] < Node->RightBranch)
|
|
|
|
Node->RightBranch = Key[Level];
|
|
|
|
}
|
2008-02-01 08:07:59 +08:00
|
|
|
Level = NextLevel(Level);
|
2007-03-08 04:03:40 +08:00
|
|
|
Node = *PtrToNode;
|
|
|
|
}
|
|
|
|
|
|
|
|
*PtrToNode = MakeKDNode (Key, (char *) Data, Level);
|
|
|
|
} /* KDStore */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
void
|
|
|
|
KDDelete (KDTREE * Tree, FLOAT32 Key[], void *Data) {
|
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** Tree K-D tree to delete node from
|
|
|
|
** Key key of node to be deleted
|
|
|
|
** Data data contents of node to be deleted
|
|
|
|
** Globals:
|
|
|
|
** N dimension of the K-D tree
|
|
|
|
** KeyDesc description of each dimension
|
|
|
|
** DeleteCount debug variables for performance tests
|
|
|
|
** DeleteProbeCount
|
|
|
|
** Operation:
|
|
|
|
** This routine deletes a node from Tree. The node to be
|
|
|
|
** deleted is specified by the Key for the node and the Data
|
|
|
|
** contents of the node. These two pointers must be identical
|
|
|
|
** to the pointers that were used for the node when it was
|
|
|
|
** originally stored in the tree. A node will be deleted from
|
|
|
|
** the tree only if its key and data pointers are identical
|
|
|
|
** to Key and Data respectively. The empty space left in the tree
|
|
|
|
** is filled by pulling a leaf up from the bottom of one of
|
|
|
|
** the subtrees of the node being deleted. The leaf node will
|
|
|
|
** be pulled from left subtrees whenever possible (this was
|
|
|
|
** an arbitrary decision). No attempt is made to pull the leaf
|
|
|
|
** from the deepest subtree (to minimize length). The branch
|
|
|
|
** point for the replacement node is changed to be the same as
|
|
|
|
** the branch point of the deleted node. This keeps us from
|
|
|
|
** having to rearrange the tree every time we delete a node.
|
|
|
|
** Also, the LeftBranch and RightBranch numbers of the
|
|
|
|
** replacement node are set to be the same as the deleted node.
|
|
|
|
** The makes the delete easier and more efficient, but it may
|
|
|
|
** make searches in the tree less efficient after many nodes are
|
|
|
|
** deleted. If the node specified by Key and Data does not
|
|
|
|
** exist in the tree, then nothing is done.
|
|
|
|
** Return: none
|
|
|
|
** None
|
|
|
|
** Exceptions: none
|
|
|
|
** None
|
|
|
|
** History: 3/13/89, DSJ, Created.
|
|
|
|
** 7/13/89, DSJ, Specify node indirectly by key and data.
|
|
|
|
*/
|
|
|
|
int Level;
|
|
|
|
KDNODE *Current;
|
|
|
|
KDNODE *Father;
|
|
|
|
KDNODE *Replacement;
|
|
|
|
KDNODE *FatherReplacement;
|
|
|
|
|
|
|
|
/* initialize search at root of tree */
|
|
|
|
N = Tree->KeySize;
|
|
|
|
KeyDesc = &(Tree->KeyDesc[0]);
|
|
|
|
Father = &(Tree->Root);
|
|
|
|
Current = Father->Left;
|
2008-02-01 08:07:59 +08:00
|
|
|
Level = NextLevel(-1);
|
2007-03-08 04:03:40 +08:00
|
|
|
|
|
|
|
/* search tree for node to be deleted */
|
|
|
|
while ((Current != NULL) && (!NodeFound (Current, Key, Data))) {
|
|
|
|
Father = Current;
|
|
|
|
if (Key[Level] < Current->BranchPoint)
|
|
|
|
Current = Current->Left;
|
|
|
|
else
|
|
|
|
Current = Current->Right;
|
|
|
|
|
2008-02-01 08:07:59 +08:00
|
|
|
Level = NextLevel(Level);
|
2007-03-08 04:03:40 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
if (Current != NULL) { /* if node to be deleted was found */
|
|
|
|
Replacement = Current;
|
|
|
|
FatherReplacement = Father;
|
|
|
|
|
|
|
|
/* search for replacement node (a leaf under node to be deleted */
|
|
|
|
while (TRUE) {
|
|
|
|
if (Replacement->Left != NULL) {
|
|
|
|
FatherReplacement = Replacement;
|
|
|
|
Replacement = Replacement->Left;
|
|
|
|
}
|
|
|
|
else if (Replacement->Right != NULL) {
|
|
|
|
FatherReplacement = Replacement;
|
|
|
|
Replacement = Replacement->Right;
|
|
|
|
}
|
|
|
|
else
|
|
|
|
break;
|
|
|
|
|
2008-02-01 08:07:59 +08:00
|
|
|
Level = NextLevel(Level);
|
2007-03-08 04:03:40 +08:00
|
|
|
}
|
|
|
|
|
|
|
|
/* compute level of replacement node's father */
|
2008-02-01 08:07:59 +08:00
|
|
|
Level = PrevLevel(Level);
|
2007-03-08 04:03:40 +08:00
|
|
|
|
|
|
|
/* disconnect replacement node from it's father */
|
|
|
|
if (FatherReplacement->Left == Replacement) {
|
|
|
|
FatherReplacement->Left = NULL;
|
|
|
|
FatherReplacement->LeftBranch = KeyDesc[Level].Min;
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
FatherReplacement->Right = NULL;
|
|
|
|
FatherReplacement->RightBranch = KeyDesc[Level].Max;
|
|
|
|
}
|
|
|
|
|
|
|
|
/* replace deleted node with replacement (unless they are the same) */
|
|
|
|
if (Replacement != Current) {
|
|
|
|
Replacement->BranchPoint = Current->BranchPoint;
|
|
|
|
Replacement->LeftBranch = Current->LeftBranch;
|
|
|
|
Replacement->RightBranch = Current->RightBranch;
|
|
|
|
Replacement->Left = Current->Left;
|
|
|
|
Replacement->Right = Current->Right;
|
|
|
|
|
|
|
|
if (Father->Left == Current)
|
|
|
|
Father->Left = Replacement;
|
|
|
|
else
|
|
|
|
Father->Right = Replacement;
|
|
|
|
}
|
2008-02-01 08:07:59 +08:00
|
|
|
FreeKDNode(Current);
|
2007-03-08 04:03:40 +08:00
|
|
|
}
|
|
|
|
} /* KDDelete */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
int
|
|
|
|
KDNearestNeighborSearch (KDTREE * Tree,
|
|
|
|
FLOAT32 Query[],
|
|
|
|
int QuerySize,
|
|
|
|
FLOAT32 MaxDistance,
|
|
|
|
void *NBuffer, FLOAT32 DBuffer[]) {
|
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** Tree ptr to K-D tree to be searched
|
|
|
|
** Query ptr to query key (point in D-space)
|
|
|
|
** QuerySize number of nearest neighbors to be found
|
|
|
|
** MaxDistance all neighbors must be within this distance
|
|
|
|
** NBuffer ptr to QuerySize buffer to hold nearest neighbors
|
|
|
|
** DBuffer ptr to QuerySize buffer to hold distances
|
|
|
|
** from nearest neighbor to query point
|
|
|
|
** Globals:
|
|
|
|
** NumberOfNeighbors # of neighbors found so far
|
|
|
|
** N # of features in each key
|
|
|
|
** KeyDesc description of tree dimensions
|
|
|
|
** QueryPoint point in D-space to find neighbors of
|
|
|
|
** MaxNeighbors maximum # of neighbors to find
|
|
|
|
** Radius current distance of furthest neighbor
|
|
|
|
** Furthest index of furthest neighbor
|
|
|
|
** Neighbor buffer of current neighbors
|
|
|
|
** Distance buffer of neighbor distances
|
|
|
|
** SBMin lower extent of small search region
|
|
|
|
** SBMax upper extent of small search region
|
|
|
|
** LBMin lower extent of large search region
|
|
|
|
** LBMax upper extent of large search region
|
|
|
|
** QuickExit quick exit from recursive search
|
|
|
|
** Operation:
|
|
|
|
** This routine searches the K-D tree specified by Tree and
|
|
|
|
** finds the QuerySize nearest neighbors of Query. All neighbors
|
|
|
|
** must be within MaxDistance of Query. The data contents of
|
|
|
|
** the nearest neighbors
|
|
|
|
** are placed in NBuffer and their distances from Query are
|
|
|
|
** placed in DBuffer.
|
|
|
|
** Return: Number of nearest neighbors actually found
|
|
|
|
** Exceptions: none
|
|
|
|
** History:
|
|
|
|
** 3/10/89, DSJ, Created.
|
|
|
|
** 7/13/89, DSJ, Return contents of node instead of node itself.
|
|
|
|
*/
|
|
|
|
int i;
|
|
|
|
|
|
|
|
NumberOfNeighbors = 0;
|
|
|
|
N = Tree->KeySize;
|
|
|
|
KeyDesc = &(Tree->KeyDesc[0]);
|
|
|
|
QueryPoint = Query;
|
|
|
|
MaxNeighbors = QuerySize;
|
|
|
|
Radius = MaxDistance;
|
|
|
|
Furthest = 0;
|
|
|
|
Neighbor = (char **) NBuffer;
|
|
|
|
Distance = DBuffer;
|
|
|
|
|
|
|
|
for (i = 0; i < N; i++) {
|
|
|
|
SBMin[i] = KeyDesc[i].Min;
|
|
|
|
SBMax[i] = KeyDesc[i].Max;
|
|
|
|
LBMin[i] = KeyDesc[i].Min;
|
|
|
|
LBMax[i] = KeyDesc[i].Max;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (Tree->Root.Left != NULL) {
|
|
|
|
if (setjmp (QuickExit) == 0)
|
|
|
|
Search (0, Tree->Root.Left);
|
|
|
|
}
|
|
|
|
return (NumberOfNeighbors);
|
|
|
|
} /* KDNearestNeighborSearch */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
2008-02-01 08:07:59 +08:00
|
|
|
void KDWalk(KDTREE *Tree, void_proc Action) {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** Tree ptr to K-D tree to be walked
|
|
|
|
** Action ptr to function to be executed at each node
|
|
|
|
** Globals:
|
|
|
|
** WalkAction action to be performed at every node
|
|
|
|
** Operation:
|
|
|
|
** This routine stores the desired action in a global
|
|
|
|
** variable and starts a recursive walk of Tree. The walk
|
|
|
|
** is started at the root node.
|
|
|
|
** Return:
|
|
|
|
** None
|
|
|
|
** Exceptions:
|
|
|
|
** None
|
|
|
|
** History:
|
|
|
|
** 3/13/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
WalkAction = Action;
|
|
|
|
if (Tree->Root.Left != NULL)
|
2008-02-01 08:07:59 +08:00
|
|
|
Walk (Tree->Root.Left, NextLevel(-1));
|
2007-03-08 04:03:40 +08:00
|
|
|
} /* KDWalk */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
2008-02-01 08:07:59 +08:00
|
|
|
void FreeKDTree(KDTREE *Tree) {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** Tree tree data structure to be released
|
|
|
|
** Globals: none
|
|
|
|
** Operation:
|
|
|
|
** This routine frees all memory which is allocated to the
|
|
|
|
** specified KD-tree. This includes the data structure for
|
|
|
|
** the kd-tree itself plus the data structures for each node
|
|
|
|
** in the tree. It does not include the Key and Data items
|
|
|
|
** which are pointed to by the nodes. This memory is left
|
|
|
|
** untouched.
|
|
|
|
** Return: none
|
|
|
|
** Exceptions: none
|
|
|
|
** History:
|
|
|
|
** 5/26/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
FreeSubTree (Tree->Root.Left);
|
2008-02-01 08:07:59 +08:00
|
|
|
memfree(Tree);
|
2007-03-08 04:03:40 +08:00
|
|
|
} /* FreeKDTree */
|
|
|
|
|
|
|
|
|
|
|
|
/**----------------------------------------------------------------------------
|
|
|
|
Private Code
|
|
|
|
----------------------------------------------------------------------------**/
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
int
|
|
|
|
Equal (FLOAT32 Key1[], FLOAT32 Key2[]) {
|
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** Key1,Key2 search keys to be compared for equality
|
|
|
|
** Globals:
|
|
|
|
** N number of parameters per key
|
|
|
|
** Operation:
|
|
|
|
** This routine returns TRUE if Key1 = Key2.
|
|
|
|
** Return:
|
|
|
|
** TRUE if Key1 = Key2, else FALSE.
|
|
|
|
** Exceptions:
|
|
|
|
** None
|
|
|
|
** History:
|
|
|
|
** 3/11/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
int i;
|
|
|
|
|
|
|
|
for (i = N; i > 0; i--, Key1++, Key2++)
|
|
|
|
if (*Key1 != *Key2)
|
|
|
|
return (FALSE);
|
|
|
|
return (TRUE);
|
|
|
|
} /* Equal */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
KDNODE *
|
|
|
|
MakeKDNode (FLOAT32 Key[], char *Data, int Index) {
|
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** Key Access key for new node in KD tree
|
|
|
|
** Data ptr to data to be stored in new node
|
|
|
|
** Index index of Key to branch on
|
|
|
|
** Globals:
|
|
|
|
** KeyDesc descriptions of key dimensions
|
|
|
|
** Operation:
|
|
|
|
** This routine allocates memory for a new K-D tree node
|
|
|
|
** and places the specified Key and Data into it. The
|
|
|
|
** left and right subtree pointers for the node are
|
|
|
|
** initialized to empty subtrees.
|
|
|
|
** Return:
|
|
|
|
** pointer to new K-D tree node
|
|
|
|
** Exceptions:
|
|
|
|
** None
|
|
|
|
** History:
|
|
|
|
** 3/11/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
KDNODE *NewNode;
|
|
|
|
|
|
|
|
NewNode = (KDNODE *) Emalloc (sizeof (KDNODE));
|
|
|
|
|
|
|
|
NewNode->Key = Key;
|
|
|
|
NewNode->Data = Data;
|
|
|
|
NewNode->BranchPoint = Key[Index];
|
|
|
|
NewNode->LeftBranch = KeyDesc[Index].Min;
|
|
|
|
NewNode->RightBranch = KeyDesc[Index].Max;
|
|
|
|
NewNode->Left = NULL;
|
|
|
|
NewNode->Right = NULL;
|
|
|
|
|
|
|
|
return (NewNode);
|
|
|
|
} /* MakeKDNode */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
2008-02-01 08:07:59 +08:00
|
|
|
void FreeKDNode(KDNODE *Node) {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** Node ptr to node data structure to be freed
|
|
|
|
** Globals:
|
|
|
|
** None
|
|
|
|
** Operation:
|
|
|
|
** This routine frees up the memory allocated to Node.
|
|
|
|
** Return:
|
|
|
|
** None
|
|
|
|
** Exceptions:
|
|
|
|
** None
|
|
|
|
** History:
|
|
|
|
** 3/13/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
memfree ((char *) Node);
|
|
|
|
} /* FreeKDNode */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
2008-02-01 08:07:59 +08:00
|
|
|
void Search(int Level, KDNODE *SubTree) {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** Level level in tree of sub-tree to be searched
|
|
|
|
** SubTree sub-tree to be searched
|
|
|
|
** Globals:
|
|
|
|
** NumberOfNeighbors # of neighbors found so far
|
|
|
|
** N # of features in each key
|
|
|
|
** KeyDesc description of key dimensions
|
|
|
|
** QueryPoint point in D-space to find neighbors of
|
|
|
|
** MaxNeighbors maximum # of neighbors to find
|
|
|
|
** Radius current distance of furthest neighbor
|
|
|
|
** Furthest index of furthest neighbor
|
|
|
|
** Neighbor buffer of current neighbors
|
|
|
|
** Distance buffer of neighbor distances
|
|
|
|
** SBMin lower extent of small search region
|
|
|
|
** SBMax upper extent of small search region
|
|
|
|
** LBMin lower extent of large search region
|
|
|
|
** LBMax upper extent of large search region
|
|
|
|
** QuickExit quick exit from recursive search
|
|
|
|
** Operation:
|
|
|
|
** This routine searches SubTree for those entries which are
|
|
|
|
** possibly among the MaxNeighbors nearest neighbors of the
|
|
|
|
** QueryPoint and places their data in the Neighbor buffer and
|
|
|
|
** their distances from QueryPoint in the Distance buffer.
|
|
|
|
** Return: none
|
|
|
|
** Exceptions: none
|
|
|
|
** History:
|
|
|
|
** 3/11/89, DSJ, Created.
|
|
|
|
** 7/13/89, DSJ, Save node contents, not node, in neighbor buffer
|
|
|
|
*/
|
|
|
|
FLOAT32 d;
|
|
|
|
FLOAT32 OldSBoxEdge;
|
|
|
|
FLOAT32 OldLBoxEdge;
|
|
|
|
|
|
|
|
if (Level >= N)
|
|
|
|
Level = 0;
|
|
|
|
|
|
|
|
d = ComputeDistance (N, KeyDesc, QueryPoint, SubTree->Key);
|
|
|
|
if (d < Radius) {
|
|
|
|
if (NumberOfNeighbors < MaxNeighbors) {
|
|
|
|
Neighbor[NumberOfNeighbors] = SubTree->Data;
|
|
|
|
Distance[NumberOfNeighbors] = d;
|
|
|
|
NumberOfNeighbors++;
|
|
|
|
if (NumberOfNeighbors == MaxNeighbors)
|
2008-02-01 08:07:59 +08:00
|
|
|
FindMaxDistance();
|
2007-03-08 04:03:40 +08:00
|
|
|
}
|
|
|
|
else {
|
|
|
|
Neighbor[Furthest] = SubTree->Data;
|
|
|
|
Distance[Furthest] = d;
|
2008-02-01 08:07:59 +08:00
|
|
|
FindMaxDistance();
|
2007-03-08 04:03:40 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
if (QueryPoint[Level] < SubTree->BranchPoint) {
|
|
|
|
OldSBoxEdge = SBMax[Level];
|
|
|
|
SBMax[Level] = SubTree->LeftBranch;
|
|
|
|
OldLBoxEdge = LBMax[Level];
|
|
|
|
LBMax[Level] = SubTree->RightBranch;
|
|
|
|
if (SubTree->Left != NULL)
|
2008-02-01 08:07:59 +08:00
|
|
|
Search (NextLevel(Level), SubTree->Left);
|
2007-03-08 04:03:40 +08:00
|
|
|
SBMax[Level] = OldSBoxEdge;
|
|
|
|
LBMax[Level] = OldLBoxEdge;
|
|
|
|
OldSBoxEdge = SBMin[Level];
|
|
|
|
SBMin[Level] = SubTree->RightBranch;
|
|
|
|
OldLBoxEdge = LBMin[Level];
|
|
|
|
LBMin[Level] = SubTree->LeftBranch;
|
|
|
|
if ((SubTree->Right != NULL) && QueryIntersectsSearch ())
|
2008-02-01 08:07:59 +08:00
|
|
|
Search (NextLevel(Level), SubTree->Right);
|
2007-03-08 04:03:40 +08:00
|
|
|
SBMin[Level] = OldSBoxEdge;
|
|
|
|
LBMin[Level] = OldLBoxEdge;
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
OldSBoxEdge = SBMin[Level];
|
|
|
|
SBMin[Level] = SubTree->RightBranch;
|
|
|
|
OldLBoxEdge = LBMin[Level];
|
|
|
|
LBMin[Level] = SubTree->LeftBranch;
|
|
|
|
if (SubTree->Right != NULL)
|
2008-02-01 08:07:59 +08:00
|
|
|
Search (NextLevel(Level), SubTree->Right);
|
2007-03-08 04:03:40 +08:00
|
|
|
SBMin[Level] = OldSBoxEdge;
|
|
|
|
LBMin[Level] = OldLBoxEdge;
|
|
|
|
OldSBoxEdge = SBMax[Level];
|
|
|
|
SBMax[Level] = SubTree->LeftBranch;
|
|
|
|
OldLBoxEdge = LBMax[Level];
|
|
|
|
LBMax[Level] = SubTree->RightBranch;
|
|
|
|
if ((SubTree->Left != NULL) && QueryIntersectsSearch ())
|
2008-02-01 08:07:59 +08:00
|
|
|
Search (NextLevel(Level), SubTree->Left);
|
2007-03-08 04:03:40 +08:00
|
|
|
SBMax[Level] = OldSBoxEdge;
|
|
|
|
LBMax[Level] = OldLBoxEdge;
|
|
|
|
}
|
|
|
|
if (QueryInSearch ())
|
|
|
|
longjmp (QuickExit, 1);
|
|
|
|
} /* Search */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
|
|
|
FLOAT32
|
|
|
|
ComputeDistance (register int N,
|
|
|
|
register PARAM_DESC Dim[],
|
|
|
|
register FLOAT32 p1[], register FLOAT32 p2[]) {
|
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** N number of dimensions in K-D space
|
|
|
|
** Dim descriptions of each dimension
|
|
|
|
** p1,p2 two different points in K-D space
|
|
|
|
** Globals:
|
|
|
|
** None
|
|
|
|
** Operation:
|
|
|
|
** This routine computes the euclidian distance
|
|
|
|
** between p1 and p2 in K-D space (an N dimensional space).
|
|
|
|
** Return:
|
|
|
|
** Distance between p1 and p2.
|
|
|
|
** Exceptions:
|
|
|
|
** None
|
|
|
|
** History:
|
|
|
|
** 3/11/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
register FLOAT32 TotalDistance;
|
|
|
|
register FLOAT32 DimensionDistance;
|
|
|
|
FLOAT32 WrapDistance;
|
|
|
|
|
|
|
|
TotalDistance = 0;
|
|
|
|
for (; N > 0; N--, p1++, p2++, Dim++) {
|
|
|
|
if (Dim->NonEssential)
|
|
|
|
continue;
|
|
|
|
|
|
|
|
DimensionDistance = *p1 - *p2;
|
|
|
|
|
|
|
|
/* if this dimension is circular - check wraparound distance */
|
|
|
|
if (Dim->Circular) {
|
|
|
|
DimensionDistance = Magnitude (DimensionDistance);
|
|
|
|
WrapDistance = Dim->Max - Dim->Min - DimensionDistance;
|
|
|
|
DimensionDistance = MIN (DimensionDistance, WrapDistance);
|
|
|
|
}
|
|
|
|
|
|
|
|
TotalDistance += DimensionDistance * DimensionDistance;
|
|
|
|
}
|
|
|
|
return ((FLOAT32) sqrt ((FLOAT64) TotalDistance));
|
|
|
|
} /* ComputeDistance */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
2008-02-01 08:07:59 +08:00
|
|
|
void FindMaxDistance() {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** None
|
|
|
|
** Globals:
|
|
|
|
** MaxNeighbors maximum # of neighbors to find
|
|
|
|
** Radius current distance of furthest neighbor
|
|
|
|
** Furthest index of furthest neighbor
|
|
|
|
** Distance buffer of neighbor distances
|
|
|
|
** Operation:
|
|
|
|
** This routine searches the Distance buffer for the maximum
|
|
|
|
** distance, places this distance in Radius, and places the
|
|
|
|
** index of this distance in Furthest.
|
|
|
|
** Return:
|
|
|
|
** None
|
|
|
|
** Exceptions:
|
|
|
|
** None
|
|
|
|
** History:
|
|
|
|
** 3/11/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
int i;
|
|
|
|
|
|
|
|
Radius = Distance[Furthest];
|
|
|
|
for (i = 0; i < MaxNeighbors; i++) {
|
|
|
|
if (Distance[i] > Radius) {
|
|
|
|
Radius = Distance[i];
|
|
|
|
Furthest = i;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
} /* FindMaxDistance */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
2008-02-01 08:07:59 +08:00
|
|
|
int QueryIntersectsSearch() {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** None
|
|
|
|
** Globals:
|
|
|
|
** N # of features in each key
|
|
|
|
** KeyDesc descriptions of each dimension
|
|
|
|
** QueryPoint point in D-space to find neighbors of
|
|
|
|
** Radius current distance of furthest neighbor
|
|
|
|
** SBMin lower extent of small search region
|
|
|
|
** SBMax upper extent of small search region
|
|
|
|
** Operation:
|
|
|
|
** This routine returns TRUE if the query region intersects
|
|
|
|
** the current smallest search region. The query region is
|
|
|
|
** the circle of radius Radius centered at QueryPoint.
|
|
|
|
** The smallest search region is the box (in N dimensions)
|
|
|
|
** whose edges in each dimension are specified by SBMin and SBMax.
|
|
|
|
** In the case of circular dimensions, we must also check the
|
|
|
|
** point which is one wrap-distance away from the query to
|
|
|
|
** see if it would intersect the search region.
|
|
|
|
** Return:
|
|
|
|
** TRUE if query region intersects search region, else FALSE
|
|
|
|
** Exceptions:
|
|
|
|
** None
|
|
|
|
** History:
|
|
|
|
** 3/11/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
register int i;
|
|
|
|
register FLOAT32 *Query;
|
|
|
|
register FLOAT32 *Lower;
|
|
|
|
register FLOAT32 *Upper;
|
|
|
|
register FLOAT64 TotalDistance;
|
|
|
|
register FLOAT32 DimensionDistance;
|
|
|
|
register FLOAT64 RadiusSquared;
|
|
|
|
register PARAM_DESC *Dim;
|
|
|
|
register FLOAT32 WrapDistance;
|
|
|
|
|
|
|
|
RadiusSquared = Radius * Radius;
|
|
|
|
Query = QueryPoint;
|
|
|
|
Lower = SBMin;
|
|
|
|
Upper = SBMax;
|
|
|
|
TotalDistance = 0.0;
|
|
|
|
Dim = KeyDesc;
|
|
|
|
for (i = N; i > 0; i--, Dim++, Query++, Lower++, Upper++) {
|
|
|
|
if (Dim->NonEssential)
|
|
|
|
continue;
|
|
|
|
|
|
|
|
if (*Query < *Lower)
|
|
|
|
DimensionDistance = *Lower - *Query;
|
|
|
|
else if (*Query > *Upper)
|
|
|
|
DimensionDistance = *Query - *Upper;
|
|
|
|
else
|
|
|
|
DimensionDistance = 0;
|
|
|
|
|
|
|
|
/* if this dimension is circular - check wraparound distance */
|
|
|
|
if (Dim->Circular) {
|
|
|
|
if (*Query < *Lower)
|
|
|
|
WrapDistance = *Query + Dim->Max - Dim->Min - *Upper;
|
|
|
|
else if (*Query > *Upper)
|
|
|
|
WrapDistance = *Lower - (*Query - (Dim->Max - Dim->Min));
|
|
|
|
else
|
|
|
|
WrapDistance = MAX_FLOAT32;
|
|
|
|
|
|
|
|
DimensionDistance = MIN (DimensionDistance, WrapDistance);
|
|
|
|
}
|
|
|
|
|
|
|
|
TotalDistance += DimensionDistance * DimensionDistance;
|
|
|
|
if (TotalDistance >= RadiusSquared)
|
|
|
|
return (FALSE);
|
|
|
|
}
|
|
|
|
return (TRUE);
|
|
|
|
} /* QueryIntersectsSearch */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
2008-02-01 08:07:59 +08:00
|
|
|
int QueryInSearch() {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** None
|
|
|
|
** Globals:
|
|
|
|
** N # of features in each key
|
|
|
|
** KeyDesc descriptions of each dimension
|
|
|
|
** QueryPoint point in D-space to find neighbors of
|
|
|
|
** Radius current distance of furthest neighbor
|
|
|
|
** LBMin lower extent of large search region
|
|
|
|
** LBMax upper extent of large search region
|
|
|
|
** Operation:
|
|
|
|
** This routine returns TRUE if the current query region is
|
|
|
|
** totally contained in the current largest search region.
|
|
|
|
** The query region is the circle of
|
|
|
|
** radius Radius centered at QueryPoint. The search region is
|
|
|
|
** the box (in N dimensions) whose edges in each
|
|
|
|
** dimension are specified by LBMin and LBMax.
|
|
|
|
** Return:
|
|
|
|
** TRUE if query region is inside search region, else FALSE
|
|
|
|
** Exceptions:
|
|
|
|
** None
|
|
|
|
** History:
|
|
|
|
** 3/11/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
register int i;
|
|
|
|
register FLOAT32 *Query;
|
|
|
|
register FLOAT32 *Lower;
|
|
|
|
register FLOAT32 *Upper;
|
|
|
|
register PARAM_DESC *Dim;
|
|
|
|
|
|
|
|
Query = QueryPoint;
|
|
|
|
Lower = LBMin;
|
|
|
|
Upper = LBMax;
|
|
|
|
Dim = KeyDesc;
|
|
|
|
|
|
|
|
for (i = N - 1; i >= 0; i--, Dim++, Query++, Lower++, Upper++) {
|
|
|
|
if (Dim->NonEssential)
|
|
|
|
continue;
|
|
|
|
|
|
|
|
if ((*Query < *Lower + Radius) || (*Query > *Upper - Radius))
|
|
|
|
return (FALSE);
|
|
|
|
}
|
|
|
|
return (TRUE);
|
|
|
|
} /* QueryInSearch */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
2008-04-22 08:23:04 +08:00
|
|
|
void Walk(KDNODE *SubTree, inT32 Level) {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** SubTree ptr to root of subtree to be walked
|
|
|
|
** Level current level in the tree for this node
|
|
|
|
** Globals:
|
|
|
|
** WalkAction action to be performed at every node
|
|
|
|
** Operation:
|
|
|
|
** This routine walks thru the specified SubTree and invokes
|
|
|
|
** WalkAction at each node. WalkAction is invoked with three
|
|
|
|
** arguments as follows:
|
|
|
|
** WalkAction( NodeData, Order, Level )
|
|
|
|
** Data is the data contents of the node being visited,
|
|
|
|
** Order is either preorder,
|
|
|
|
** postorder, endorder, or leaf depending on whether this is
|
|
|
|
** the 1st, 2nd, or 3rd time a node has been visited, or
|
|
|
|
** whether the node is a leaf. Level is the level of the node in
|
|
|
|
** the tree with the root being level 0.
|
|
|
|
** Return: none
|
|
|
|
** Exceptions: none
|
|
|
|
** History:
|
|
|
|
** 3/13/89, DSJ, Created.
|
|
|
|
** 7/13/89, DSJ, Pass node contents, not node, to WalkAction().
|
|
|
|
*/
|
|
|
|
if ((SubTree->Left == NULL) && (SubTree->Right == NULL))
|
|
|
|
(*WalkAction) (SubTree->Data, leaf, Level);
|
|
|
|
else {
|
|
|
|
(*WalkAction) (SubTree->Data, preorder, Level);
|
|
|
|
if (SubTree->Left != NULL)
|
2008-02-01 08:07:59 +08:00
|
|
|
Walk (SubTree->Left, NextLevel(Level));
|
2007-03-08 04:03:40 +08:00
|
|
|
(*WalkAction) (SubTree->Data, postorder, Level);
|
|
|
|
if (SubTree->Right != NULL)
|
2008-02-01 08:07:59 +08:00
|
|
|
Walk (SubTree->Right, NextLevel(Level));
|
2007-03-08 04:03:40 +08:00
|
|
|
(*WalkAction) (SubTree->Data, endorder, Level);
|
|
|
|
}
|
|
|
|
} /* Walk */
|
|
|
|
|
|
|
|
|
|
|
|
/*---------------------------------------------------------------------------*/
|
2008-02-01 08:07:59 +08:00
|
|
|
void FreeSubTree(KDNODE *SubTree) {
|
2007-03-08 04:03:40 +08:00
|
|
|
/*
|
|
|
|
** Parameters:
|
|
|
|
** SubTree ptr to root node of sub-tree to be freed
|
|
|
|
** Globals: none
|
|
|
|
** Operation:
|
|
|
|
** This routine recursively frees the memory allocated to
|
|
|
|
** to the specified subtree.
|
|
|
|
** Return: none
|
|
|
|
** Exceptions: none
|
|
|
|
** History: 7/13/89, DSJ, Created.
|
|
|
|
*/
|
|
|
|
if (SubTree != NULL) {
|
|
|
|
FreeSubTree (SubTree->Left);
|
|
|
|
FreeSubTree (SubTree->Right);
|
2008-02-01 08:07:59 +08:00
|
|
|
memfree(SubTree);
|
2007-03-08 04:03:40 +08:00
|
|
|
}
|
|
|
|
} /* FreeSubTree */
|