struct kdnode *kd_find_exact(struct kdtree *kd, const double *pos)
{
struct kdhyperrect *rect;
struct kdnode *result;
double dist_sq;
int i;
if (!kd) return 0;
if (!kd->rect) return 0;
/* Duplicate the bounding hyperrectangle, we will work on the copy */
if (!(rect = hyperrect_duplicate(kd->rect))) {
return NULL;
}
/* 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);
return dist_sq == 0.0 ? result : NULL;
}
void kd_clear(struct kdtree *tree) {
clear_rec(tree->root, tree->destr);
tree->root = 0;
if (tree->rect) {
hyperrect_free(tree->rect);
tree->rect = 0;
}
}
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;
}
}
