static void kd_nearest_i(struct kdnode *node, const double *pos, struct kdnode **result, double *result_dist_sq, struct kdhyperrect* rect)
{
int dir = node->dir;
int i, side;
double dummy, dist_sq;
struct kdnode *nearer_subtree, *farther_subtree;
double *nearer_hyperrect_coord, *farther_hyperrect_coord;
/* Decide whether to go left or right in the tree */
dummy = pos[dir] - node->pos[dir];
if (dummy <= 0) {
nearer_subtree = node->left;
farther_subtree = node->right;
nearer_hyperrect_coord = rect->max + dir;
farther_hyperrect_coord = rect->min + dir;
side = 0;
} else {
nearer_subtree = node->right;
farther_subtree = node->left;
nearer_hyperrect_coord = rect->min + dir;
farther_hyperrect_coord = rect->max + dir;
side = 1;
}
if (nearer_subtree) {
/* Slice the hyperrect to get the hyperrect of the nearer subtree */
dummy = *nearer_hyperrect_coord;
*nearer_hyperrect_coord = node->pos[dir];
/* Recurse down into nearer subtree */
kd_nearest_i(nearer_subtree, pos, result, result_dist_sq, rect);
/* Undo the slice */
*nearer_hyperrect_coord = dummy;
}
/* Check the distance of the point at the current node, compare it
* with our best so far */
dist_sq = 0;
for(i=0; i < rect->dim; i++) {
dist_sq += SQ(node->pos[i] - pos[i]);
}
if (dist_sq < *result_dist_sq) {
*result = node;
*result_dist_sq = dist_sq;
}
if (farther_subtree) {
/* Get the hyperrect of the farther subtree */
dummy = *farther_hyperrect_coord;
*farther_hyperrect_coord = node->pos[dir];
/* Check if we have to recurse down by calculating the closest
* point of the hyperrect and see if it's closer than our
* minimum distance in result_dist_sq. */
if (hyperrect_dist_sq(rect, pos) < *result_dist_sq) {
/* Recurse down into farther subtree */
kd_nearest_i(farther_subtree, pos, result, result_dist_sq, rect);
}
/* Undo the slice on the hyperrect */
*farther_hyperrect_coord = dummy;
}
}
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;
}
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;
}
}
/// return a point in the tree that is nearest to the given point
/// false is returned if the tree is empty.
std::pair<point_type,bool> nearest( const point_type& pos) {
if (!root_) return std::make_pair(pos,false);
num_nearest_i_calls=0;
kd_hyperrect<point_type> rect = *rect_;
kd_node<point_type>* result;
/* Our first guesstimate is the root node */
result = root_;
/* Search for the nearest neighbour recursively */
double result_dist = root_->pos.dist(pos);
kd_nearest_i( root_, pos, result, result_dist , rect);
return std::make_pair( result->pos, true);
}
/// find nearest point
void kd_nearest_i(kd_node<point_type> *node, const point_type pos,
kd_node<point_type>*& result, double& result_dist_sq , kd_hyperrect<point_type>& rect) {
int dir = node->dir;
num_nearest_i_calls++;
//int i;
//double dummy;
kd_node<point_type> *nearer_subtree, *farther_subtree;
double nearer_hyperrect_coord, farther_hyperrect_coord;
/* Decide whether to go left or right in the tree */
//dummy = pos[dir] - node->pos[dir];
//if (dummy <= 0) {
bool b;
if (pos[dir] <= node->pos[dir]) {
nearer_subtree = node->left;
farther_subtree = node->right;
nearer_hyperrect_coord = rect.max[dir];
farther_hyperrect_coord = rect.min[dir];
b= true;
} else {
nearer_subtree = node->right;
farther_subtree = node->left;
nearer_hyperrect_coord = rect.min[dir];
farther_hyperrect_coord = rect.max[dir];
b=false;
}
if (nearer_subtree) {
//std::cout << " recurse into nearer subtree \n";
/* Slice the hyperrect to get the hyperrect of the nearer subtree */
double dummy = nearer_hyperrect_coord;
//nearer_hyperrect_coord = node->pos[dir];
if (b)
rect.max[dir] = node->pos[dir];
else
rect.min[dir] = node->pos[dir];
/* Recurse down into nearer subtree */
kd_nearest_i(nearer_subtree, pos, result, result_dist_sq, rect);
/* Undo the slice */
if (b)
rect.max[dir] = dummy; //node->pos[dir];
else
rect.min[dir] = dummy; //node->pos[dir];
//rect->min[dir] = //nearer_hyperrect_coord = dummy;
}
/* Check the distance of the point at the current node, compare it
* with our best so far */
double dist_sq = node->pos.dist( pos );
//for(int i=0; i < dim_; i++) {
// dist_sq += sq(node->pos[i] - pos[i]);
//}
if (dist_sq < result_dist_sq) {
result = node;
result_dist_sq = dist_sq;
}
if (farther_subtree) {
//std::cout << " recurse into farther subtree \n";
/* Get the hyperrect of the farther subtree */
double dummy = farther_hyperrect_coord;
if (b)
rect.min[dir] = node->pos[dir];
else
rect.max[dir] = node->pos[dir];
//farther_hyperrect_coord = node->pos[dir]; // this changes rect_ !!
/* Check if we have to recurse down by calculating the closest
* point of the hyperrect and see if it's closer than our
* minimum distance in result_dist_sq. */
if (hyperrect_dist_sq(rect, pos) < result_dist_sq)
{
/* Recurse down into farther subtree */
kd_nearest_i(farther_subtree, pos, result, result_dist_sq, rect);
//double dist_sq = node->pos.dist( pos );
//for(int i=0; i < dim_; i++) {
// dist_sq += sq(node->pos[i] - pos[i]);
//}
/*
if (dist_sq < result_dist_sq) {
result = node;
result_dist_sq = dist_sq;
}*/
}
/* Undo the slice on the hyperrect */
if (b)
rect.min[dir] = dummy;
else
rect.max[dir] = dummy;
//*farther_hyperrect_coord = dummy;
}
}
