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