From 59cf5606175a59aa024005cf66dd1d25fb633d58 Mon Sep 17 00:00:00 2001 From: Timo Kokkonen Date: Wed, 13 Jul 2011 19:04:52 +0300 Subject: [PATCH] quadtree: Add better nearest neighborhood search This one attempts to leave majority of the tree unchecked while searching for the nearest neighborhood, but ensures still that the nearest node is always found. Signed-off-by: Timo Kokkonen --- quadtree.c | 152 +++++++++++++++++++++++++++++++++-------------------- 1 file changed, 95 insertions(+), 57 deletions(-) diff --git a/quadtree.c b/quadtree.c index 827384a..9338a85 100644 --- a/quadtree.c +++ b/quadtree.c @@ -293,29 +293,44 @@ static int is_within(struct vector *pos, struct vector *corner) return 0; } -static int quadrants_to_search(struct quadtree *node, struct vector *corner) +static int rectangle_and_circle_overlap(struct vector *corner, + struct vector *pos, double radius) { - int direction = 0, i; - int up = 0, left = 0, right = 0, down = 0; + int vertically_overlapping = 0, horizontally_overlapping = 0; - for (i = 0; i < 2; i++) { - if (corner[i].x <= node->pos.x) - left = 1; - else if (corner[i].x >= node->pos.x) - right = 1; - if (corner[i].y <= node->pos.y) - up = 1; - else if (corner[i].y >= node->pos.y) - down = 1; - } + if ((pos->x < corner[1].x && pos->x + radius > corner[0].x) || + (pos->x > corner[0].x && pos->x - radius < corner[1].x)) + horizontally_overlapping = 1; - if (left || up) + if ((pos->y < corner[1].y && pos->y + radius > corner[0].y) || + (pos->y > corner[0].y && pos->y - radius < corner[1].y)) + vertically_overlapping = 1; + + return horizontally_overlapping && vertically_overlapping; +} + +static int quadrants_to_search(struct quadtree *node, struct vector *pos, + double dist) +{ + int direction = 0; + + if (dist <= 0) + return 0xf; + + if (node->child[0] && + rectangle_and_circle_overlap(node->child[0]->corner, pos, dist)) direction |= QUADTREE_UPLEFT; - if (right || up) + + if (node->child[1] && + rectangle_and_circle_overlap(node->child[1]->corner, pos, dist)) direction |= QUADTREE_UPRIGHT; - if (left || down) + + if (node->child[2] && + rectangle_and_circle_overlap(node->child[2]->corner, pos, dist)) direction |= QUADTREE_DOWNLEFT; - if (right || down) + + if (node->child[3] && + rectangle_and_circle_overlap(node->child[3]->corner, pos, dist)) direction |= QUADTREE_DOWNRIGHT; return direction; @@ -323,68 +338,77 @@ static int quadrants_to_search(struct quadtree *node, struct vector *corner) static struct quadtree *quadtree_find_nearest(struct quadtree *tree, struct vector *pos, - struct vector *corner) + struct vector *corner, + struct quadtree *nearest, + int depth) { struct vector tmp; - struct quadtree *nearest, *node; + struct quadtree *node; double distance = 0, dist; int i, directions; - if (!is_within(pos, corner)) { - if (debug) { - printf("No nearest to be found from (%f %f) (%f %f) " - "pos (%f %f)\n", - corner[0].x, corner[0].y, - corner[1].x, corner[1].y, - pos->x, pos->y); - fflush(stdout); - } - return NULL; - } - - if (is_within(&tree->pos, corner)) { - vector_sub(pos, &tree->pos, &tmp); - distance = vector_abs(&tmp); + if (nearest == NULL) nearest = tree; - } else { + + vector_sub(&nearest->pos, pos, &tmp); + distance = vector_abs(&tmp); + + if (!is_within(&nearest->pos, corner)) { + if (debug) + printf("Discarding nearest %p " + "as outside of search window\n", nearest); nearest = NULL; + distance = -1; } - directions = quadrants_to_search(tree, corner); - + directions = quadrants_to_search(tree, pos, distance); + if (debug) + printf("%d: Directions: %x\n", depth, directions); for (i = 0; i < 4; i++) { if (!tree->child[i]) continue; -/* if (!(directions & (1 << i))) continue; -*/ - node = quadtree_find_nearest(tree->child[i], pos, corner); - if (!node) - continue; - vector_sub(pos, &node->pos, &tmp); - dist = vector_abs(&tmp); + node = quadtree_find_nearest(tree->child[i], pos, corner, + nearest, depth + 1); - if (!nearest || dist < distance) { - nearest = node; - distance = dist; + if (!node) + continue; + if (node != nearest) { + vector_sub(&node->pos, pos, &tmp); + dist = vector_abs(&tmp); + if (dist < distance || !nearest) { + nearest = node; + distance = dist; + } + /* + * Update the directions where to search for + * since those may not be valid any more + */ + directions = quadrants_to_search(tree, pos, distance); + continue; } } if (nearest && !is_within(&nearest->pos, corner)) { if (debug) - printf("Node %p (%f %f) is not within " - "search window (%f %f) (%f %f)\n", + printf("%d: Node %p (%f %f) is not within " + "search window (%f %f) (%f %f)\n", depth, nearest, nearest->pos.x, nearest->pos.y, corner[0].x, corner[0].y, corner[1].x, corner[1].y); return NULL; } - + if (debug) + printf("%d: Node %p (%f %f) is nearest node, dist %f\n", + depth, nearest, + nearest ? nearest->pos.x : -1, + nearest ? nearest->pos.y : -1, + nearest ? distance : -1); return nearest; } @@ -392,13 +416,13 @@ static struct quadtree *quadtree_find_nearest_noparent(struct quadtree *tree, struct vector *pos, struct vector *corner) { - struct quadtree *nearest; + struct quadtree *nearest = NULL; struct vector sub; struct quadtree *node; double dist = 0, dist2; int i; - nearest = quadtree_find_nearest(tree, pos, corner); + nearest = quadtree_find_nearest(tree, pos, corner, nearest, 0); if (nearest != tree) goto out; @@ -417,7 +441,7 @@ static struct quadtree *quadtree_find_nearest_noparent(struct quadtree *tree, if (!nearest) { nearest = quadtree_find_nearest(tree->child[i], pos, - corner); + corner, nearest, 0); if (nearest == NULL) continue; @@ -426,8 +450,8 @@ static struct quadtree *quadtree_find_nearest_noparent(struct quadtree *tree, dist = vector_abs(&sub); continue; } - node = quadtree_find_nearest(tree->child[i], - pos, corner); + node = quadtree_find_nearest(tree->child[i], pos, corner, + nearest, 0); if (node == NULL) continue; @@ -460,6 +484,12 @@ static int quadtree_split_by_node(struct quadtree *node, return 0; } + if (debug) + printf("Search rectangle was before (%f %f) (%f %f), (%f %f)\n", + corner[0].x, corner[0].y, + corner[1].x, corner[1].y, + node->pos.x, node->pos.y); + switch (dir) { case QUADTREE_UPLEFT: corner[0] = node->pos; @@ -521,6 +551,7 @@ static int _quadtree_del(struct quadtree *node, struct quadtree *parent) n = node->child[i]; _quadtree_del(n, parent); _quadtree_add(parent, n); + recalculate_parent_area_stats(n); } return 0; @@ -593,7 +624,7 @@ static int optimally_move_tree(struct quadtree *tree, struct quadtree *parent, moved += optimally_move_tree(tree, parent, corner, ops); get_middle_point(corner_orig, &mid); - tmp = quadtree_find_nearest(tree, &mid, corner_orig); + tmp = quadtree_find_nearest(tree, &mid, corner_orig, NULL, 0); if (tmp && tmp != tree) trap(); @@ -668,7 +699,7 @@ struct quadtree *quadtree_del(struct quadtree *node, * The sub branch is now detached from the main * tree. Fix the stats. */ - quadtree_recalculate_parent_stats(node->parent, ops); + quadtree_recalculate_parent_stats(parent, ops); } validate_tree(parent); @@ -682,6 +713,13 @@ struct quadtree *quadtree_del(struct quadtree *node, corner[0] = node->corner[1]; corner[1] = node->corner[0]; + if (debug) { + printf("\nInitial Search rectangle (%f %f) (%f %f), (%f %f)\n", + corner[0].x, corner[0].y, + corner[1].x, corner[1].y, + node->pos.x, node->pos.y); + fflush(stdout); + } moved = optimally_move_tree(node, parent, corner, ops); if (debug) { -- 2.44.0