struct kdres *kd_generic_nearest_range(
struct kdtree *kd,
const double *pos,
double range,
double (*distance_function)(const double *, const double *, int k),
int (*compare_function)(double, double)
)
{
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, distance_function, compare_function)) == -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;
//pthread_mutex_lock(&alloc_mutex2);
rset = malloc(sizeof *rset);
//pthread_mutex_unlock(&alloc_mutex2);
if(!(rset)) {
return 0;
}
//pthread_mutex_lock(&alloc_mutex2);
rset->rlist = alloc_resnode();
//pthread_mutex_unlock(&alloc_mutex2);
if(!(rset->rlist)) {
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(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;
}
}
/* 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;
}
