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 <kaapeli@itanic.dy.fi>

diff --git a/quadtree.c b/quadtree.c
index 827384aae129ce5fa8f97f4d40771dd90629535a..9338a8523f326fe362066e4129c7b3d96123be36 100644 (file)
--- 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) {