struct kdres *kd_nearest_range(struct kdtree *kd, const double *pos, double range)
{
int ret;
struct kdres *rset;
if(!(rset = new kdres)){
return 0;
}
if(!(rset->rlist = new res_node)){
delete rset;
return 0;
}
rset->tlist->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 = rst;
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_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(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;
}
}
// Takes a kdres set and returns a list of nodes
GSList *opttree_kdtree_to_gslist (state_t * state, kdres_t *kdres) {
GSList *node_list = NULL;
kd_res_rewind (kdres);
while (!kd_res_end(kdres)) {
node_t * node_curr = kd_res_item_data (kdres);
node_list = g_slist_prepend (node_list, node_curr);
kd_res_next (kdres);
}
return node_list;
}