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; }