fix bug in getting closest point

1 files changed, 55 insertions, 8 deletions

diff --git a/experimental/tectonics/quadtree.c b/experimental/tectonics/quadtree.c
index 0f3f9c7..3cb6639 100644
--- a/experimental/tectonics/quadtree.c
+++ b/experimental/tectonics/quadtree.c
@@ -3,12 +3,13 @@
 
 bool contains_point(struct quad_region_t region, struct point_t pt)
 {
-   if (-region.half_dim < (region.center.x - pt.x) <= region.half_dim)
+   if (region.center.x - region.half_dim > pt.x ||
+       region.center.x + region.half_dim <= pt.x)
       return false;
 
-   if (-region.half_dim < (region.center.y - pt.y) <= region.half_dim)
+   if (region.center.y - region.half_dim > pt.y ||
+       region.center.y + region.half_dim <= pt.y)
       return false;
-
    return true;
 }
 
@@ -91,7 +92,29 @@ bool insert(struct quadtree_node_t *node,
 }
 
 
+static int get_points_in_region(struct quadtree_node_t *node,
+				int* buffer)
+{
+   if (node->id != -1) {
+      *buffer = node->id;
+      return 1;
+   }
+
+   if (node->nw != NULL) {
+      int n_points = 0;
+      n_points += get_points_in_region(node->nw, buffer + n_points);
+      n_points += get_points_in_region(node->ne, buffer + n_points);
+      n_points += get_points_in_region(node->sw, buffer + n_points);
+      n_points += get_points_in_region(node->se, buffer + n_points);
+      return n_points;
+   }
+
+   return 0;
+}
+
+
 int get_closest(struct quadtree_node_t *node,
+		struct point_t *points,
 		struct point_t pt)
 {
    if (!contains_point(node->region, pt))
@@ -101,11 +124,35 @@ int get_closest(struct quadtree_node_t *node,
       return node->id;
 
    int closest;
-
-   if ((closest = get_closest(node->nw, pt)) != -1) return closest;
-   if ((closest = get_closest(node->ne, pt)) != -1) return closest;
-   if ((closest = get_closest(node->sw, pt)) != -1) return closest;
-   if ((closest = get_closest(node->se, pt)) != -1) return closest;
+   if (node->nw != NULL) {
+      if ((closest = get_closest(node->nw, points, pt)) != -1)
+	 return closest;
+      if ((closest = get_closest(node->ne, points, pt)) != -1)
+	 return closest;
+      if ((closest = get_closest(node->sw, points, pt)) != -1)
+	 return closest;
+      if ((closest = get_closest(node->se, points, pt)) != -1)
+	 return closest;
+
+      /* closest point is in a subregion, but not the *same*
+       * subregion as the point itself, so we need to check against
+       * all points contained within the region
+       */
+
+      int buffer[1000];
+      int n_points = get_points_in_region(node, buffer);
+      double dist = 1e9;
+      for (int i=0; i<n_points; i++) {
+	 int id = buffer[i];
+	 double d = distance(points[id], pt);
+	 if (d < dist) {
+	    closest = id;
+	    dist = d;
+	 }
+      }
+
+      return closest;
+   }
 
    return -1;
 }