fix bug in getting closest point

7 files changed, 119 insertions, 21 deletions

diff --git a/experimental/tectonics/output-relaxed.png b/experimental/tectonics/output-relaxed.png
new file mode 100644
index 0000000..f53c0d1
--- /dev/null
+++ b/experimental/tectonics/output-relaxed.png
diff --git a/experimental/tectonics/output.png b/experimental/tectonics/output.png
index 041699a..1fddaf3 100644
--- a/experimental/tectonics/output.png
+++ b/experimental/tectonics/output.png
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;
 }
diff --git a/experimental/tectonics/quadtree.test.c b/experimental/tectonics/quadtree.test.c
index 273087b..472cc0d 100644
--- a/experimental/tectonics/quadtree.test.c
+++ b/experimental/tectonics/quadtree.test.c
@@ -209,16 +209,33 @@ mu_test test_get_closest()
    insert(&node, pts, 1);
    insert(&node, pts, 2);
 
-   int closest_75 = get_closest(&node, (struct point_t){0.75, 0.75});
+   mu_assert_equal(node.nw->nw->id,  2);
+   mu_assert_equal(node.nw->nw->nw,  NULL);
+   mu_assert_equal(node.nw->ne->id, -1);
+   mu_assert_equal(node.nw->ne->nw,  NULL);
+   mu_assert_equal(node.nw->sw->id, -1);
+   mu_assert_equal(node.nw->sw->nw,  NULL);
+   mu_assert_equal(node.nw->se->id,  1);
+   mu_assert_equal(node.nw->se->nw,  NULL);
+
+   int closest_75 = get_closest(&node, pts,
+				(struct point_t){0.75, 0.75});
    mu_assert_equal(closest_75, 0);
 
-   int closest_33 = get_closest(&node, (struct point_t){0.3, 0.3});
+   int closest_33 = get_closest(&node, pts,
+				(struct point_t){0.3, 0.3});
    mu_assert_equal(closest_33, 1);
 
-   int closest_22 = get_closest(&node, (struct point_t){0.2, 0.2});
+   int closest_22 = get_closest(&node, pts,
+				(struct point_t){0.2, 0.2});
    mu_assert_equal(closest_22, 2);
 
-   int closest_nil = get_closest(&node, (struct point_t){100, 100});
+   int closest_252 = get_closest(&node, pts,
+				 (struct point_t){0.251, 0.21});
+   mu_assert_equal(closest_252, 2);
+
+   int closest_nil = get_closest(&node, pts,
+				 (struct point_t){100, 100});
    mu_assert_equal(closest_nil, -1);
 
    return 0;
diff --git a/experimental/tectonics/tectonics.h b/experimental/tectonics/tectonics.h
index 85b86fe..b2793e9 100644
--- a/experimental/tectonics/tectonics.h
+++ b/experimental/tectonics/tectonics.h
@@ -46,6 +46,7 @@ void subdivide(struct quadtree_node_t *node, struct point_t *points);
 bool insert(struct quadtree_node_t *node,
 	    struct point_t *points, int id);
 int get_closest(struct quadtree_node_t *root,
+		struct point_t *points,
 		struct point_t pt);
 void draw_quadtree(cairo_t *cr, struct quadtree_node_t *root);
 void free_tree(struct quadtree_node_t *root);
@@ -59,7 +60,7 @@ void free_tree(struct quadtree_node_t *root);
 
 void get_cairo_size(cairo_t *cr, int *width, int *height);
 double rand01();
-double distance(double x0, double y0, double x1, double y1);
+double distance(struct point_t p1, struct point_t p2);
 
 
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
diff --git a/experimental/tectonics/util.c b/experimental/tectonics/util.c
index a0a32a3..991ebc6 100644
--- a/experimental/tectonics/util.c
+++ b/experimental/tectonics/util.c
@@ -16,10 +16,10 @@ double rand01()
 }
 
 
-double distance(double x0, double y0, double x1, double y1)
+double distance(struct point_t p1, struct point_t p2)
 {
-   double dx = x1-x0;
-   double dy = y1-y0;
+   double dx = p1.x-p2.x;
+   double dy = p1.y-p2.y;
 
    return sqrt(dx*dx + dy*dy);
 }
diff --git a/experimental/tectonics/world.c b/experimental/tectonics/world.c
index cd56cdd..a1dd4de 100644
--- a/experimental/tectonics/world.c
+++ b/experimental/tectonics/world.c
@@ -8,6 +8,25 @@ struct centroid_t {
    int updates;
 };
 
+static void rebuild_tree(struct world_t *world)
+{
+   if (world->tree.nw != NULL) {
+      free_tree(world->tree.nw);
+      free_tree(world->tree.ne);
+      free_tree(world->tree.sw);
+      free_tree(world->tree.se);
+      world->tree.nw = NULL;
+      world->tree.ne = NULL;
+      world->tree.sw = NULL;
+      world->tree.se = NULL;
+   }
+   world->tree.id = -1;
+
+   for (int i=0; i<world->n_points; i++) {
+      insert(&(world->tree), world->points, i);
+   }
+}
+
 static void relax_points(struct world_t *world, int iterations)
 {
    struct centroid_t *centroid =
@@ -26,12 +45,19 @@ static void relax_points(struct world_t *world, int iterations)
       double x = rand01();
       double y = rand01();
       int closest = get_closest(&(world->tree),
+				world->points,
 				(struct point_t){x, y});
 
-      int t = centroid[closest].updates;
-      centroid[closest].x = (t*centroid[closest].x + x)/(t+1);
-      centroid[closest].y = (t*centroid[closest].y + y)/(t+1);
-      centroid[closest].updates += 1;
+      if (closest == -1) {
+	 printf("WARN: bad closest point!\n");
+      }
+      else {
+	 struct centroid_t *c = centroid + closest;
+	 int u = c->updates;
+	 c->x = (u*c->x + x)/(u+1);
+	 c->y = (u*c->y + y)/(u+1);
+	 c->updates += 1;
+      }
    }
 
    for (int i=0; i<world->n_points; i++) {
@@ -41,6 +67,8 @@ static void relax_points(struct world_t *world, int iterations)
       pt->y = c->y;
    }
 
+   rebuild_tree(world);
+
    free(centroid);
 }
 
@@ -63,8 +91,13 @@ void create_world(struct world_t *world, int n_points)
       pt->x = rand01();
       pt->y = rand01();
 
-      insert(&(world->tree), world->points, i);
+      //insert(&(world->tree), world->points, i);
    }
+
+   rebuild_tree(world);
+
+   for (int i=0; i<10; i++)
+      relax_points(world, 100000);
 }