From 6fc3af0008de04713806b7bbcaa1725dbf97259f Mon Sep 17 00:00:00 2001 From: jtsiomb Date: Thu, 27 Aug 2009 20:18:27 +0000 Subject: [PATCH] moving current code to trunk git-svn-id: http://kdtree.googlecode.com/svn/trunk@24 58b2c0e6-ac2f-0410-a5b6-b51bcff41737 --- kdtree/Makefile.in | 36 ++ kdtree/configure | 96 +++++ kdtree/doc/guide.html | 98 +++++ kdtree/doc/index.html | 23 ++ kdtree/doc/install.html | 41 +++ kdtree/examples/Makefile | 15 + kdtree/examples/test.c | 58 +++ kdtree/examples/test2.c | 88 +++++ kdtree/kdtree.c | 761 +++++++++++++++++++++++++++++++++++++++ kdtree/kdtree.h | 114 ++++++ 10 files changed, 1330 insertions(+) create mode 100644 kdtree/Makefile.in create mode 100755 kdtree/configure create mode 100644 kdtree/doc/guide.html create mode 100644 kdtree/doc/index.html create mode 100644 kdtree/doc/install.html create mode 100644 kdtree/examples/Makefile create mode 100644 kdtree/examples/test.c create mode 100644 kdtree/examples/test2.c create mode 100644 kdtree/kdtree.c create mode 100644 kdtree/kdtree.h diff --git a/kdtree/Makefile.in b/kdtree/Makefile.in new file mode 100644 index 0000000..c75b8bc --- /dev/null +++ b/kdtree/Makefile.in @@ -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) diff --git a/kdtree/configure b/kdtree/configure new file mode 100755 index 0000000..5481e0a --- /dev/null +++ b/kdtree/configure @@ -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=: 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.' diff --git a/kdtree/doc/guide.html b/kdtree/doc/guide.html new file mode 100644 index 0000000..62bce6d --- /dev/null +++ b/kdtree/doc/guide.html @@ -0,0 +1,98 @@ + + + kdtree library manual - installation + + +

Programming guide

+

Using the kdtree library in your programs is very easy.

+ +

Creating a tree

+

+ Call the kd_create 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. +

+

+ For example you may create a 3-dimensional kd-tree with:
+ void *kd = kd_create(3); +

+ +

Destroying a tree

+

+ Call kd_free with a pointer returned from kd_create in + order to free the memory occupied by the tree. Note that any data pointers passed + by the user with the kd_insert functions will not be freed, unless a + "data destructor" function is provided (see below). +

+ +

Managing user pointers

+

+ 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 + kd_data_destructor. The first argument is again a valid pointer to a + kd-tree, while the second argument is a function pointer with the signature: + void (*)(void*). +

+ +

Populating the tree

+

+ To insert data to the kd-tree you may use one of the kd_insert + functions.
+

+

+ 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). +

+

+ kd_insert, and kd_insertf expect a + pointer to an array of k doubles or floats 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. +

+

+ The convenience kd_insert3, and kd_insert3f are meant + to be called for 3-dimensional kd-trees (which is considered the most common + case), and expect 3 values (doubles or floats) + signifying the position of the 3-dimensional point to be stored. +

+ +

Performing nearest-neighbor queries

+

+ 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 "result set", which can from then on be accessed with the + kd_res_* functions. +

+

+ The nearest-neighbor queries are performed with the kd_nearest_range + functions. Like the kd_insert functions described above, they also + provide generic array argument versions for k-dimensional trees, and + 3-dimensional convenience functions, all in double and + float varieties. +

+

+ 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:
+ +void *result_set; +double pt[] = {10.0, 15.0}; + +result_set = kd_nearest_range(kd, pt, 3.0); + + where "kd" is a pointer to a 2-dimensional kd-tree returned by + kd_create(2). +

+

+ A result set aquired with one of the kd_nearest_range functions, + must be freed by calling kd_res_free and supplying the result set + pointer as its only argument. +

+

+ to be continued... +

+ + diff --git a/kdtree/doc/index.html b/kdtree/doc/index.html new file mode 100644 index 0000000..d783443 --- /dev/null +++ b/kdtree/doc/index.html @@ -0,0 +1,23 @@ + + + kdtree library manual + + +

kdtree library manual

+

This document describes the use of the kdtree library.

+ +

index

+ + +
+

+ Copyright (c) 2007 John Tsiombikas + Redistribution of this document, with or without modification, is permitted + provided that this copyright notice is retained. +

+ + diff --git a/kdtree/doc/install.html b/kdtree/doc/install.html new file mode 100644 index 0000000..2ffe3cb --- /dev/null +++ b/kdtree/doc/install.html @@ -0,0 +1,41 @@ + + + kdtree library manual - installation + + +

kdtree library manual

+

installation guide

+ +

+ 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. +

+ +

proper installation

+

+ Just run: ./configure, make, and make + install (the last one should be executed as root if you are installing the + library system-wide). +

+

+ Run ./configure --help for configuration options. The most important + options are --prefix=<installation path prefix>, and + --disable-fastalloc to disable the fast result node allocator, which + depends on pthreads for locking. +

+ +

kdtree source files in your program

+

+ Just copy kdtree.c and kdtree.h files and build them along + with your program. Make sure you have USE_LIST_NODE_ALLOCATOR + defined when compiling kdtree.c if you want the fast result node + allocator. If you do so, remember to link with libpthread too. +

+

+ Of course you need to keep the copyright notices intact if you merge the kdtree + source files with your program in any way. +

+ + diff --git a/kdtree/examples/Makefile b/kdtree/examples/Makefile new file mode 100644 index 0000000..dab0f7e --- /dev/null +++ b/kdtree/examples/Makefile @@ -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 diff --git a/kdtree/examples/test.c b/kdtree/examples/test.c new file mode 100644 index 0000000..966ca7b --- /dev/null +++ b/kdtree/examples/test.c @@ -0,0 +1,58 @@ +/*! gcc -Wall -g -o test test.c libkdtree.a */ +#include +#include +#include +#include +#include +#include +#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 +#include +#include +#include +#include +#include +#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= 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; +} diff --git a/kdtree/kdtree.c b/kdtree/kdtree.c new file mode 100644 index 0000000..4322ff0 --- /dev/null +++ b/kdtree/kdtree.c @@ -0,0 +1,761 @@ +/* +This file is part of ``kdtree'', a library for working with kd-trees. +Copyright (C) 2007-2009 John Tsiombikas + +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 */ +#include +#include +#include +#include +#include "kdtree.h" + +#if defined(WIN32) || defined(__WIN32__) +#include +#endif + +#ifdef USE_LIST_NODE_ALLOCATOR + +#ifndef NO_PTHREADS +#include +#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; ipos[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; itree->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; +} diff --git a/kdtree/kdtree.h b/kdtree/kdtree.h new file mode 100644 index 0000000..97ec4cf --- /dev/null +++ b/kdtree/kdtree.h @@ -0,0 +1,114 @@ +/* +This file is part of ``kdtree'', a library for working with kd-trees. +Copyright (C) 2007-2009 John Tsiombikas + +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_ */