From 3b63e131791315d89c35a96ebb8b8eca9b193614 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Briol=20Fr=C3=A9d=C3=A9ric?= Date: Mon, 19 Dec 2016 12:04:21 +0100 Subject: [PATCH 1/5] Implementing the kd_nearest_n function --- examples/Makefile | 5 +- examples/test3.c | 33 ++ kdtree.c | 1335 ++++++++++++++++++++++++--------------------- kdtree.h | 9 +- 4 files changed, 763 insertions(+), 619 deletions(-) create mode 100644 examples/test3.c diff --git a/examples/Makefile b/examples/Makefile index d2462f5..66c0e0d 100644 --- a/examples/Makefile +++ b/examples/Makefile @@ -5,11 +5,12 @@ CFLAGS = -std=c89 -pedantic -Wall -g -I.. LDFLAGS = $(kdlib) -lm .PHONY: all -all: test test2 +all: test test2 test3 test: test.c $(LDFLAGS) test2: test2.c $(LDFLAGS) +test3: test3.c $(LDFLAGS) .PHONY: clean clean: - rm -f test test2 + rm -f test test2 test3 diff --git a/examples/test3.c b/examples/test3.c new file mode 100644 index 0000000..c79d996 --- /dev/null +++ b/examples/test3.c @@ -0,0 +1,33 @@ +#include +#include "kdtree.h" + + +int main(void) { + void* tree = kd_create(2); + void* res; + double data[] = { + 2, 3, + 5, 4, + 9, 6, + 4, 7, + 8, 1, + 7, 2 + }; + double pos[2] = {9, 2}; + kd_insert(tree, &data[0], NULL); + kd_insert(tree, &data[2], NULL); + kd_insert(tree, &data[4], NULL); + kd_insert(tree, &data[6], NULL); + kd_insert(tree, &data[8], NULL); + kd_insert(tree, &data[10], NULL); + + res = kd_nearest_n(tree, pos, 3); + while(!kd_res_end(res)) { + kd_res_item(res, &data[0]); + printf("%f %f %f\n", data[0], data[1], kd_res_dist(res)); + kd_res_next(res); + } + kd_res_free(res); + kd_free(tree); + return 0; +} diff --git a/kdtree.c b/kdtree.c index 41cd9c0..9925079 100644 --- a/kdtree.c +++ b/kdtree.c @@ -24,17 +24,19 @@ 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 */ +/* single nearest neighbor search written by Tamas Nepusz + */ #ifdef HAVE_CONFIG_H #include #endif +#include "kdtree.h" +#include +#include #include #include #include -#include -#include "kdtree.h" #if defined(WIN32) || defined(__WIN32__) #include @@ -47,795 +49,902 @@ OF SUCH DAMAGE. #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 */ +#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 */ +#endif /* pthread support */ +#endif /* use list node allocator */ struct kdhyperrect { - int dim; - double *min, *max; /* minimum/maximum coords */ + int dim; + double *min, *max; /* minimum/maximum coords */ }; struct kdnode { - double *pos; - int dir; - void *data; + double* pos; + int dir; + void* data; - struct kdnode *left, *right; /* negative/positive side */ + struct kdnode *left, *right; /* negative/positive side */ }; struct res_node { - struct kdnode *item; - double dist_sq; - struct res_node *next; + struct kdnode* item; + double dist_sq; + struct res_node* next; }; struct kdtree { - int dim; - struct kdnode *root; - struct kdhyperrect *rect; - void (*destr)(void*); + int dim; + struct kdnode* root; + struct kdhyperrect* rect; + void (*destr)(void*); }; struct kdres { - struct kdtree *tree; - struct res_node *rlist, *riter; - int size; + struct kdtree* tree; + struct res_node *rlist, *riter; + int size; }; -#define SQ(x) ((x) * (x)) +#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 struct res_node* rlist_pop_back(struct res_node* list); +static void clear_results(struct kdres* set); -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); +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 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) +#define alloc_resnode() malloc(sizeof(struct res_node)) +#define free_resnode(n) free(n) #endif - - -struct kdtree *kd_create(int k) +struct kdtree* +kd_create(int k) { - struct kdtree *tree; + struct kdtree* tree; - if(!(tree = malloc(sizeof *tree))) { - return 0; - } + if (!(tree = malloc(sizeof *tree))) { + return 0; + } - tree->dim = k; - tree->root = 0; - tree->destr = 0; - tree->rect = 0; + tree->dim = k; + tree->root = 0; + tree->destr = 0; + tree->rect = 0; - return tree; + return tree; } -void kd_free(struct kdtree *tree) +void kd_free(struct kdtree* tree) { - if(tree) { - kd_clear(tree); - free(tree); - } + if (tree) { + kd_clear(tree); + free(tree); + } } -static void clear_rec(struct kdnode *node, void (*destr)(void*)) +static void +clear_rec(struct kdnode* node, void (*destr)(void*)) { - if(!node) return; + if (!node) + return; - clear_rec(node->left, destr); - clear_rec(node->right, destr); - - if(destr) { - destr(node->data); - } - free(node->pos); - free(node); + 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) +void kd_clear(struct kdtree* tree) { - clear_rec(tree->root, tree->destr); - tree->root = 0; + clear_rec(tree->root, tree->destr); + tree->root = 0; - if (tree->rect) { - hyperrect_free(tree->rect); - tree->rect = 0; - } + if (tree->rect) { + hyperrect_free(tree->rect); + tree->rect = 0; + } } -void kd_data_destructor(struct kdtree *tree, void (*destr)(void*)) +void kd_data_destructor(struct kdtree* tree, void (*destr)(void*)) { - tree->destr = destr; + tree->destr = destr; } - -static int insert_rec(struct kdnode **nptr, const double *pos, void *data, int dir, int dim) +static int +insert_rec(struct kdnode** nptr, const double* pos, void* data, int dir, + int dim) { - int new_dir; - struct kdnode *node; + 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; - } + 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); + 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) +int kd_insert(struct kdtree* tree, const double* pos, void* data) { - if (insert_rec(&tree->root, pos, data, 0, tree->dim)) { - return -1; - } + 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); - } + if (tree->rect == 0) { + tree->rect = hyperrect_create(tree->dim, pos, pos); + } else { + hyperrect_extend(tree->rect, pos); + } - return 0; + return 0; } -int kd_insertf(struct kdtree *tree, const float *pos, void *data) +int kd_insertf(struct kdtree* tree, const float* pos, void* data) { - static double sbuf[16]; - double *bptr, *buf = 0; - int res, dim = tree->dim; + static double sbuf[16]; + double *bptr, *buf = 0; + int res, dim = tree->dim; - if(dim > 16) { + if (dim > 16) { #ifndef NO_ALLOCA - if(dim <= 256) - bptr = buf = alloca(dim * sizeof *bptr); - else + if (dim <= 256) + bptr = buf = alloca(dim * sizeof *bptr); + else #endif - if(!(bptr = buf = malloc(dim * sizeof *bptr))) { - return -1; - } - } else { - bptr = buf = sbuf; - } - - while(dim-- > 0) { - *bptr++ = *pos++; - } - - res = kd_insert(tree, buf, data); + if (!(bptr = buf = malloc(dim * sizeof *bptr))) { + return -1; + } + } else { + bptr = buf = sbuf; + } + + while (dim-- > 0) { + *bptr++ = *pos++; + } + + res = kd_insert(tree, buf, data); #ifndef NO_ALLOCA - if(tree->dim > 256) + if (tree->dim > 256) #else - if(tree->dim > 16) + 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); + 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 int +find_nearest_n(struct kdnode* node, const double* pos, int num, int* size, + double* dist_max, struct res_node* list, int dim) +{ + double dist_sq, dx; + int i, ret; + + if (!node) + return 0; + + dist_sq = 0; + for (i = 0; i < dim; i++) { + dist_sq += SQ(node->pos[i] - pos[i]); + } + + if (dist_sq < *dist_max) { + if (*size < num) { + ++(*size); + } else { + struct res_node* back = rlist_pop_back(list); + if (back == 0) + return -1; + *dist_max = back->dist_sq > dist_sq ? back->dist_sq : dist_sq; + } + if (rlist_insert(list, node, dist_sq) == -1) { + return -1; + } + } + + /* find signed distance from the splitting plane */ + dx = pos[node->dir] - node->pos[node->dir]; + + ret = find_nearest_n(dx <= 0.0 ? node->left : node->right, pos, num, size, + dist_max, list, dim); + if (ret >= 0 && SQ(dx) < *dist_max) { + ret = find_nearest_n(dx <= 0.0 ? node->right : node->left, pos, num, size, + dist_max, list, dim); + } + return ret; +} + +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; + 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; + } else { + nearer_subtree = node->right; + farther_subtree = node->left; + nearer_hyperrect_coord = rect->min + dir; + farther_hyperrect_coord = rect->max + dir; + } + + 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 = buf = 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; } -int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data) +struct kdres* +kd_nearest3(struct kdtree* tree, double x, double y, double z) { - double buf[3]; - buf[0] = x; - buf[1] = y; - buf[2] = z; - return kd_insert(tree, buf, data); + double pos[3]; + pos[0] = x; + pos[1] = y; + pos[2] = z; + return kd_nearest(tree, pos); } -static int find_nearest(struct kdnode *node, const double *pos, double range, struct res_node *list, int ordered, int dim) +struct kdres* +kd_nearest3f(struct kdtree* tree, float x, float y, float z) { - 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; + double pos[3]; + pos[0] = x; + pos[1] = y; + pos[2] = z; + return kd_nearest(tree, pos); } -#if 0 -static int find_nearest_n(struct kdnode *node, const double *pos, double range, int num, struct rheap *heap, int dim) -{ - double dist_sq, dx; - int i, ret, added_res = 0; - - if(!node) return 0; - - /* if the photon is close enough, add it to the result heap */ - dist_sq = 0; - for(i=0; ipos[i] - pos[i]); - } - if(dist_sq <= range_sq) { - if(heap->size >= num) { - /* get furthest element */ - struct res_node *maxelem = rheap_get_max(heap); - - /* and check if the new one is closer than that */ - if(maxelem->dist_sq > dist_sq) { - rheap_remove_max(heap); - - if(rheap_insert(heap, node, dist_sq) == -1) { - return -1; - } - added_res = 1; - - range_sq = dist_sq; - } - } else { - if(rheap_insert(heap, node, dist_sq) == -1) { - return =1; - } - added_res = 1; - } - } - - - /* find signed distance from the splitting plane */ - dx = pos[node->dir] - node->pos[node->dir]; - - ret = find_nearest_n(dx <= 0.0 ? node->left : node->right, pos, range, num, heap, dim); - if(ret >= 0 && fabs(dx) < range) { - added_res += ret; - ret = find_nearest_n(dx <= 0.0 ? node->right : node->left, pos, range, num, heap, dim); - } - -} -#endif +/* ---- nearest N search ---- */ -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; - 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; - } else { - nearer_subtree = node->right; - farther_subtree = node->left; - nearer_hyperrect_coord = rect->min + dir; - farther_hyperrect_coord = rect->max + dir; - } - - 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) { +struct kdres* kd_nearest_n(struct kdtree* kd, const double* pos, int num) +{ + int ret, size = 0; + struct kdres* rset; + double dist_max = DBL_MAX; + + if (!(rset = malloc(sizeof *rset))) { + return 0; + } + if (!(rset->rlist = alloc_resnode())) { + free(rset); + return 0; + } + rset->rlist->next = 0; + rset->tree = kd; + + ret = find_nearest_n(kd->root, pos, num, &size, &dist_max, rset->rlist, kd->dim); + if (ret == -1) { + kd_res_free(rset); + return 0; + } + rset->size = size; + kd_res_rewind(rset); + return rset; +} + +struct kdres* +kd_nearestnf(struct kdtree* tree, const float* pos, int num) +{ + 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 + if (dim <= 256) + bptr = buf = alloca(dim * sizeof *bptr); + else #endif - if(!(bptr = buf = malloc(dim * sizeof *bptr))) { - return 0; - } - } else { - bptr = buf = sbuf; - } - - while(dim-- > 0) { - *bptr++ = *pos++; - } - - res = kd_nearest(tree, buf); + if (!(bptr = buf = malloc(dim * sizeof *bptr))) { + return 0; + } + } else { + bptr = buf = sbuf; + } + + while (dim-- > 0) { + *bptr++ = *pos++; + } + + res = kd_nearest_n(tree, buf, num); #ifndef NO_ALLOCA - if(tree->dim > 256) + if (tree->dim > 256) #else - if(tree->dim > 16) + if (tree->dim > 16) #endif - free(buf); - return res; + free(buf); + return res; } -struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z) +struct kdres* +kd_nearestn3(struct kdtree* tree, double x, double y, double z, int num) { - double pos[3]; - pos[0] = x; - pos[1] = y; - pos[2] = z; - return kd_nearest(tree, pos); + double pos[3]; + pos[0] = x; + pos[1] = y; + pos[2] = z; + return kd_nearest_n(tree, pos, num); } -struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z) +struct kdres* +kd_nearestn3f(struct kdtree* tree, float x, float y, float z, int num) { - double pos[3]; - pos[0] = x; - pos[1] = y; - pos[2] = z; - return kd_nearest(tree, pos); + double pos[3]; + pos[0] = x; + pos[1] = y; + pos[2] = z; + return kd_nearest_n(tree, pos, num); } -/* ---- nearest N search ---- */ -/* -static kdres *kd_nearest_n(struct kdtree *kd, const double *pos, int num) -{ - 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_n(kd->root, pos, range, num, rset->rlist, kd->dim)) == -1) { - kd_res_free(rset); - return 0; - } - rset->size = ret; - kd_res_rewind(rset); - return rset; -}*/ - -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) { + +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 + if (dim <= 256) + bptr = buf = alloca(dim * sizeof *bptr); + else #endif - if(!(bptr = buf = malloc(dim * sizeof *bptr))) { - return 0; - } - } else { - bptr = buf = sbuf; - } - - while(dim-- > 0) { - *bptr++ = *pos++; - } - - res = kd_nearest_range(kd, buf, range); + if (!(bptr = buf = malloc(dim * sizeof *bptr))) { + return 0; + } + } else { + bptr = buf = sbuf; + } + + while (dim-- > 0) { + *bptr++ = *pos++; + } + + res = kd_nearest_range(kd, buf, range); #ifndef NO_ALLOCA - if(kd->dim > 256) + if (kd->dim > 256) #else - if(kd->dim > 16) + if (kd->dim > 16) #endif - free(buf); - return res; + free(buf); + return res; } -struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range) +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); + 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) +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); + 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) +void kd_res_free(struct kdres* rset) { - clear_results(rset); - free_resnode(rset->rlist); - free(rset); + clear_results(rset); + free_resnode(rset->rlist); + free(rset); } -int kd_res_size(struct kdres *set) +int kd_res_size(struct kdres* set) { - return (set->size); + return (set->size); } -void kd_res_rewind(struct kdres *rset) +void kd_res_rewind(struct kdres* rset) { - rset->riter = rset->rlist->next; + rset->riter = rset->rlist->next; } -int kd_res_end(struct kdres *rset) +int kd_res_end(struct kdres* rset) { - return rset->riter == 0; + return rset->riter == 0; } -int kd_res_next(struct kdres *rset) +int kd_res_next(struct kdres* rset) { - rset->riter = rset->riter->next; - return rset->riter != 0; + rset->riter = rset->riter->next; + return rset->riter != 0; } -void *kd_res_item(struct kdres *rset, double *pos) +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; + 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) +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; + 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) +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; + 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) +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; + 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) +void* kd_res_item_data(struct kdres* set) { - return kd_res_item(set, 0); + return kd_res_item(set, 0); +} + +double kd_res_dist(struct kdres *set) +{ + return set->riter->dist_sq; } /* ---- hyperrectangle helpers ---- */ -static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max) +static struct kdhyperrect* +hyperrect_create(int dim, const double* min, const double* max) { - size_t size = dim * sizeof(double); - struct kdhyperrect* rect = 0; + size_t size = dim * sizeof(double); + struct kdhyperrect* rect = 0; - if (!(rect = malloc(sizeof(struct kdhyperrect)))) { - return 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); + 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; + return rect; } -static void hyperrect_free(struct kdhyperrect *rect) +static void +hyperrect_free(struct kdhyperrect* rect) { - free(rect->min); - free(rect->max); - free(rect); + free(rect->min); + free(rect->max); + free(rect); } -static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect) +static struct kdhyperrect* +hyperrect_duplicate(const struct kdhyperrect* rect) { - return hyperrect_create(rect->dim, rect->min, rect->max); + return hyperrect_create(rect->dim, rect->min, rect->max); } -static void hyperrect_extend(struct kdhyperrect *rect, const double *pos) +static void +hyperrect_extend(struct kdhyperrect* rect, const double* pos) { - int i; + 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]; - } - } + 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) +static double +hyperrect_dist_sq(struct kdhyperrect* rect, const double* pos) { - int i; - double result = 0; + 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]); - } - } + 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; + return result; } /* ---- static helpers ---- */ #ifdef USE_LIST_NODE_ALLOCATOR /* special list node allocators. */ -static struct res_node *free_nodes; +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) +static struct res_node* +alloc_resnode(void) { - struct res_node *node; + struct res_node* node; #ifndef NO_PTHREADS - pthread_mutex_lock(&alloc_mutex); + 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; - } + 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); + pthread_mutex_unlock(&alloc_mutex); #endif - return node; + return node; } -static void free_resnode(struct res_node *node) +static void +free_resnode(struct res_node* node) { #ifndef NO_PTHREADS - pthread_mutex_lock(&alloc_mutex); + pthread_mutex_lock(&alloc_mutex); #endif - node->next = free_nodes; - free_nodes = node; + node->next = free_nodes; + free_nodes = node; #ifndef NO_PTHREADS - pthread_mutex_unlock(&alloc_mutex); + pthread_mutex_unlock(&alloc_mutex); #endif } -#endif /* list node allocator or not */ - +#endif /* list node allocator or not */ /* inserts the item. if dist_sq is >= 0, then do an ordered insert */ /* TODO make the ordering code use heapsort */ -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; +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 struct res_node* +rlist_pop_back(struct res_node* list) +{ + struct res_node* previous = 0; + while (list->next) { + previous = list; + list = list->next; + } + if (previous) { + previous->next = 0; + } + free_resnode(list); + return previous; +} + +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.h b/kdtree.h index 92d43e4..8eca13c 100644 --- a/kdtree.h +++ b/kdtree.h @@ -73,12 +73,10 @@ struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z); * 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_n(struct kdtree *tree, const double *pos, int num); struct kdres *kd_nearest_nf(struct kdtree *tree, const float *pos, int num); -struct kdres *kd_nearest_n3(struct kdtree *tree, double x, double y, double z); -struct kdres *kd_nearest_n3f(struct kdtree *tree, float x, float y, float z); -*/ +struct kdres *kd_nearest_n3(struct kdtree *tree, double x, double y, double z, int num); +struct kdres *kd_nearest_n3f(struct kdtree *tree, float x, float y, float z, int num); /* Find any nearest nodes from a given point within a range. * @@ -121,6 +119,9 @@ 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); +/* returns the distance between the requested position and the found point. + */ +double kd_res_dist(struct kdres *set); #ifdef __cplusplus } From 336fbea84f6cab8e3fe0f7b2bd269c9f403dd388 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Briol=20Fr=C3=A9d=C3=A9ric?= Date: Mon, 19 Dec 2016 14:19:45 +0100 Subject: [PATCH 2/5] The function names are misspelled. --- kdtree.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/kdtree.c b/kdtree.c index 9925079..2741598 100644 --- a/kdtree.c +++ b/kdtree.c @@ -549,7 +549,7 @@ struct kdres* kd_nearest_n(struct kdtree* kd, const double* pos, int num) } struct kdres* -kd_nearestnf(struct kdtree* tree, const float* pos, int num) +kd_nearest_nf(struct kdtree* tree, const float* pos, int num) { static double sbuf[16]; double *bptr, *buf = 0; @@ -584,7 +584,7 @@ kd_nearestnf(struct kdtree* tree, const float* pos, int num) } struct kdres* -kd_nearestn3(struct kdtree* tree, double x, double y, double z, int num) +kd_nearest_n3(struct kdtree* tree, double x, double y, double z, int num) { double pos[3]; pos[0] = x; @@ -594,7 +594,7 @@ kd_nearestn3(struct kdtree* tree, double x, double y, double z, int num) } struct kdres* -kd_nearestn3f(struct kdtree* tree, float x, float y, float z, int num) +kd_nearest_n3f(struct kdtree* tree, float x, float y, float z, int num) { double pos[3]; pos[0] = x; From 5aa70acfed36aecd0bfab7a9bc49700326143115 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Briol=20Fr=C3=A9d=C3=A9ric?= Date: Tue, 20 Dec 2016 09:26:11 +0100 Subject: [PATCH 3/5] return the distance and not the square of the distance --- kdtree.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/kdtree.c b/kdtree.c index 2741598..6113a8f 100644 --- a/kdtree.c +++ b/kdtree.c @@ -771,7 +771,7 @@ void* kd_res_item_data(struct kdres* set) double kd_res_dist(struct kdres *set) { - return set->riter->dist_sq; + return sqrt(set->riter->dist_sq); } /* ---- hyperrectangle helpers ---- */ From 102f76ae3f6ab26dd4c9420735e5bcf762850f8e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Briol=20Fr=C3=A9d=C3=A9ric?= Date: Tue, 20 Dec 2016 11:32:26 +0100 Subject: [PATCH 4/5] free list node buffer --- kdtree.c | 33 ++++++++++++++++++++++++++++++--- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/kdtree.c b/kdtree.c index 6113a8f..6d5ebce 100644 --- a/kdtree.c +++ b/kdtree.c @@ -108,6 +108,7 @@ 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*); +static void free_resnode_buffer(); #else #define alloc_resnode() malloc(sizeof(struct res_node)) #define free_resnode(n) free(n) @@ -136,6 +137,10 @@ void kd_free(struct kdtree* tree) kd_clear(tree); free(tree); } + +#ifdef USE_LIST_NODE_ALLOCATOR + free_resnode_buffer(); +#endif } static void @@ -603,7 +608,6 @@ kd_nearest_n3f(struct kdtree* tree, float x, float y, float z, int num) return kd_nearest_n(tree, pos, num); } - struct kdres* kd_nearest_range(struct kdtree* kd, const double* pos, double range) { @@ -769,9 +773,9 @@ void* kd_res_item_data(struct kdres* set) return kd_res_item(set, 0); } -double kd_res_dist(struct kdres *set) +double kd_res_dist(struct kdres* set) { - return sqrt(set->riter->dist_sq); + return sqrt(set->riter->dist_sq); } /* ---- hyperrectangle helpers ---- */ @@ -895,6 +899,29 @@ free_resnode(struct res_node* node) pthread_mutex_unlock(&alloc_mutex); #endif } + +static void +free_resnode_buffer() +{ +#ifndef NO_PTHREADS + pthread_mutex_lock(&alloc_mutex); +#endif + + if (free_nodes) { + struct res_node* ptr = free_nodes; + while (ptr) { + ptr = ptr->next; + free(free_nodes); + free_nodes = ptr; + } + free_nodes = 0; + } + +#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 */ From fb01df53e33e0e54a3bcf8703d25aac16a805b6d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Fr=C3=A9d=C3=A9ric=20BRIOL?= Date: Sat, 7 Aug 2021 17:56:25 +0200 Subject: [PATCH 5/5] revert to LLVM formatting --- kdtree.c | 1365 ++++++++++++++++++++++++++---------------------------- 1 file changed, 657 insertions(+), 708 deletions(-) diff --git a/kdtree.c b/kdtree.c index 6d5ebce..c1d1605 100644 --- a/kdtree.c +++ b/kdtree.c @@ -24,19 +24,17 @@ 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 - */ +/* single nearest neighbor search written by Tamas Nepusz */ #ifdef HAVE_CONFIG_H #include #endif -#include "kdtree.h" -#include -#include #include #include #include +#include +#include "kdtree.h" #if defined(WIN32) || defined(__WIN32__) #include @@ -49,876 +47,830 @@ OF SUCH DAMAGE. #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 */ +#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 */ +#endif /* pthread support */ +#endif /* use list node allocator */ struct kdhyperrect { - int dim; - double *min, *max; /* minimum/maximum coords */ + int dim; + double *min, *max; /* minimum/maximum coords */ }; struct kdnode { - double* pos; - int dir; - void* data; + double *pos; + int dir; + void *data; - struct kdnode *left, *right; /* negative/positive side */ + struct kdnode *left, *right; /* negative/positive side */ }; struct res_node { - struct kdnode* item; - double dist_sq; - struct res_node* next; + struct kdnode *item; + double dist_sq; + struct res_node *next; }; struct kdtree { - int dim; - struct kdnode* root; - struct kdhyperrect* rect; - void (*destr)(void*); + int dim; + struct kdnode *root; + struct kdhyperrect *rect; + void (*destr)(void*); }; struct kdres { - struct kdtree* tree; - struct res_node *rlist, *riter; - int size; + struct kdtree *tree; + struct res_node *rlist, *riter; + int size; }; -#define SQ(x) ((x) * (x)) +#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 struct res_node* rlist_pop_back(struct res_node* list); -static void clear_results(struct kdres* set); +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 struct res_node *rlist_pop_back(struct res_node *list); +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); +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 struct res_node *alloc_resnode(void); static void free_resnode(struct res_node*); static void free_resnode_buffer(); #else -#define alloc_resnode() malloc(sizeof(struct res_node)) -#define free_resnode(n) free(n) +#define alloc_resnode() malloc(sizeof(struct res_node)) +#define free_resnode(n) free(n) #endif -struct kdtree* -kd_create(int k) + + +struct kdtree *kd_create(int k) { - struct kdtree* tree; + struct kdtree *tree; - if (!(tree = malloc(sizeof *tree))) { - return 0; - } + if(!(tree = malloc(sizeof *tree))) { + return 0; + } - tree->dim = k; - tree->root = 0; - tree->destr = 0; - tree->rect = 0; + tree->dim = k; + tree->root = 0; + tree->destr = 0; + tree->rect = 0; - return tree; + return tree; } -void kd_free(struct kdtree* tree) +void kd_free(struct kdtree *tree) { - if (tree) { - kd_clear(tree); - free(tree); - } + if(tree) { + kd_clear(tree); + free(tree); + } #ifdef USE_LIST_NODE_ALLOCATOR - free_resnode_buffer(); + free_resnode_buffer(); #endif } -static void -clear_rec(struct kdnode* node, void (*destr)(void*)) +static void clear_rec(struct kdnode *node, void (*destr)(void*)) { - if (!node) - return; - - clear_rec(node->left, destr); - clear_rec(node->right, destr); + if(!node) return; - if (destr) { - destr(node->data); - } - free(node->pos); - free(node); + 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) +void kd_clear(struct kdtree *tree) { - clear_rec(tree->root, tree->destr); - tree->root = 0; + clear_rec(tree->root, tree->destr); + tree->root = 0; - if (tree->rect) { - hyperrect_free(tree->rect); - tree->rect = 0; - } + if (tree->rect) { + hyperrect_free(tree->rect); + tree->rect = 0; + } } -void kd_data_destructor(struct kdtree* tree, void (*destr)(void*)) +void kd_data_destructor(struct kdtree *tree, void (*destr)(void *)) { - tree->destr = destr; + tree->destr = destr; } -static int -insert_rec(struct kdnode** nptr, const double* pos, void* data, int dir, - int dim) + +static int insert_rec(struct kdnode **nptr, const double *pos, void *data, int dir, int dim) { - int new_dir; - struct kdnode* node; + 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; - } + 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); + 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) +int kd_insert(struct kdtree *tree, const double *pos, void *data) { - if (insert_rec(&tree->root, pos, data, 0, tree->dim)) { - return -1; - } + 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); - } + if (tree->rect == 0) { + tree->rect = hyperrect_create(tree->dim, pos, pos); + } else { + hyperrect_extend(tree->rect, pos); + } - return 0; + return 0; } -int kd_insertf(struct kdtree* tree, const float* pos, void* data) +int kd_insertf(struct kdtree *tree, const float *pos, void *data) { - static double sbuf[16]; - double *bptr, *buf = 0; - int res, dim = tree->dim; + static double sbuf[16]; + double *bptr, *buf = 0; + int res, dim = tree->dim; - if (dim > 16) { + if(dim > 16) { #ifndef NO_ALLOCA - if (dim <= 256) - bptr = buf = alloca(dim * sizeof *bptr); - else + if(dim <= 256) + bptr = buf = alloca(dim * sizeof *bptr); + else #endif - if (!(bptr = buf = malloc(dim * sizeof *bptr))) { - return -1; - } - } else { - bptr = buf = sbuf; - } - - while (dim-- > 0) { - *bptr++ = *pos++; - } - - res = kd_insert(tree, buf, data); + if(!(bptr = buf = malloc(dim * sizeof *bptr))) { + return -1; + } + } else { + bptr = buf = sbuf; + } + + while(dim-- > 0) { + *bptr++ = *pos++; + } + + res = kd_insert(tree, buf, data); #ifndef NO_ALLOCA - if (tree->dim > 256) + if(tree->dim > 256) #else - if (tree->dim > 16) + 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 int -find_nearest_n(struct kdnode* node, const double* pos, int num, int* size, - double* dist_max, struct res_node* list, int dim) -{ - double dist_sq, dx; - int i, ret; - - if (!node) - return 0; - - dist_sq = 0; - for (i = 0; i < dim; i++) { - dist_sq += SQ(node->pos[i] - pos[i]); - } - - if (dist_sq < *dist_max) { - if (*size < num) { - ++(*size); - } else { - struct res_node* back = rlist_pop_back(list); - if (back == 0) - return -1; - *dist_max = back->dist_sq > dist_sq ? back->dist_sq : dist_sq; - } - if (rlist_insert(list, node, dist_sq) == -1) { - return -1; - } - } - - /* find signed distance from the splitting plane */ - dx = pos[node->dir] - node->pos[node->dir]; - - ret = find_nearest_n(dx <= 0.0 ? node->left : node->right, pos, num, size, - dist_max, list, dim); - if (ret >= 0 && SQ(dx) < *dist_max) { - ret = find_nearest_n(dx <= 0.0 ? node->right : node->left, pos, num, size, - dist_max, list, dim); - } - return ret; -} - -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; - 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; - } else { - nearer_subtree = node->right; - farther_subtree = node->left; - nearer_hyperrect_coord = rect->min + dir; - farther_hyperrect_coord = rect->max + dir; - } - - 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) { + 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 int find_nearest_n(struct kdnode *node, const double *pos, int num, int *size, double *dist_max, struct res_node *list, int dim) +{ + double dist_sq, dx; + int i, ret; + + if(!node) return 0; + + dist_sq = 0; + for(i=0; ipos[i] - pos[i]); + } + + if(dist_sq < *dist_max) { + if(*size < num) { + ++(*size); + } else { + struct res_node *back = rlist_pop_back(list); + if (back == 0) + return -1; + *dist_max = back->dist_sq > dist_sq ? back->dist_sq : dist_sq; + } + if (rlist_insert(list, node, dist_sq) == -1) { + return -1; + } + } + + /* find signed distance from the splitting plane */ + dx = pos[node->dir] - node->pos[node->dir]; + + ret = find_nearest_n(dx <= 0.0 ? node->left : node->right, pos, num, size, dist_max, list, dim); + if(ret >= 0 && SQ(dx) < *dist_max) { + ret = find_nearest_n(dx <= 0.0 ? node->right : node->left, pos, num, size, dist_max, list, dim); + } + return ret; +} + +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; + 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; + } else { + nearer_subtree = node->right; + farther_subtree = node->left; + nearer_hyperrect_coord = rect->min + dir; + farther_hyperrect_coord = rect->max + dir; + } + + 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 + if(dim <= 256) + bptr = buf = alloca(dim * sizeof *bptr); + else #endif - if (!(bptr = buf = malloc(dim * sizeof *bptr))) { - return 0; - } - } else { - bptr = buf = sbuf; - } - - while (dim-- > 0) { - *bptr++ = *pos++; - } - - res = kd_nearest(tree, buf); + if(!(bptr = buf = malloc(dim * sizeof *bptr))) { + return 0; + } + } else { + bptr = buf = sbuf; + } + + while(dim-- > 0) { + *bptr++ = *pos++; + } + + res = kd_nearest(tree, buf); #ifndef NO_ALLOCA - if (tree->dim > 256) + if(tree->dim > 256) #else - if (tree->dim > 16) + if(tree->dim > 16) #endif - free(buf); - return res; + free(buf); + return res; } -struct kdres* -kd_nearest3(struct kdtree* tree, double x, double y, double z) +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); + 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) +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); + double pos[3]; + pos[0] = x; + pos[1] = y; + pos[2] = z; + return kd_nearest(tree, pos); } /* ---- nearest N search ---- */ -struct kdres* kd_nearest_n(struct kdtree* kd, const double* pos, int num) -{ - int ret, size = 0; - struct kdres* rset; - double dist_max = DBL_MAX; - - if (!(rset = malloc(sizeof *rset))) { - return 0; - } - if (!(rset->rlist = alloc_resnode())) { - free(rset); - return 0; - } - rset->rlist->next = 0; - rset->tree = kd; - - ret = find_nearest_n(kd->root, pos, num, &size, &dist_max, rset->rlist, kd->dim); - if (ret == -1) { - kd_res_free(rset); - return 0; - } - rset->size = size; - kd_res_rewind(rset); - return rset; -} - -struct kdres* -kd_nearest_nf(struct kdtree* tree, const float* pos, int num) -{ - static double sbuf[16]; - double *bptr, *buf = 0; - int dim = tree->dim; - struct kdres* res; - - if (dim > 16) { +struct kdres *kd_nearest_n(struct kdtree *kd, const double *pos, int num) +{ + int ret, size = 0; + struct kdres *rset; + double dist_max = DBL_MAX; + + if(!(rset = malloc(sizeof *rset))) { + return 0; + } + if(!(rset->rlist = alloc_resnode())) { + free(rset); + return 0; + } + rset->rlist->next = 0; + rset->tree = kd; + + ret = find_nearest_n(kd->root, pos, num, &size, &dist_max, rset->rlist, kd->dim); + if(ret == -1) { + kd_res_free(rset); + return 0; + } + rset->size = size; + kd_res_rewind(rset); + return rset; +} + +struct kdres *kd_nearest_nf(struct kdtree *tree, const float *pos, int num) +{ + 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 + if (dim <= 256) + bptr = buf = alloca(dim * sizeof *bptr); + else #endif - if (!(bptr = buf = malloc(dim * sizeof *bptr))) { - return 0; - } - } else { - bptr = buf = sbuf; - } - - while (dim-- > 0) { - *bptr++ = *pos++; - } - - res = kd_nearest_n(tree, buf, num); + if (!(bptr = buf = malloc(dim * sizeof *bptr))) { + return 0; + } + } else { + bptr = buf = sbuf; + } + + while (dim-- > 0) { + *bptr++ = *pos++; + } + + res = kd_nearest_n(tree, buf, num); #ifndef NO_ALLOCA - if (tree->dim > 256) + if (tree->dim > 256) #else - if (tree->dim > 16) + if (tree->dim > 16) #endif - free(buf); - return res; + free(buf); + return res; } -struct kdres* -kd_nearest_n3(struct kdtree* tree, double x, double y, double z, int num) +struct kdres *kd_nearest_n3(struct kdtree *tree, double x, double y, double z, int num) { - double pos[3]; - pos[0] = x; - pos[1] = y; - pos[2] = z; - return kd_nearest_n(tree, pos, num); + double pos[3]; + pos[0] = x; + pos[1] = y; + pos[2] = z; + return kd_nearest_n(tree, pos, num); } -struct kdres* -kd_nearest_n3f(struct kdtree* tree, float x, float y, float z, int num) +struct kdres *kd_nearest_n3f(struct kdtree *tree, float x, float y, float z, int num) { - double pos[3]; - pos[0] = x; - pos[1] = y; - pos[2] = z; - return kd_nearest_n(tree, pos, num); + double pos[3]; + pos[0] = x; + pos[1] = y; + pos[2] = z; + return kd_nearest_n(tree, pos, num); } -struct kdres* -kd_nearest_range(struct kdtree* kd, const double* pos, double range) +struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range) { - int ret; - struct kdres* rset; + 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(!(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; + 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) +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; + static double sbuf[16]; + double *bptr, *buf = 0; + int dim = kd->dim; + struct kdres *res; - if (dim > 16) { + if(dim > 16) { #ifndef NO_ALLOCA - if (dim <= 256) - bptr = buf = alloca(dim * sizeof *bptr); - else + if(dim <= 256) + bptr = buf = alloca(dim * sizeof *bptr); + else #endif - if (!(bptr = buf = malloc(dim * sizeof *bptr))) { - return 0; - } - } else { - bptr = buf = sbuf; - } - - while (dim-- > 0) { - *bptr++ = *pos++; - } - - res = kd_nearest_range(kd, buf, range); + if(!(bptr = buf = malloc(dim * sizeof *bptr))) { + return 0; + } + } else { + bptr = buf = sbuf; + } + + while(dim-- > 0) { + *bptr++ = *pos++; + } + + res = kd_nearest_range(kd, buf, range); #ifndef NO_ALLOCA - if (kd->dim > 256) + if(kd->dim > 256) #else - if (kd->dim > 16) + if(kd->dim > 16) #endif - free(buf); - return res; + free(buf); + return res; } -struct kdres* -kd_nearest_range3(struct kdtree* tree, double x, double y, double z, - double range) +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); + 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) +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); + 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) +void kd_res_free(struct kdres *rset) { - clear_results(rset); - free_resnode(rset->rlist); - free(rset); + clear_results(rset); + free_resnode(rset->rlist); + free(rset); } -int kd_res_size(struct kdres* set) +int kd_res_size(struct kdres *set) { - return (set->size); + return (set->size); } -void kd_res_rewind(struct kdres* rset) +void kd_res_rewind(struct kdres *rset) { - rset->riter = rset->rlist->next; + rset->riter = rset->rlist->next; } -int kd_res_end(struct kdres* rset) +int kd_res_end(struct kdres *rset) { - return rset->riter == 0; + return rset->riter == 0; } -int kd_res_next(struct kdres* rset) +int kd_res_next(struct kdres *rset) { - rset->riter = rset->riter->next; - return rset->riter != 0; + rset->riter = rset->riter->next; + return rset->riter != 0; } -void* kd_res_item(struct kdres* rset, double* pos) +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; + 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) +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; + 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) +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; + 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) +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; + 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) +void *kd_res_item_data(struct kdres *set) { - return kd_res_item(set, 0); + return kd_res_item(set, 0); } -double kd_res_dist(struct kdres* set) +double kd_res_dist(struct kdres *set) { - return sqrt(set->riter->dist_sq); + return sqrt(set->riter->dist_sq); } /* ---- hyperrectangle helpers ---- */ -static struct kdhyperrect* -hyperrect_create(int dim, const double* min, const double* max) +static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max) { - size_t size = dim * sizeof(double); - struct kdhyperrect* rect = 0; + size_t size = dim * sizeof(double); + struct kdhyperrect* rect = 0; - if (!(rect = malloc(sizeof(struct kdhyperrect)))) { - return 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); + 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; + return rect; } -static void -hyperrect_free(struct kdhyperrect* rect) +static void hyperrect_free(struct kdhyperrect *rect) { - free(rect->min); - free(rect->max); - free(rect); + free(rect->min); + free(rect->max); + free(rect); } -static struct kdhyperrect* -hyperrect_duplicate(const struct kdhyperrect* rect) +static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect) { - return hyperrect_create(rect->dim, rect->min, rect->max); + return hyperrect_create(rect->dim, rect->min, rect->max); } -static void -hyperrect_extend(struct kdhyperrect* rect, const double* pos) +static void hyperrect_extend(struct kdhyperrect *rect, const double *pos) { - int i; + 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]; - } - } + 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) +static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos) { - int i; - double result = 0; + 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]); - } - } + 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; + return result; } /* ---- static helpers ---- */ #ifdef USE_LIST_NODE_ALLOCATOR /* special list node allocators. */ -static struct res_node* free_nodes; +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) +static struct res_node *alloc_resnode(void) { - struct res_node* node; + struct res_node *node; #ifndef NO_PTHREADS - pthread_mutex_lock(&alloc_mutex); + 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; - } + 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); + pthread_mutex_unlock(&alloc_mutex); #endif - return node; + return node; } -static void -free_resnode(struct res_node* node) +static void free_resnode(struct res_node *node) { #ifndef NO_PTHREADS - pthread_mutex_lock(&alloc_mutex); + pthread_mutex_lock(&alloc_mutex); #endif - node->next = free_nodes; - free_nodes = node; + node->next = free_nodes; + free_nodes = node; #ifndef NO_PTHREADS - pthread_mutex_unlock(&alloc_mutex); + pthread_mutex_unlock(&alloc_mutex); #endif } -static void -free_resnode_buffer() +static void free_resnode_buffer() { #ifndef NO_PTHREADS - pthread_mutex_lock(&alloc_mutex); + pthread_mutex_lock(&alloc_mutex); #endif - if (free_nodes) { - struct res_node* ptr = free_nodes; - while (ptr) { - ptr = ptr->next; - free(free_nodes); - free_nodes = ptr; - } - free_nodes = 0; - } + if (free_nodes) { + struct res_node *ptr = free_nodes; + while (ptr) { + ptr = ptr->next; + free(free_nodes); + free_nodes = ptr; + } + free_nodes = 0; + } #ifndef NO_PTHREADS - pthread_mutex_unlock(&alloc_mutex); + pthread_mutex_unlock(&alloc_mutex); #endif } @@ -926,52 +878,49 @@ free_resnode_buffer() /* inserts the item. if dist_sq is >= 0, then do an ordered insert */ /* TODO make the ordering code use heapsort */ -static int -rlist_insert(struct res_node* list, struct kdnode* item, double dist_sq) +static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq) { - struct res_node* rnode; + struct res_node *rnode; - if (!(rnode = alloc_resnode())) { - return -1; - } - rnode->item = item; - rnode->dist_sq = dist_sq; + 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; + 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 struct res_node* -rlist_pop_back(struct res_node* list) +static struct res_node *rlist_pop_back(struct res_node *list) { - struct res_node* previous = 0; - while (list->next) { - previous = list; - list = list->next; - } - if (previous) { - previous->next = 0; - } - free_resnode(list); - return previous; + struct res_node *previous = 0; + while (list->next) { + previous = list; + list = list->next; + } + if (previous) { + previous->next = 0; + } + free_resnode(list); + return previous; } -static void -clear_results(struct kdres* rset) +static void clear_results(struct kdres *rset) { - struct res_node *tmp, *node = rset->rlist->next; + struct res_node *tmp, *node = rset->rlist->next; - while (node) { - tmp = node; - node = node->next; - free_resnode(tmp); - } + while(node) { + tmp = node; + node = node->next; + free_resnode(tmp); + } - rset->rlist->next = 0; -} + rset->rlist->next = 0; +} \ No newline at end of file