1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/prototype/kdtree/kdtree.c	Mon Sep 17 08:40:59 2012 +0300
     1.3 @@ -0,0 +1,840 @@
     1.4 +/*
     1.5 +This file is part of ``kdtree'', a library for working with kd-trees.
     1.6 +Copyright (C) 2007-2011 John Tsiombikas <nuclear@member.fsf.org>
     1.7 +
     1.8 +Redistribution and use in source and binary forms, with or without
     1.9 +modification, are permitted provided that the following conditions are met:
    1.10 +
    1.11 +1. Redistributions of source code must retain the above copyright notice, this
    1.12 +   list of conditions and the following disclaimer.
    1.13 +2. Redistributions in binary form must reproduce the above copyright notice,
    1.14 +   this list of conditions and the following disclaimer in the documentation
    1.15 +   and/or other materials provided with the distribution.
    1.16 +3. The name of the author may not be used to endorse or promote products
    1.17 +   derived from this software without specific prior written permission.
    1.18 +
    1.19 +THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
    1.20 +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
    1.21 +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
    1.22 +EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
    1.23 +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
    1.24 +OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
    1.25 +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
    1.26 +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
    1.27 +IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
    1.28 +OF SUCH DAMAGE.
    1.29 +*/
    1.30 +/* single nearest neighbor search written by Tamas Nepusz <tamas@cs.rhul.ac.uk> */
    1.31 +#include <stdio.h>
    1.32 +#include <stdlib.h>
    1.33 +#include <string.h>
    1.34 +#include <math.h>
    1.35 +#include "kdtree.h"
    1.36 +
    1.37 +#define USE_LIST_NODE_ALLOCATOR
    1.38 +#define NO_PTHREADS
    1.39 +#define I_WANT_THREAD_BUGS
    1.40 +
    1.41 +#if defined(WIN32) || defined(__WIN32__)
    1.42 +#include <malloc.h>
    1.43 +#endif
    1.44 +
    1.45 +#ifdef USE_LIST_NODE_ALLOCATOR
    1.46 +
    1.47 +#ifndef NO_PTHREADS
    1.48 +#include <pthread.h>
    1.49 +#else
    1.50 +
    1.51 +#ifndef I_WANT_THREAD_BUGS
    1.52 +#error "You are compiling with the fast list node allocator, with pthreads disabled! This WILL break if used from multiple threads."
    1.53 +#endif	/* I want thread bugs */
    1.54 +
    1.55 +#endif	/* pthread support */
    1.56 +#endif	/* use list node allocator */
    1.57 +
    1.58 +struct kdhyperrect {
    1.59 +	int dim;
    1.60 +	double *min, *max;              /* minimum/maximum coords */
    1.61 +};
    1.62 +
    1.63 +struct kdnode {
    1.64 +	double *pos;
    1.65 +	int dir;
    1.66 +	void *data;
    1.67 +
    1.68 +	struct kdnode *left, *right;	/* negative/positive side */
    1.69 +};
    1.70 +
    1.71 +struct res_node {
    1.72 +	struct kdnode *item;
    1.73 +	double dist_sq;
    1.74 +	struct res_node *next;
    1.75 +};
    1.76 +
    1.77 +struct kdtree {
    1.78 +	int dim;
    1.79 +	struct kdnode *root;
    1.80 +	struct kdhyperrect *rect;
    1.81 +	void (*destr)(void*);
    1.82 +};
    1.83 +
    1.84 +struct kdres {
    1.85 +	struct kdtree *tree;
    1.86 +	struct res_node *rlist, *riter;
    1.87 +	int size;
    1.88 +};
    1.89 +
    1.90 +#define SQ(x)			((x) * (x))
    1.91 +
    1.92 +
    1.93 +static void clear_rec(struct kdnode *node, void (*destr)(void*));
    1.94 +static int insert_rec(struct kdnode **node, const double *pos, void *data, int dir, int dim);
    1.95 +static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq);
    1.96 +static void clear_results(struct kdres *set);
    1.97 +
    1.98 +static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max);
    1.99 +static void hyperrect_free(struct kdhyperrect *rect);
   1.100 +static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect);
   1.101 +static void hyperrect_extend(struct kdhyperrect *rect, const double *pos);
   1.102 +static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos);
   1.103 +
   1.104 +#ifdef USE_LIST_NODE_ALLOCATOR
   1.105 +static struct res_node *alloc_resnode(void);
   1.106 +static void free_resnode(struct res_node*);
   1.107 +#else
   1.108 +#define alloc_resnode()		malloc(sizeof(struct res_node))
   1.109 +#define free_resnode(n)		free(n)
   1.110 +#endif
   1.111 +
   1.112 +
   1.113 +
   1.114 +struct kdtree *kd_create(int k)
   1.115 +{
   1.116 +	struct kdtree *tree;
   1.117 +
   1.118 +	if(!(tree = malloc(sizeof *tree))) {
   1.119 +		return 0;
   1.120 +	}
   1.121 +
   1.122 +	tree->dim = k;
   1.123 +	tree->root = 0;
   1.124 +	tree->destr = 0;
   1.125 +	tree->rect = 0;
   1.126 +
   1.127 +	return tree;
   1.128 +}
   1.129 +
   1.130 +void kd_free(struct kdtree *tree)
   1.131 +{
   1.132 +	if(tree) {
   1.133 +		kd_clear(tree);
   1.134 +		free(tree);
   1.135 +	}
   1.136 +}
   1.137 +
   1.138 +static void clear_rec(struct kdnode *node, void (*destr)(void*))
   1.139 +{
   1.140 +	if(!node) return;
   1.141 +
   1.142 +	clear_rec(node->left, destr);
   1.143 +	clear_rec(node->right, destr);
   1.144 +	
   1.145 +	if(destr) {
   1.146 +		destr(node->data);
   1.147 +	}
   1.148 +	free(node->pos);
   1.149 +	free(node);
   1.150 +}
   1.151 +
   1.152 +void kd_clear(struct kdtree *tree)
   1.153 +{
   1.154 +	clear_rec(tree->root, tree->destr);
   1.155 +	tree->root = 0;
   1.156 +
   1.157 +	if (tree->rect) {
   1.158 +		hyperrect_free(tree->rect);
   1.159 +		tree->rect = 0;
   1.160 +	}
   1.161 +}
   1.162 +
   1.163 +void kd_data_destructor(struct kdtree *tree, void (*destr)(void*))
   1.164 +{
   1.165 +	tree->destr = destr;
   1.166 +}
   1.167 +
   1.168 +
   1.169 +static int insert_rec(struct kdnode **nptr, const double *pos, void *data, int dir, int dim)
   1.170 +{
   1.171 +	int new_dir;
   1.172 +	struct kdnode *node;
   1.173 +
   1.174 +	if(!*nptr) {
   1.175 +		if(!(node = malloc(sizeof *node))) {
   1.176 +			return -1;
   1.177 +		}
   1.178 +		if(!(node->pos = malloc(dim * sizeof *node->pos))) {
   1.179 +			free(node);
   1.180 +			return -1;
   1.181 +		}
   1.182 +		memcpy(node->pos, pos, dim * sizeof *node->pos);
   1.183 +		node->data = data;
   1.184 +		node->dir = dir;
   1.185 +		node->left = node->right = 0;
   1.186 +		*nptr = node;
   1.187 +		return 0;
   1.188 +	}
   1.189 +
   1.190 +	node = *nptr;
   1.191 +	new_dir = (node->dir + 1) % dim;
   1.192 +	if(pos[node->dir] < node->pos[node->dir]) {
   1.193 +		return insert_rec(&(*nptr)->left, pos, data, new_dir, dim);
   1.194 +	}
   1.195 +	return insert_rec(&(*nptr)->right, pos, data, new_dir, dim);
   1.196 +}
   1.197 +
   1.198 +int kd_insert(struct kdtree *tree, const double *pos, void *data)
   1.199 +{
   1.200 +	if (insert_rec(&tree->root, pos, data, 0, tree->dim)) {
   1.201 +		return -1;
   1.202 +	}
   1.203 +
   1.204 +	if (tree->rect == 0) {
   1.205 +		tree->rect = hyperrect_create(tree->dim, pos, pos);
   1.206 +	} else {
   1.207 +		hyperrect_extend(tree->rect, pos);
   1.208 +	}
   1.209 +
   1.210 +	return 0;
   1.211 +}
   1.212 +
   1.213 +int kd_insertf(struct kdtree *tree, const float *pos, void *data)
   1.214 +{
   1.215 +	static double sbuf[16];
   1.216 +	double *bptr, *buf = 0;
   1.217 +	int res, dim = tree->dim;
   1.218 +
   1.219 +	if(dim > 16) {
   1.220 +#ifndef NO_ALLOCA
   1.221 +		if(dim <= 256)
   1.222 +			bptr = buf = alloca(dim * sizeof *bptr);
   1.223 +		else
   1.224 +#endif
   1.225 +			if(!(bptr = buf = malloc(dim * sizeof *bptr))) {
   1.226 +				return -1;
   1.227 +			}
   1.228 +	} else {
   1.229 +		bptr = buf = sbuf;
   1.230 +	}
   1.231 +
   1.232 +	while(dim-- > 0) {
   1.233 +		*bptr++ = *pos++;
   1.234 +	}
   1.235 +
   1.236 +	res = kd_insert(tree, buf, data);
   1.237 +#ifndef NO_ALLOCA
   1.238 +	if(tree->dim > 256)
   1.239 +#else
   1.240 +	if(tree->dim > 16)
   1.241 +#endif
   1.242 +		free(buf);
   1.243 +	return res;
   1.244 +}
   1.245 +
   1.246 +int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data)
   1.247 +{
   1.248 +	double buf[3];
   1.249 +	buf[0] = x;
   1.250 +	buf[1] = y;
   1.251 +	buf[2] = z;
   1.252 +	return kd_insert(tree, buf, data);
   1.253 +}
   1.254 +
   1.255 +int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data)
   1.256 +{
   1.257 +	double buf[3];
   1.258 +	buf[0] = x;
   1.259 +	buf[1] = y;
   1.260 +	buf[2] = z;
   1.261 +	return kd_insert(tree, buf, data);
   1.262 +}
   1.263 +
   1.264 +static int find_nearest(struct kdnode *node, const double *pos, double range, struct res_node *list, int ordered, int dim)
   1.265 +{
   1.266 +	double dist_sq, dx;
   1.267 +	int i, ret, added_res = 0;
   1.268 +
   1.269 +	if(!node) return 0;
   1.270 +
   1.271 +	dist_sq = 0;
   1.272 +	for(i=0; i<dim; i++) {
   1.273 +		dist_sq += SQ(node->pos[i] - pos[i]);
   1.274 +	}
   1.275 +	if(dist_sq <= SQ(range)) {
   1.276 +		if(rlist_insert(list, node, ordered ? dist_sq : -1.0) == -1) {
   1.277 +			return -1;
   1.278 +		}
   1.279 +		added_res = 1;
   1.280 +	}
   1.281 +
   1.282 +	dx = pos[node->dir] - node->pos[node->dir];
   1.283 +
   1.284 +	ret = find_nearest(dx <= 0.0 ? node->left : node->right, pos, range, list, ordered, dim);
   1.285 +	if(ret >= 0 && fabs(dx) < range) {
   1.286 +		added_res += ret;
   1.287 +		ret = find_nearest(dx <= 0.0 ? node->right : node->left, pos, range, list, ordered, dim);
   1.288 +	}
   1.289 +	if(ret == -1) {
   1.290 +		return -1;
   1.291 +	}
   1.292 +	added_res += ret;
   1.293 +
   1.294 +	return added_res;
   1.295 +}
   1.296 +
   1.297 +#if 0
   1.298 +static int find_nearest_n(struct kdnode *node, const double *pos, double range, int num, struct rheap *heap, int dim)
   1.299 +{
   1.300 +	double dist_sq, dx;
   1.301 +	int i, ret, added_res = 0;
   1.302 +
   1.303 +	if(!node) return 0;
   1.304 +	
   1.305 +	/* if the photon is close enough, add it to the result heap */
   1.306 +	dist_sq = 0;
   1.307 +	for(i=0; i<dim; i++) {
   1.308 +		dist_sq += SQ(node->pos[i] - pos[i]);
   1.309 +	}
   1.310 +	if(dist_sq <= range_sq) {
   1.311 +		if(heap->size >= num) {
   1.312 +			/* get furthest element */
   1.313 +			struct res_node *maxelem = rheap_get_max(heap);
   1.314 +
   1.315 +			/* and check if the new one is closer than that */
   1.316 +			if(maxelem->dist_sq > dist_sq) {
   1.317 +				rheap_remove_max(heap);
   1.318 +
   1.319 +				if(rheap_insert(heap, node, dist_sq) == -1) {
   1.320 +					return -1;
   1.321 +				}
   1.322 +				added_res = 1;
   1.323 +
   1.324 +				range_sq = dist_sq;
   1.325 +			}
   1.326 +		} else {
   1.327 +			if(rheap_insert(heap, node, dist_sq) == -1) {
   1.328 +				return =1;
   1.329 +			}
   1.330 +			added_res = 1;
   1.331 +		}
   1.332 +	}
   1.333 +
   1.334 +
   1.335 +	/* find signed distance from the splitting plane */
   1.336 +	dx = pos[node->dir] - node->pos[node->dir];
   1.337 +
   1.338 +	ret = find_nearest_n(dx <= 0.0 ? node->left : node->right, pos, range, num, heap, dim);
   1.339 +	if(ret >= 0 && fabs(dx) < range) {
   1.340 +		added_res += ret;
   1.341 +		ret = find_nearest_n(dx <= 0.0 ? node->right : node->left, pos, range, num, heap, dim);
   1.342 +	}
   1.343 +
   1.344 +}
   1.345 +#endif
   1.346 +
   1.347 +static void kd_nearest_i(struct kdnode *node, const double *pos, struct kdnode **result, double *result_dist_sq, struct kdhyperrect* rect)
   1.348 +{
   1.349 +	int dir = node->dir;
   1.350 +	int i;
   1.351 +	double dummy, dist_sq;
   1.352 +	struct kdnode *nearer_subtree, *farther_subtree;
   1.353 +	double *nearer_hyperrect_coord, *farther_hyperrect_coord;
   1.354 +
   1.355 +	/* Decide whether to go left or right in the tree */
   1.356 +	dummy = pos[dir] - node->pos[dir];
   1.357 +	if (dummy <= 0) {
   1.358 +		nearer_subtree = node->left;
   1.359 +		farther_subtree = node->right;
   1.360 +		nearer_hyperrect_coord = rect->max + dir;
   1.361 +		farther_hyperrect_coord = rect->min + dir;
   1.362 +	} else {
   1.363 +		nearer_subtree = node->right;
   1.364 +		farther_subtree = node->left;
   1.365 +		nearer_hyperrect_coord = rect->min + dir;
   1.366 +		farther_hyperrect_coord = rect->max + dir;
   1.367 +	}
   1.368 +
   1.369 +	if (nearer_subtree) {
   1.370 +		/* Slice the hyperrect to get the hyperrect of the nearer subtree */
   1.371 +		dummy = *nearer_hyperrect_coord;
   1.372 +		*nearer_hyperrect_coord = node->pos[dir];
   1.373 +		/* Recurse down into nearer subtree */
   1.374 +		kd_nearest_i(nearer_subtree, pos, result, result_dist_sq, rect);
   1.375 +		/* Undo the slice */
   1.376 +		*nearer_hyperrect_coord = dummy;
   1.377 +	}
   1.378 +
   1.379 +	/* Check the distance of the point at the current node, compare it
   1.380 +	 * with our best so far */
   1.381 +	dist_sq = 0;
   1.382 +	for(i=0; i < rect->dim; i++) {
   1.383 +		dist_sq += SQ(node->pos[i] - pos[i]);
   1.384 +	}
   1.385 +	if (dist_sq < *result_dist_sq) {
   1.386 +		*result = node;
   1.387 +		*result_dist_sq = dist_sq;
   1.388 +	}
   1.389 +
   1.390 +	if (farther_subtree) {
   1.391 +		/* Get the hyperrect of the farther subtree */
   1.392 +		dummy = *farther_hyperrect_coord;
   1.393 +		*farther_hyperrect_coord = node->pos[dir];
   1.394 +		/* Check if we have to recurse down by calculating the closest
   1.395 +		 * point of the hyperrect and see if it's closer than our
   1.396 +		 * minimum distance in result_dist_sq. */
   1.397 +		if (hyperrect_dist_sq(rect, pos) < *result_dist_sq) {
   1.398 +			/* Recurse down into farther subtree */
   1.399 +			kd_nearest_i(farther_subtree, pos, result, result_dist_sq, rect);
   1.400 +		}
   1.401 +		/* Undo the slice on the hyperrect */
   1.402 +		*farther_hyperrect_coord = dummy;
   1.403 +	}
   1.404 +}
   1.405 +
   1.406 +struct kdres *kd_nearest(struct kdtree *kd, const double *pos)
   1.407 +{
   1.408 +	struct kdhyperrect *rect;
   1.409 +	struct kdnode *result;
   1.410 +	struct kdres *rset;
   1.411 +	double dist_sq;
   1.412 +	int i;
   1.413 +
   1.414 +	if (!kd) return 0;
   1.415 +	if (!kd->rect) return 0;
   1.416 +
   1.417 +	/* Allocate result set */
   1.418 +	if(!(rset = malloc(sizeof *rset))) {
   1.419 +		return 0;
   1.420 +	}
   1.421 +	if(!(rset->rlist = alloc_resnode())) {
   1.422 +		free(rset);
   1.423 +		return 0;
   1.424 +	}
   1.425 +	rset->rlist->next = 0;
   1.426 +	rset->tree = kd;
   1.427 +
   1.428 +	/* Duplicate the bounding hyperrectangle, we will work on the copy */
   1.429 +	if (!(rect = hyperrect_duplicate(kd->rect))) {
   1.430 +		kd_res_free(rset);
   1.431 +		return 0;
   1.432 +	}
   1.433 +
   1.434 +	/* Our first guesstimate is the root node */
   1.435 +	result = kd->root;
   1.436 +	dist_sq = 0;
   1.437 +	for (i = 0; i < kd->dim; i++)
   1.438 +		dist_sq += SQ(result->pos[i] - pos[i]);
   1.439 +
   1.440 +	/* Search for the nearest neighbour recursively */
   1.441 +	kd_nearest_i(kd->root, pos, &result, &dist_sq, rect);
   1.442 +
   1.443 +	/* Free the copy of the hyperrect */
   1.444 +	hyperrect_free(rect);
   1.445 +
   1.446 +	/* Store the result */
   1.447 +	if (result) {
   1.448 +		if (rlist_insert(rset->rlist, result, -1.0) == -1) {
   1.449 +			kd_res_free(rset);
   1.450 +			return 0;
   1.451 +		}
   1.452 +		rset->size = 1;
   1.453 +		kd_res_rewind(rset);
   1.454 +		return rset;
   1.455 +	} else {
   1.456 +		kd_res_free(rset);
   1.457 +		return 0;
   1.458 +	}
   1.459 +}
   1.460 +
   1.461 +struct kdres *kd_nearestf(struct kdtree *tree, const float *pos)
   1.462 +{
   1.463 +	static double sbuf[16];
   1.464 +	double *bptr, *buf = 0;
   1.465 +	int dim = tree->dim;
   1.466 +	struct kdres *res;
   1.467 +
   1.468 +	if(dim > 16) {
   1.469 +#ifndef NO_ALLOCA
   1.470 +		if(dim <= 256)
   1.471 +			bptr = buf = alloca(dim * sizeof *bptr);
   1.472 +		else
   1.473 +#endif
   1.474 +			if(!(bptr = buf = malloc(dim * sizeof *bptr))) {
   1.475 +				return 0;
   1.476 +			}
   1.477 +	} else {
   1.478 +		bptr = buf = sbuf;
   1.479 +	}
   1.480 +
   1.481 +	while(dim-- > 0) {
   1.482 +		*bptr++ = *pos++;
   1.483 +	}
   1.484 +
   1.485 +	res = kd_nearest(tree, buf);
   1.486 +#ifndef NO_ALLOCA
   1.487 +	if(tree->dim > 256)
   1.488 +#else
   1.489 +	if(tree->dim > 16)
   1.490 +#endif
   1.491 +		free(buf);
   1.492 +	return res;
   1.493 +}
   1.494 +
   1.495 +struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z)
   1.496 +{
   1.497 +	double pos[3];
   1.498 +	pos[0] = x;
   1.499 +	pos[1] = y;
   1.500 +	pos[2] = z;
   1.501 +	return kd_nearest(tree, pos);
   1.502 +}
   1.503 +
   1.504 +struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z)
   1.505 +{
   1.506 +	double pos[3];
   1.507 +	pos[0] = x;
   1.508 +	pos[1] = y;
   1.509 +	pos[2] = z;
   1.510 +	return kd_nearest(tree, pos);
   1.511 +}
   1.512 +
   1.513 +/* ---- nearest N search ---- */
   1.514 +/*
   1.515 +static kdres *kd_nearest_n(struct kdtree *kd, const double *pos, int num)
   1.516 +{
   1.517 +	int ret;
   1.518 +	struct kdres *rset;
   1.519 +
   1.520 +	if(!(rset = malloc(sizeof *rset))) {
   1.521 +		return 0;
   1.522 +	}
   1.523 +	if(!(rset->rlist = alloc_resnode())) {
   1.524 +		free(rset);
   1.525 +		return 0;
   1.526 +	}
   1.527 +	rset->rlist->next = 0;
   1.528 +	rset->tree = kd;
   1.529 +
   1.530 +	if((ret = find_nearest_n(kd->root, pos, range, num, rset->rlist, kd->dim)) == -1) {
   1.531 +		kd_res_free(rset);
   1.532 +		return 0;
   1.533 +	}
   1.534 +	rset->size = ret;
   1.535 +	kd_res_rewind(rset);
   1.536 +	return rset;
   1.537 +}*/
   1.538 +
   1.539 +struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range)
   1.540 +{
   1.541 +	int ret;
   1.542 +	struct kdres *rset;
   1.543 +
   1.544 +	if(!(rset = malloc(sizeof *rset))) {
   1.545 +		return 0;
   1.546 +	}
   1.547 +	if(!(rset->rlist = alloc_resnode())) {
   1.548 +		free(rset);
   1.549 +		return 0;
   1.550 +	}
   1.551 +	rset->rlist->next = 0;
   1.552 +	rset->tree = kd;
   1.553 +
   1.554 +	if((ret = find_nearest(kd->root, pos, range, rset->rlist, 0, kd->dim)) == -1) {
   1.555 +		kd_res_free(rset);
   1.556 +		return 0;
   1.557 +	}
   1.558 +	rset->size = ret;
   1.559 +	kd_res_rewind(rset);
   1.560 +	return rset;
   1.561 +}
   1.562 +
   1.563 +struct kdres *kd_nearest_rangef(struct kdtree *kd, const float *pos, float range)
   1.564 +{
   1.565 +	static double sbuf[16];
   1.566 +	double *bptr, *buf = 0;
   1.567 +	int dim = kd->dim;
   1.568 +	struct kdres *res;
   1.569 +
   1.570 +	if(dim > 16) {
   1.571 +#ifndef NO_ALLOCA
   1.572 +		if(dim <= 256)
   1.573 +			bptr = buf = alloca(dim * sizeof *bptr);
   1.574 +		else
   1.575 +#endif
   1.576 +			if(!(bptr = buf = malloc(dim * sizeof *bptr))) {
   1.577 +				return 0;
   1.578 +			}
   1.579 +	} else {
   1.580 +		bptr = buf = sbuf;
   1.581 +	}
   1.582 +
   1.583 +	while(dim-- > 0) {
   1.584 +		*bptr++ = *pos++;
   1.585 +	}
   1.586 +
   1.587 +	res = kd_nearest_range(kd, buf, range);
   1.588 +#ifndef NO_ALLOCA
   1.589 +	if(kd->dim > 256)
   1.590 +#else
   1.591 +	if(kd->dim > 16)
   1.592 +#endif
   1.593 +		free(buf);
   1.594 +	return res;
   1.595 +}
   1.596 +
   1.597 +struct kdres *kd_nearest_range3(struct kdtree *tree, double x, double y, double z, double range)
   1.598 +{
   1.599 +	double buf[3];
   1.600 +	buf[0] = x;
   1.601 +	buf[1] = y;
   1.602 +	buf[2] = z;
   1.603 +	return kd_nearest_range(tree, buf, range);
   1.604 +}
   1.605 +
   1.606 +struct kdres *kd_nearest_range3f(struct kdtree *tree, float x, float y, float z, float range)
   1.607 +{
   1.608 +	double buf[3];
   1.609 +	buf[0] = x;
   1.610 +	buf[1] = y;
   1.611 +	buf[2] = z;
   1.612 +	return kd_nearest_range(tree, buf, range);
   1.613 +}
   1.614 +
   1.615 +void kd_res_free(struct kdres *rset)
   1.616 +{
   1.617 +	clear_results(rset);
   1.618 +	free_resnode(rset->rlist);
   1.619 +	free(rset);
   1.620 +}
   1.621 +
   1.622 +int kd_res_size(struct kdres *set)
   1.623 +{
   1.624 +	return (set->size);
   1.625 +}
   1.626 +
   1.627 +void kd_res_rewind(struct kdres *rset)
   1.628 +{
   1.629 +	rset->riter = rset->rlist->next;
   1.630 +}
   1.631 +
   1.632 +int kd_res_end(struct kdres *rset)
   1.633 +{
   1.634 +	return rset->riter == 0;
   1.635 +}
   1.636 +
   1.637 +int kd_res_next(struct kdres *rset)
   1.638 +{
   1.639 +	rset->riter = rset->riter->next;
   1.640 +	return rset->riter != 0;
   1.641 +}
   1.642 +
   1.643 +void *kd_res_item(struct kdres *rset, double *pos)
   1.644 +{
   1.645 +	if(rset->riter) {
   1.646 +		if(pos) {
   1.647 +			memcpy(pos, rset->riter->item->pos, rset->tree->dim * sizeof *pos);
   1.648 +		}
   1.649 +		return rset->riter->item->data;
   1.650 +	}
   1.651 +	return 0;
   1.652 +}
   1.653 +
   1.654 +void *kd_res_itemf(struct kdres *rset, float *pos)
   1.655 +{
   1.656 +	if(rset->riter) {
   1.657 +		if(pos) {
   1.658 +			int i;
   1.659 +			for(i=0; i<rset->tree->dim; i++) {
   1.660 +				pos[i] = rset->riter->item->pos[i];
   1.661 +			}
   1.662 +		}
   1.663 +		return rset->riter->item->data;
   1.664 +	}
   1.665 +	return 0;
   1.666 +}
   1.667 +
   1.668 +void *kd_res_item3(struct kdres *rset, double *x, double *y, double *z)
   1.669 +{
   1.670 +	if(rset->riter) {
   1.671 +		if(*x) *x = rset->riter->item->pos[0];
   1.672 +		if(*y) *y = rset->riter->item->pos[1];
   1.673 +		if(*z) *z = rset->riter->item->pos[2];
   1.674 +	}
   1.675 +	return 0;
   1.676 +}
   1.677 +
   1.678 +void *kd_res_item3f(struct kdres *rset, float *x, float *y, float *z)
   1.679 +{
   1.680 +	if(rset->riter) {
   1.681 +		if(*x) *x = rset->riter->item->pos[0];
   1.682 +		if(*y) *y = rset->riter->item->pos[1];
   1.683 +		if(*z) *z = rset->riter->item->pos[2];
   1.684 +	}
   1.685 +	return 0;
   1.686 +}
   1.687 +
   1.688 +void *kd_res_item_data(struct kdres *set)
   1.689 +{
   1.690 +	return kd_res_item(set, 0);
   1.691 +}
   1.692 +
   1.693 +/* ---- hyperrectangle helpers ---- */
   1.694 +static struct kdhyperrect* hyperrect_create(int dim, const double *min, const double *max)
   1.695 +{
   1.696 +	size_t size = dim * sizeof(double);
   1.697 +	struct kdhyperrect* rect = 0;
   1.698 +
   1.699 +	if (!(rect = malloc(sizeof(struct kdhyperrect)))) {
   1.700 +		return 0;
   1.701 +	}
   1.702 +
   1.703 +	rect->dim = dim;
   1.704 +	if (!(rect->min = malloc(size))) {
   1.705 +		free(rect);
   1.706 +		return 0;
   1.707 +	}
   1.708 +	if (!(rect->max = malloc(size))) {
   1.709 +		free(rect->min);
   1.710 +		free(rect);
   1.711 +		return 0;
   1.712 +	}
   1.713 +	memcpy(rect->min, min, size);
   1.714 +	memcpy(rect->max, max, size);
   1.715 +
   1.716 +	return rect;
   1.717 +}
   1.718 +
   1.719 +static void hyperrect_free(struct kdhyperrect *rect)
   1.720 +{
   1.721 +	free(rect->min);
   1.722 +	free(rect->max);
   1.723 +	free(rect);
   1.724 +}
   1.725 +
   1.726 +static struct kdhyperrect* hyperrect_duplicate(const struct kdhyperrect *rect)
   1.727 +{
   1.728 +	return hyperrect_create(rect->dim, rect->min, rect->max);
   1.729 +}
   1.730 +
   1.731 +static void hyperrect_extend(struct kdhyperrect *rect, const double *pos)
   1.732 +{
   1.733 +	int i;
   1.734 +
   1.735 +	for (i=0; i < rect->dim; i++) {
   1.736 +		if (pos[i] < rect->min[i]) {
   1.737 +			rect->min[i] = pos[i];
   1.738 +		}
   1.739 +		if (pos[i] > rect->max[i]) {
   1.740 +			rect->max[i] = pos[i];
   1.741 +		}
   1.742 +	}
   1.743 +}
   1.744 +
   1.745 +static double hyperrect_dist_sq(struct kdhyperrect *rect, const double *pos)
   1.746 +{
   1.747 +	int i;
   1.748 +	double result = 0;
   1.749 +
   1.750 +	for (i=0; i < rect->dim; i++) {
   1.751 +		if (pos[i] < rect->min[i]) {
   1.752 +			result += SQ(rect->min[i] - pos[i]);
   1.753 +		} else if (pos[i] > rect->max[i]) {
   1.754 +			result += SQ(rect->max[i] - pos[i]);
   1.755 +		}
   1.756 +	}
   1.757 +
   1.758 +	return result;
   1.759 +}
   1.760 +
   1.761 +/* ---- static helpers ---- */
   1.762 +
   1.763 +#ifdef USE_LIST_NODE_ALLOCATOR
   1.764 +/* special list node allocators. */
   1.765 +static struct res_node *free_nodes;
   1.766 +
   1.767 +#ifndef NO_PTHREADS
   1.768 +static pthread_mutex_t alloc_mutex = PTHREAD_MUTEX_INITIALIZER;
   1.769 +#endif
   1.770 +
   1.771 +static struct res_node *alloc_resnode(void)
   1.772 +{
   1.773 +	struct res_node *node;
   1.774 +
   1.775 +#ifndef NO_PTHREADS
   1.776 +	pthread_mutex_lock(&alloc_mutex);
   1.777 +#endif
   1.778 +
   1.779 +	if(!free_nodes) {
   1.780 +		node = malloc(sizeof *node);
   1.781 +	} else {
   1.782 +		node = free_nodes;
   1.783 +		free_nodes = free_nodes->next;
   1.784 +		node->next = 0;
   1.785 +	}
   1.786 +
   1.787 +#ifndef NO_PTHREADS
   1.788 +	pthread_mutex_unlock(&alloc_mutex);
   1.789 +#endif
   1.790 +
   1.791 +	return node;
   1.792 +}
   1.793 +
   1.794 +static void free_resnode(struct res_node *node)
   1.795 +{
   1.796 +#ifndef NO_PTHREADS
   1.797 +	pthread_mutex_lock(&alloc_mutex);
   1.798 +#endif
   1.799 +
   1.800 +	node->next = free_nodes;
   1.801 +	free_nodes = node;
   1.802 +
   1.803 +#ifndef NO_PTHREADS
   1.804 +	pthread_mutex_unlock(&alloc_mutex);
   1.805 +#endif
   1.806 +}
   1.807 +#endif	/* list node allocator or not */
   1.808 +
   1.809 +
   1.810 +/* inserts the item. if dist_sq is >= 0, then do an ordered insert */
   1.811 +/* TODO make the ordering code use heapsort */
   1.812 +static int rlist_insert(struct res_node *list, struct kdnode *item, double dist_sq)
   1.813 +{
   1.814 +	struct res_node *rnode;
   1.815 +
   1.816 +	if(!(rnode = alloc_resnode())) {
   1.817 +		return -1;
   1.818 +	}
   1.819 +	rnode->item = item;
   1.820 +	rnode->dist_sq = dist_sq;
   1.821 +
   1.822 +	if(dist_sq >= 0.0) {
   1.823 +		while(list->next && list->next->dist_sq < dist_sq) {
   1.824 +			list = list->next;
   1.825 +		}
   1.826 +	}
   1.827 +	rnode->next = list->next;
   1.828 +	list->next = rnode;
   1.829 +	return 0;
   1.830 +}
   1.831 +
   1.832 +static void clear_results(struct kdres *rset)
   1.833 +{
   1.834 +	struct res_node *tmp, *node = rset->rlist->next;
   1.835 +
   1.836 +	while(node) {
   1.837 +		tmp = node;
   1.838 +		node = node->next;
   1.839 +		free_resnode(tmp);
   1.840 +	}
   1.841 +
   1.842 +	rset->rlist->next = 0;
   1.843 +}