greg/kdtree
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

moving current code to trunk

Browse source 
git-svn-id: http://kdtree.googlecode.com/svn/trunk@24 58b2c0e6-ac2f-0410-a5b6-b51bcff41737
This commit is contained in:
jtsiomb 2009-08-27 20:18:27 +00:00
parent 4d8a628a38
commit 6fc3af0008
10 changed files with 1330 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

36
kdtree/Makefile.in Normal file
View file

@ -0,0 +1,36 @@
obj = kdtree.o
lib_a = libkdtree.a
lib_so = libkdtree.$(so_suffix)
CC = gcc
CFLAGS = -pedantic -Wall -g -O3 $(pic) $(opt) $(dbg) $(falloc) $(pthreads)
LDFLAGS = $(ldpthread)
.PHONY: all
all: $(lib_a) $(lib_so)
$(lib_a): $(obj)
$(AR) rcs $@ $(obj)
$(lib_so): $(obj)
$(CC) $(shared) -o $@ $(obj) $(LDFLAGS)
.PHONY: examples
examples:
cd examples; $(MAKE)
.PHONY: clean
clean:
rm -f $(obj) $(lib_a) $(lib_so)
.PHONY: install
install:
cp kdtree.h $(PREFIX)/include/kdtree.h
cp $(lib_so) $(PREFIX)/lib/$(lib_so)
cp $(lib_a) $(PREFIX)/lib/$(lib_a)
.PHONY: uninstall
uninstall:
rm -f $(PREFIX)/include/kdtree.h
rm -f $(PREFIX)/lib/$(lib_so)
rm -f $(PREFIX)/lib/$(lib_a)

96
kdtree/configure vendored Executable file
View file

@ -0,0 +1,96 @@
#!/bin/sh
echo 'configuring kdtree ...'
PREFIX=/usr/local
OPT=yes
DBG=yes
FALLOC=yes
PTHREAD=yes
for arg; do
case "$arg" in
--prefix=*)
value=`echo $arg | sed 's/--prefix=//'`
PREFIX=${value:-$prefix}
;;
--enable-opt)
OPT=yes;;
--disable-opt)
OPT=no;;
--enable-debug)
DBG=yes;;
--disable-debug)
DBG=no;;
--enable-pthread)
PTHREAD=yes;;
--disable-pthread)
PTHREAD=no;;
--enable-fastalloc)
FALLOC=yes;;
--disable-fastalloc)
FALLOC=no;;
--help)
echo 'usage: ./configure [options]'
echo 'options:'
echo ' --prefix=<path>: installation path (default: /usr/local)'
echo ' --enable-fastalloc: enable fast result node allocator (default)'
echo ' --disable-fastalloc: disable fast result node allocator'
echo ' --enable-pthread: enable pthread support (default if fastalloc is enabled)'
echo " --disable-pthread: disable pthread support (don't)"
echo ' --enable-opt: enable speed optimizations (default)'
echo ' --disable-opt: disable speed optimizations'
echo ' --enable-debug: include debugging symbols (default)'
echo ' --disable-debug: do not include debugging symbols'
echo 'all invalid options are silently ignored'
exit 0
;;
esac
done
echo "prefix: $PREFIX"
echo "optimize for speed: $OPT"
echo "include debugging symbols: $DBG"
echo "fast node allocator: $FALLOC"
if [ "$FALLOC" = "yes" ]; then
echo "pthread support: $PTHREAD"
fi
# create makefile
echo 'creating makefile ...'
echo "PREFIX = $PREFIX" >Makefile
if [ "$DBG" = 'yes' ]; then
echo 'dbg = -g' >>Makefile
fi
if [ "$OPT" = 'yes' ]; then
echo 'opt = -O3' >>Makefile
fi
if [ "$FALLOC" = 'yes' ]; then
echo 'falloc = -DUSE_LIST_NODE_ALLOCATOR' >>Makefile
fi
if [ "$PTHREAD" = 'no' ]; then
echo 'pthreads = -DNO_PTHREADS' >>Makefile
else
echo 'ldpthread = -lpthread' >>Makefile
fi
if [ "`uname -s`" = Darwin ]; then
echo 'shared = -dynamiclib' >>Makefile
echo 'so_suffix = dylib' >>Makefile
else
echo 'shared = -shared' >>Makefile
echo 'so_suffix = so' >>Makefile
fi
if [ "`uname -s`" != MINGW32 ]; then
echo 'pic = -fPIC' >>Makefile
fi
cat Makefile.in >>Makefile
echo 'configuration completed, type make (or gmake) to build.'

98
kdtree/doc/guide.html Normal file
View file

@ -0,0 +1,98 @@
<html>
<head>
<title>kdtree library manual - installation</title>
</head>
<body>
<h1>Programming guide</h1>
<p>Using the kdtree library in your programs is very easy.</p>
<h2>Creating a tree</h2>
<p>
Call the <code>kd_create</code> function to create a tree. It takes a single
argument specifying the dimensionality of the data, and returns a pointer to the
tree. You need to pass that pointer as an argument to any functions that
manipulate the kd-tree.
</p>
<p>
For example you may create a 3-dimensional kd-tree with:<br>
<code>void *kd = kd_create(3);</code>
</p>
<h2>Destroying a tree</h2>
<p>
Call <code>kd_free</code> with a pointer returned from <code>kd_create</code> in
order to free the memory occupied by the tree. Note that any data pointers passed
by the user with the <code>kd_insert</code> functions will not be freed, unless a
"<em>data destructor</em>" function is provided (see below).
</p>
<h2>Managing user pointers</h2>
<p>
When inserting data to the tree, you may pass a pointer to be stored in the
node. If you wish, you may provide a custom "destructor" function to be called
for each of these pointers when their node is removed from the tree. You can do
that by supplying a pointer to that destructor function with a call to
<code>kd_data_destructor</code>. The first argument is again a valid pointer to a
kd-tree, while the second argument is a function pointer with the signature:
<code>void (*)(void*)</code>.
</p>
<h2>Populating the tree</h2>
<p>
To insert data to the kd-tree you may use one of the <code>kd_insert</code>
functions.<br>
</p>
<p>
All of the insertion functions take a valid tree pointer as their first
argument, and an optional pointer to user data to be stored along with the node
as their last argument (it can be null if no user data are needed).
</p>
<p>
<code>kd_insert</code>, and <code>kd_insertf</code> expect a
pointer to an array of <em>k</em> <code>double</code>s or <code>float</code>s respectively as
a second argument, which contain the position of the inserted point. So for
example, for a 3D tree you need to pass an array of 3 values.
</p>
<p>
The convenience <code>kd_insert3</code>, and <code>kd_insert3f</code> are meant
to be called for 3-dimensional kd-trees (which is considered the most common
case), and expect 3 values (<code>double</code>s or <code>float</code>s)
signifying the position of the 3-dimensional point to be stored.
</p>
<h2>Performing nearest-neighbor queries</h2>
<p>
After you have your data in the kd-tree, you can perform queries for discoverying
nearest neighbors in a given range around an arbitrary point. The query returns a
pointer to the "<em>result set</em>", which can from then on be accessed with the
<code>kd_res_*</code> functions.
</p>
<p>
The nearest-neighbor queries are performed with the <code>kd_nearest_range</code>
functions. Like the <code>kd_insert</code> functions described above, they also
provide generic array argument versions for k-dimensional trees, and
3-dimensional convenience functions, all in <code>double</code> and
<code>float</code> varieties.
</p>
<p>
For example in order to query for the nearest neighbors around the 2D point (10,
15), inside a radius of 3 units, you could do:<br>
<code>
void *result_set;
double pt[] = {10.0, 15.0};
result_set = kd_nearest_range(kd, pt, 3.0);
</code>
where "kd" is a pointer to a 2-dimensional kd-tree returned by
<code>kd_create(2)</code>.
</p>
<p>
A result set aquired with one of the <code>kd_nearest_range</code> functions,
must be freed by calling <code>kd_res_free</code> and supplying the result set
pointer as its only argument.
</p>
<p>
to be continued...
</p>
</body>
</html>

23
kdtree/doc/index.html Normal file
View file

@ -0,0 +1,23 @@
<html>
<head>
<title>kdtree library manual</title>
</head>
<body>
<h1>kdtree library manual</h1>
<p>This document describes the use of the kdtree library.</p>
<h2>index</h2>
<ul>
<li><a href="install.html">setup kdtree</a></li>
<li><a href="guide.html">kdtree programming guide</a></li>
<li><a href="ref.html">kdtree reference</a></li>
</ul>
<hr>
<p>
Copyright (c) 2007 John Tsiombikas
Redistribution of this document, with or without modification, is permitted
provided that this copyright notice is retained.
</p>
</body>
</html>

41
kdtree/doc/install.html Normal file
View file

@ -0,0 +1,41 @@
<html>
<head>
<title>kdtree library manual - installation</title>
</head>
<body>
<h1>kdtree library manual</h1>
<h2>installation guide</h2>
<p>
Setting up kdtree for use is extremely easy. There are two ways to use this
library: you can either "properly" install it in the system, or you can just
build the kdtree source files along with your program, since the library is very
small.
</p>
<h3>proper installation</h3>
<p>
Just run: <code>./configure</code>, <code>make</code>, and <code>make
install</code> (the last one should be executed as root if you are installing the
library system-wide).
</p>
<p>
Run <code>./configure --help</code> for configuration options. The most important
options are <code>--prefix=&lt;installation path prefix&gt;</code>, and
<code>--disable-fastalloc</code> to disable the fast result node allocator, which
depends on pthreads for locking.
</p>
<h3>kdtree source files in your program</h3>
<p>
Just copy <code>kdtree.c</code> and <code>kdtree.h</code> files and build them along
with your program. Make sure you have <code>USE_LIST_NODE_ALLOCATOR</code>
defined when compiling <code>kdtree.c</code> if you want the fast result node
allocator. If you do so, remember to link with libpthread too.
</p>
<p>
Of course you need to keep the copyright notices intact if you merge the kdtree
source files with your program in any way.
</p>
</body>
</html>

15
kdtree/examples/Makefile Normal file
View file

@ -0,0 +1,15 @@
kdlib = ../libkdtree.a
CC = gcc
CFLAGS = -std=c89 -pedantic -Wall -g -I..
LDFLAGS = $(kdlib) -lm
.PHONY: all
all: test test2
test: test.c $(kdlib)
test2: test2.c $(kdlib)
.PHONY: clean
clean:
rm -f test test2

58
kdtree/examples/test.c Normal file
View file

@ -0,0 +1,58 @@
/*! gcc -Wall -g -o test test.c libkdtree.a */
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <assert.h>
#include <sys/time.h>
#include <time.h>
#include "kdtree.h"
unsigned int get_msec(void)
{
static struct timeval timeval, first_timeval;
gettimeofday(&timeval, 0);
if(first_timeval.tv_sec == 0) {
first_timeval = timeval;
return 0;
}
return (timeval.tv_sec - first_timeval.tv_sec) * 1000 + (timeval.tv_usec - first_timeval.tv_usec) / 1000;
}
int main(int argc, char **argv)
{
int i, vcount = 10;
void *kd, *set;
unsigned int msec, start;
if(argc > 1 && isdigit(argv[1][0])) {
vcount = atoi(argv[1]);
}
printf("inserting %d random vectors... ", vcount);
fflush(stdout);
kd = kd_create(3);
start = get_msec();
for(i=0; i<vcount; i++) {
float x, y, z;
x = ((float)rand() / RAND_MAX) * 200.0 - 100.0;
y = ((float)rand() / RAND_MAX) * 200.0 - 100.0;
z = ((float)rand() / RAND_MAX) * 200.0 - 100.0;
assert(kd_insert3(kd, x, y, z, 0) == 0);
}
msec = get_msec() - start;
printf("%.3f sec\n", (float)msec / 1000.0);
start = get_msec();
set = kd_nearest_range3(kd, 0, 0, 0, 40);
msec = get_msec() - start;
printf("range query returned %d items in %.5f sec\n", kd_res_size(set), (float)msec / 1000.0);
kd_res_free(set);
kd_free(kd);
return 0;
}

88
kdtree/examples/test2.c Normal file
View file

@ -0,0 +1,88 @@
/*! gcc -std=c89 -pedantic -Wall -g -o test2 test2.c libkdtree.a -lm */
/* Extended test program, contributed by David Underhill */
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <time.h>
#include "kdtree.h"
#define DEF_NUM_PTS 10
/* returns the distance squared between two dims-dimensional double arrays */
static double dist_sq( double *a1, double *a2, int dims );
/* get a random double between -10 and 10 */
static double rd( void );
int main(int argc, char **argv) {
int i, num_pts = DEF_NUM_PTS;
void *ptree;
char *data, *pch;
struct kdres *presults;
double pos[3], dist;
double pt[3] = { 0, 0, 1 };
double radius = 10;
if(argc > 1 && isdigit(argv[1][0])) {
num_pts = atoi(argv[1]);
}
if(!(data = malloc(num_pts))) {
perror("malloc failed");
return 1;
}
srand( time(0) );
/* create a k-d tree for 3-dimensional points */
ptree = kd_create( 3 );
/* add some random nodes to the tree (assert nodes are successfully inserted) */
for( i=0; i<num_pts; i++ ) {
data[i] = 'a' + i;
assert( 0 == kd_insert3( ptree, rd(), rd(), rd(), &data[i] ) );
}
/* find points closest to the origin and within distance radius */
presults = kd_nearest_range( ptree, pt, radius );
/* print out all the points found in results */
printf( "found %d results:\n", kd_res_size(presults) );
while( !kd_res_end( presults ) ) {
/* get the data and position of the current result item */
pch = (char*)kd_res_item( presults, pos );
/* compute the distance of the current result from the pt */
dist = sqrt( dist_sq( pt, pos, 3 ) );
/* print out the retrieved data */
printf( "node at (%.3f, %.3f, %.3f) is %.3f away and has data=%c\n",
pos[0], pos[1], pos[2], dist, *pch );
/* go to the next entry */
kd_res_next( presults );
}
/* free our tree, results set, and other allocated memory */
free( data );
kd_res_free( presults );
kd_free( ptree );
return 0;
}
static double dist_sq( double *a1, double *a2, int dims ) {
double dist_sq = 0, diff;
while( --dims >= 0 ) {
diff = (a1[dims] - a2[dims]);
dist_sq += diff*diff;
}
return dist_sq;
}
static double rd( void ) {
return (double)rand()/RAND_MAX * 20.0 - 10.0;
}

761
kdtree/kdtree.c Normal file
View file

@ -0,0 +1,761 @@
/*
This file is part of ``kdtree'', a library for working with kd-trees.
Copyright (C) 2007-2009 John Tsiombikas <nuclear@siggraph.org>
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. The name of the author may not be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE.
*/
/* single nearest neighbor search written by Tamas Nepusz <tamas@cs.rhul.ac.uk> */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "kdtree.h"
#if defined(WIN32) || defined(__WIN32__)
#include <malloc.h>
#endif
#ifdef USE_LIST_NODE_ALLOCATOR
#ifndef NO_PTHREADS
#include <pthread.h>
#else
#ifndef I_WANT_THREAD_BUGS
#error "You are compiling with the fast list node allocator, with pthreads disabled! This WILL break if used from multiple threads."
#endif /* I want thread bugs */
#endif /* pthread support */
#endif /* use list node allocator */
struct kdhyperrect {
int dim;
double *min, *max; /* minimum/maximum coords */
};
struct kdnode {
double *pos;
int dir;
void *data;
struct kdnode *left, *right; /* negative/positive side */
};
struct res_node {
struct kdnode *item;
double dist_sq;
struct res_node *next;
};
struct kdtree {
int dim;
struct kdnode *root;
struct kdhyperrect *rect;
void (*destr)(void*);
};
struct kdres {
struct kdtree *tree;
struct res_node *rlist, *riter;
int size;
};
#define SQ(x) ((x) * (x))
static void clear_rec(struct kdnode *node, void (*destr)(void*));
static int insert_rec(struct kdnode **node, const double *pos, void *data, int dir, int dim);
static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq);
static void clear_results(struct kdres *set);
static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max);
static void hyperrect_free(struct kdhyperrect *rect);
static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect);
static void hyperrect_extend(struct kdhyperrect *rect, const double *pos);
static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos);
#ifdef USE_LIST_NODE_ALLOCATOR
static struct res_node *alloc_resnode(void);
static void free_resnode(struct res_node*);
#else
#define alloc_resnode() malloc(sizeof(struct res_node))
#define free_resnode(n) free(n)
#endif
struct kdtree *kd_create(int k)
{
struct kdtree *tree;
if(!(tree = malloc(sizeof *tree))) {
return 0;
}
tree->dim = k;
tree->root = 0;
tree->destr = 0;
tree->rect = 0;
return tree;
}
void kd_free(struct kdtree *tree)
{
if(tree) {
kd_clear(tree);
free(tree);
}
}
static void clear_rec(struct kdnode *node, void (*destr)(void*))
{
if(!node) return;
clear_rec(node->left, destr);
clear_rec(node->right, destr);
if(destr) {
destr(node->data);
}
free(node->pos);
free(node);
}
void kd_clear(struct kdtree *tree)
{
clear_rec(tree->root, tree->destr);
tree->root = 0;
if (tree->rect) {
hyperrect_free(tree->rect);
tree->rect = 0;
}
}
void kd_data_destructor(struct kdtree *tree, void (*destr)(void*))
{
tree->destr = destr;
}
static int insert_rec(struct kdnode **nptr, const double *pos, void *data, int dir, int dim)
{
int new_dir;
struct kdnode *node;
if(!*nptr) {
if(!(node = malloc(sizeof *node))) {
return -1;
}
if(!(node->pos = malloc(dim * sizeof *node->pos))) {
free(node);
return -1;
}
memcpy(node->pos, pos, dim * sizeof *node->pos);
node->data = data;
node->dir = dir;
node->left = node->right = 0;
*nptr = node;
return 0;
}
node = *nptr;
new_dir = (node->dir + 1) % dim;
if(pos[node->dir] < node->pos[node->dir]) {
return insert_rec(&(*nptr)->left, pos, data, new_dir, dim);
}
return insert_rec(&(*nptr)->right, pos, data, new_dir, dim);
}
int kd_insert(struct kdtree *tree, const double *pos, void *data)
{
if (insert_rec(&tree->root, pos, data, 0, tree->dim)) {
return -1;
}
if (tree->rect == 0) {
tree->rect = hyperrect_create(tree->dim, pos, pos);
} else {
hyperrect_extend(tree->rect, pos);
}
return 0;
}
int kd_insertf(struct kdtree *tree, const float *pos, void *data)
{
static double sbuf[16];
double *bptr, *buf = 0;
int res, dim = tree->dim;
if(dim > 16) {
#ifndef NO_ALLOCA
if(dim <= 256)
bptr = buf = alloca(dim * sizeof *bptr);
else
#endif
if(!(bptr = buf = malloc(dim * sizeof *bptr))) {
return -1;
}
} else {
bptr = sbuf;
}
while(dim-- > 0) {
*bptr++ = *pos++;
}
res = kd_insert(tree, buf, data);
#ifndef NO_ALLOCA
if(tree->dim > 256)
#else
if(tree->dim > 16)
#endif
free(buf);
return res;
}
int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data)
{
double buf[3];
buf[0] = x;
buf[1] = y;
buf[2] = z;
return kd_insert(tree, buf, data);
}
int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data)
{
double buf[3];
buf[0] = x;
buf[1] = y;
buf[2] = z;
return kd_insert(tree, buf, data);
}
static int find_nearest(struct kdnode *node, const double *pos, double range, struct res_node *list, int ordered, int dim)
{
double dist_sq, dx;
int i, ret, added_res = 0;
if(!node) return 0;
dist_sq = 0;
for(i=0; i<dim; i++) {
dist_sq += SQ(node->pos[i] - pos[i]);
}
if(dist_sq <= SQ(range)) {
if(rlist_insert(list, node, ordered ? dist_sq : -1.0) == -1) {
return -1;
}
added_res = 1;
}
dx = pos[node->dir] - node->pos[node->dir];
ret = find_nearest(dx <= 0.0 ? node->left : node->right, pos, range, list, ordered, dim);
if(ret >= 0 && fabs(dx) < range) {
added_res += ret;
ret = find_nearest(dx <= 0.0 ? node->right : node->left, pos, range, list, ordered, dim);
}
if(ret == -1) {
return -1;
}
added_res += ret;
return added_res;
}
static void kd_nearest_i(struct kdnode *node, const double *pos, struct kdnode **result, double *result_dist_sq, struct kdhyperrect* rect)
{
int dir = node->dir;
int i, side;
double dummy, dist_sq;
struct kdnode *nearer_subtree, *farther_subtree;
double *nearer_hyperrect_coord, *farther_hyperrect_coord;
/* Decide whether to go left or right in the tree */
dummy = pos[dir] - node->pos[dir];
if (dummy <= 0) {
nearer_subtree = node->left;
farther_subtree = node->right;
nearer_hyperrect_coord = rect->max + dir;
farther_hyperrect_coord = rect->min + dir;
side = 0;
} else {
nearer_subtree = node->right;
farther_subtree = node->left;
nearer_hyperrect_coord = rect->min + dir;
farther_hyperrect_coord = rect->max + dir;
side = 1;
}
if (nearer_subtree) {
/* Slice the hyperrect to get the hyperrect of the nearer subtree */
dummy = *nearer_hyperrect_coord;
*nearer_hyperrect_coord = node->pos[dir];
/* Recurse down into nearer subtree */
kd_nearest_i(nearer_subtree, pos, result, result_dist_sq, rect);
/* Undo the slice */
*nearer_hyperrect_coord = dummy;
}
/* Check the distance of the point at the current node, compare it
* with our best so far */
dist_sq = 0;
for(i=0; i < rect->dim; i++) {
dist_sq += SQ(node->pos[i] - pos[i]);
}
if (dist_sq < *result_dist_sq) {
*result = node;
*result_dist_sq = dist_sq;
}
if (farther_subtree) {
/* Get the hyperrect of the farther subtree */
dummy = *farther_hyperrect_coord;
*farther_hyperrect_coord = node->pos[dir];
/* Check if we have to recurse down by calculating the closest
* point of the hyperrect and see if it's closer than our
* minimum distance in result_dist_sq. */
if (hyperrect_dist_sq(rect, pos) < *result_dist_sq) {
/* Recurse down into farther subtree */
kd_nearest_i(farther_subtree, pos, result, result_dist_sq, rect);
}
/* Undo the slice on the hyperrect */
*farther_hyperrect_coord = dummy;
}
}
struct kdres *kd_nearest(struct kdtree *kd, const double *pos)
{
struct kdhyperrect *rect;
struct kdnode *result;
struct kdres *rset;
double dist_sq;
int i;
if (!kd) return 0;
if (!kd->rect) return 0;
/* Allocate result set */
if(!(rset = malloc(sizeof *rset))) {
return 0;
}
if(!(rset->rlist = alloc_resnode())) {
free(rset);
return 0;
}
rset->rlist->next = 0;
rset->tree = kd;
/* Duplicate the bounding hyperrectangle, we will work on the copy */
if (!(rect = hyperrect_duplicate(kd->rect))) {
kd_res_free(rset);
return 0;
}
/* Our first guesstimate is the root node */
result = kd->root;
dist_sq = 0;
for (i = 0; i < kd->dim; i++)
dist_sq += SQ(result->pos[i] - pos[i]);
/* Search for the nearest neighbour recursively */
kd_nearest_i(kd->root, pos, &result, &dist_sq, rect);
/* Free the copy of the hyperrect */
hyperrect_free(rect);
/* Store the result */
if (result) {
if (rlist_insert(rset->rlist, result, -1.0) == -1) {
kd_res_free(rset);
return 0;
}
rset->size = 1;
kd_res_rewind(rset);
return rset;
} else {
kd_res_free(rset);
return 0;
}
}
struct kdres *kd_nearestf(struct kdtree *tree, const float *pos)
{
static double sbuf[16];
double *bptr, *buf = 0;
int dim = tree->dim;
struct kdres *res;
if(dim > 16) {
#ifndef NO_ALLOCA
if(dim <= 256)
bptr = buf = alloca(dim * sizeof *bptr);
else
#endif
if(!(bptr = buf = malloc(dim * sizeof *bptr))) {
return 0;
}
} else {
bptr = sbuf;
}
while(dim-- > 0) {
*bptr++ = *pos++;
}
res = kd_nearest(tree, buf);
#ifndef NO_ALLOCA
if(tree->dim > 256)
#else
if(tree->dim > 16)
#endif
free(buf);
return res;
}
struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z)
{
double pos[3];
pos[0] = x;
pos[1] = y;
pos[2] = z;
return kd_nearest(tree, pos);
}
struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z)
{
double pos[3];
pos[0] = x;
pos[1] = y;
pos[2] = z;
return kd_nearest(tree, pos);
}
struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range)
{
int ret;
struct kdres *rset;
if(!(rset = malloc(sizeof *rset))) {
return 0;
}
if(!(rset->rlist = alloc_resnode())) {
free(rset);
return 0;
}
rset->rlist->next = 0;
rset->tree = kd;
if((ret = find_nearest(kd->root, pos, range, rset->rlist, 0, kd->dim)) == -1) {
kd_res_free(rset);
return 0;
}
rset->size = ret;
kd_res_rewind(rset);
return rset;
}
struct kdres *kd_nearest_rangef(struct kdtree *kd, const float *pos, float range)
{
static double sbuf[16];
double *bptr, *buf = 0;
int dim = kd->dim;
struct kdres *res;
if(dim > 16) {
#ifndef NO_ALLOCA
if(dim <= 256)
bptr = buf = alloca(dim * sizeof *bptr);
else
#endif
if(!(bptr = buf = malloc(dim * sizeof *bptr))) {
return 0;
}
} else {
bptr = sbuf;
}
while(dim-- > 0) {
*bptr++ = *pos++;
}
res = kd_nearest_range(kd, buf, range);
#ifndef NO_ALLOCA
if(kd->dim > 256)
#else
if(kd->dim > 16)
#endif
free(buf);
return res;
}
struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range)
{
double buf[3];
buf[0] = x;
buf[1] = y;
buf[2] = z;
return kd_nearest_range(tree, buf, range);
}
struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range)
{
double buf[3];
buf[0] = x;
buf[1] = y;
buf[2] = z;
return kd_nearest_range(tree, buf, range);
}
void kd_res_free(struct kdres *rset)
{
clear_results(rset);
free_resnode(rset->rlist);
free(rset);
}
int kd_res_size(struct kdres *set)
{
return (set->size);
}
void kd_res_rewind(struct kdres *rset)
{
rset->riter = rset->rlist->next;
}
int kd_res_end(struct kdres *rset)
{
return rset->riter == 0;
}
int kd_res_next(struct kdres *rset)
{
rset->riter = rset->riter->next;
return rset->riter != 0;
}
void *kd_res_item(struct kdres *rset, double *pos)
{
if(rset->riter) {
if(pos) {
memcpy(pos, rset->riter->item->pos, rset->tree->dim * sizeof *pos);
}
return rset->riter->item->data;
}
return 0;
}
void *kd_res_itemf(struct kdres *rset, float *pos)
{
if(rset->riter) {
if(pos) {
int i;
for(i=0; i<rset->tree->dim; i++) {
pos[i] = rset->riter->item->pos[i];
}
}
return rset->riter->item->data;
}
return 0;
}
void *kd_res_item3(struct kdres *rset, double *x, double *y, double *z)
{
if(rset->riter) {
if(*x) *x = rset->riter->item->pos[0];
if(*y) *y = rset->riter->item->pos[1];
if(*z) *z = rset->riter->item->pos[2];
}
return 0;
}
void *kd_res_item3f(struct kdres *rset, float *x, float *y, float *z)
{
if(rset->riter) {
if(*x) *x = rset->riter->item->pos[0];
if(*y) *y = rset->riter->item->pos[1];
if(*z) *z = rset->riter->item->pos[2];
}
return 0;
}
void *kd_res_item_data(struct kdres *set)
{
return kd_res_item(set, 0);
}
/* ---- hyperrectangle helpers ---- */
static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max)
{
size_t size = dim * sizeof(double);
struct kdhyperrect* rect = 0;
if (!(rect = malloc(sizeof(struct kdhyperrect)))) {
return 0;
}
rect->dim = dim;
if (!(rect->min = malloc(size))) {
free(rect);
return 0;
}
if (!(rect->max = malloc(size))) {
free(rect->min);
free(rect);
return 0;
}
memcpy(rect->min, min, size);
memcpy(rect->max, max, size);
return rect;
}
static void hyperrect_free(struct kdhyperrect *rect)
{
free(rect->min);
free(rect->max);
free(rect);
}
static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect)
{
return hyperrect_create(rect->dim, rect->min, rect->max);
}
static void hyperrect_extend(struct kdhyperrect *rect, const double *pos)
{
int i;
for (i=0; i < rect->dim; i++) {
if (pos[i] < rect->min[i]) {
rect->min[i] = pos[i];
}
if (pos[i] > rect->max[i]) {
rect->max[i] = pos[i];
}
}
}
static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos)
{
int i;
double result = 0;
for (i=0; i < rect->dim; i++) {
if (pos[i] < rect->min[i]) {
result += SQ(rect->min[i] - pos[i]);
} else if (pos[i] > rect->max[i]) {
result += SQ(rect->max[i] - pos[i]);
}
}
return result;
}
/* ---- static helpers ---- */
#ifdef USE_LIST_NODE_ALLOCATOR
/* special list node allocators. */
static struct res_node *free_nodes;
#ifndef NO_PTHREADS
static pthread_mutex_t alloc_mutex = PTHREAD_MUTEX_INITIALIZER;
#endif
static struct res_node *alloc_resnode(void)
{
struct res_node *node;
#ifndef NO_PTHREADS
pthread_mutex_lock(&alloc_mutex);
#endif
if(!free_nodes) {
node = malloc(sizeof *node);
} else {
node = free_nodes;
free_nodes = free_nodes->next;
node->next = 0;
}
#ifndef NO_PTHREADS
pthread_mutex_unlock(&alloc_mutex);
#endif
return node;
}
static void free_resnode(struct res_node *node)
{
#ifndef NO_PTHREADS
pthread_mutex_lock(&alloc_mutex);
#endif
node->next = free_nodes;
free_nodes = node;
#ifndef NO_PTHREADS
pthread_mutex_unlock(&alloc_mutex);
#endif
}
#endif /* list node allocator or not */
/* inserts the item. if dist_sq is >= 0, then do an ordered insert */
static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq)
{
struct res_node *rnode;
if(!(rnode = alloc_resnode())) {
return -1;
}
rnode->item = item;
rnode->dist_sq = dist_sq;
if(dist_sq >= 0.0) {
while(list->next && list->next->dist_sq < dist_sq) {
list = list->next;
}
}
rnode->next = list->next;
list->next = rnode;
return 0;
}
static void clear_results(struct kdres *rset)
{
struct res_node *tmp, *node = rset->rlist->next;
while(node) {
tmp = node;
node = node->next;
free_resnode(tmp);
}
rset->rlist->next = 0;
}

114
kdtree/kdtree.h Normal file
View file

@ -0,0 +1,114 @@
/*
This file is part of ``kdtree'', a library for working with kd-trees.
Copyright (C) 2007-2009 John Tsiombikas <nuclear@siggraph.org>
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. The name of the author may not be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE.
*/
#ifndef _KDTREE_H_
#define _KDTREE_H_
#ifdef __cplusplus
extern "C" {
#endif
struct kdtree;
struct kdres;
/* create a kd-tree for "k"-dimensional data */
struct kdtree *kd_create(int k);
/* free the struct kdtree */
void kd_free(struct kdtree *tree);
/* remove all the elements from the tree */
void kd_clear(struct kdtree *tree);
/* if called with non-null 2nd argument, the function provided
* will be called on data pointers (see kd_insert) when nodes
* are to be removed from the tree.
*/
void kd_data_destructor(struct kdtree *tree, void (*destr)(void*));
/* insert a node, specifying its position, and optional data */
int kd_insert(struct kdtree *tree, const double *pos, void *data);
int kd_insertf(struct kdtree *tree, const float *pos, void *data);
int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data);
int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data);
/* Find one of the nearest nodes from the specified point.
*
* This function returns a pointer to a result set with at most one element.
*/
struct kdres *kd_nearest(struct kdtree *tree, const double *pos);
struct kdres *kd_nearestf(struct kdtree *tree, const float *pos);
struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z);
struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z);
/* Find any nearest nodes from the specified point within a range.
*
* This function returns a pointer to a result set, which can be manipulated
* by the kd_res_* functions.
* The returned pointer can be null as an indication of an error. Otherwise
* a valid result set is always returned which may contain 0 or more elements.
* The result set must be deallocated with kd_res_free, after use.
*/
struct kdres *kd_nearest_range(struct kdtree *tree, const double *pos, double range);
struct kdres *kd_nearest_rangef(struct kdtree *tree, const float *pos, float range);
struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range);
struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range);
/* frees a result set returned by kd_nearest_range() */
void kd_res_free(struct kdres *set);
/* returns the size of the result set (in elements) */
int kd_res_size(struct kdres *set);
/* rewinds the result set iterator */
void kd_res_rewind(struct kdres *set);
/* returns non-zero if the set iterator reached the end after the last element */
int kd_res_end(struct kdres *set);
/* advances the result set iterator, returns non-zero on success, zero if
* there are no more elements in the result set.
*/
int kd_res_next(struct kdres *set);
/* returns the data pointer (can be null) of the current result set item
* and optionally sets its position to the pointers(s) if not null.
*/
void *kd_res_item(struct kdres *set, double *pos);
void *kd_res_itemf(struct kdres *set, float *pos);
void *kd_res_item3(struct kdres *set, double *x, double *y, double *z);
void *kd_res_item3f(struct kdres *set, float *x, float *y, float *z);
/* equivalent to kd_res_item(set, 0) */
void *kd_res_item_data(struct kdres *set);
#ifdef __cplusplus
}
#endif
#endif /* _KDTREE_H_ */