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;
}
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: add > 16 dimensions handling */
static int find_nearest(
struct kdnode *node,
const double *pos,
double range,
struct res_node *list,
int ordered,
int dim,
double (*distance_function)(const double *, const double *, int k),
int (*compare_function)(double, double)
)
{
double dist, one_dim_dist, one_dim_relative_dist;
int i, ret, added_res = 0;
double one_dim_pos[16];
if(!node) return 0;
if(dim > 16) return -1;
dist = (*distance_function)(node->pos, pos, dim);
if((*compare_function)(dist, range) <= 0) {
if(rlist_insert(list, node, ordered ? dist : -1.0) == -1) {
return -1;
}
added_res = 1;
}
/* isolate single dimension position from node->pos into one_dim_pos for single dimension distance computation */
for(i = 0; i < dim; i++) {
one_dim_pos[i] = pos[i];
}
one_dim_pos[node->dir] = node->pos[node->dir];
one_dim_dist = (*distance_function)(pos, one_dim_pos, dim);
one_dim_relative_dist = pos[node->dir] - node->pos[node->dir];
ret = find_nearest(one_dim_relative_dist <= 0.0 ? node->left : node->right, pos, range, list, ordered, dim, distance_function, compare_function);
if(ret >= 0 && (*compare_function)(one_dim_dist, range) < 0) {
added_res += ret;
ret = find_nearest(one_dim_relative_dist <= 0.0 ? node->right : node->left, pos, range, list, ordered, dim, distance_function, compare_function);
}
if(ret == -1) {
return -1;
}
added_res += ret;
return added_res;
}
