1 files changed, 135 insertions, 0 deletions
diff --git a/experimental/tectonics/quadtree.c b/experimental/tectonics/quadtree.c
new file mode 100644
index 0000000..dead9f2
--- /dev/null
+++ b/experimental/tectonics/quadtree.c
@@ -0,0 +1,135 @@
+#include <math.h>
+#include "tectonics.h"
+
+bool contains_point(struct quad_region_t region, struct point_t pt)
+{
+   if (-region.half_dim < (region.center.x - pt.x) <= region.half_dim)
+      return false;
+
+   if (-region.half_dim < (region.center.y - pt.y) <= region.half_dim)
+      return false;
+
+   return true;
+}
+
+
+struct quadtree_node_t new_node(struct point_t center, double half_dim)
+{
+   struct quadtree_node_t node;
+
+   node.region = (struct quad_region_t) { center, half_dim };
+   node.id = -1;
+   node.nw = NULL;
+   node.ne = NULL;
+   node.sw = NULL;
+   node.se = NULL;
+
+   return node;
+}
+
+
+void subdivide(struct quadtree_node_t *node, struct point_t *points)
+{
+   node->nw = malloc(sizeof(struct quadtree_node_t));
+   node->ne = malloc(sizeof(struct quadtree_node_t));
+   node->sw = malloc(sizeof(struct quadtree_node_t));
+   node->se = malloc(sizeof(struct quadtree_node_t));
+
+   struct point_t center = node->region.center;
+   double quarter_dim = node->region.half_dim / 2;
+
+   struct point_t c_nw =
+      { center.x - quarter_dim, center.y - quarter_dim };
+   *(node->nw) = new_node(c_nw, quarter_dim);
+
+   struct point_t c_ne =
+      { center.x + quarter_dim, center.y - quarter_dim };
+   *(node->ne) = new_node(c_ne, quarter_dim);
+
+   struct point_t c_sw =
+      { center.x - quarter_dim, center.y + quarter_dim };
+   *(node->sw) = new_node(c_sw, quarter_dim);
+
+   struct point_t c_se =
+      { center.x + quarter_dim, center.y + quarter_dim };
+   *(node->se) = new_node(c_se, quarter_dim);
+
+   if (node->id != -1) {
+   /* move point to a child */
+      if (contains_point(node->nw->region, points[node->id]))
+	 node->nw->id = node->id;
+      else if (contains_point(node->ne->region, points[node->id]))
+	 node->ne->id = node->id;
+      else if (contains_point(node->sw->region, points[node->id]))
+	 node->sw->id = node->id;
+      else
+	 node->se->id = node->id;
+      node->id = -1;
+   }
+}
+
+bool insert(struct quadtree_node_t *node,
+	    struct point_t *points, int id)
+{
+   if (!contains_point(node->region, points[id]))
+      return false;
+
+   if (node->id == -1 && node->nw == NULL) {
+      node->id = id;
+      return true;
+   }
+
+   if (node->nw == NULL)
+      subdivide(node, points);
+
+   if (insert(node->nw, points, id)) return true;
+   if (insert(node->ne, points, id)) return true;
+   if (insert(node->sw, points, id)) return true;
+   if (insert(node->se, points, id)) return true;
+
+   return false;
+}
+
+
+int get_closest(struct quadtree_node_t *node,
+		struct point_t pt)
+{
+   if (!contains_point(node->region, pt))
+      return -1;
+
+   if (node->id != -1)
+      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;
+
+   return -1;
+}
+
+
+void draw_quadtree(cairo_t *cr, struct quadtree_node_t *node)
+{
+   int width, height;
+   get_cairo_size(cr, &width, &height);
+
+   cairo_set_source_rgba(cr, 0, 0, 1, 1);
+   struct point_t center = node->region.center;
+   double half_dim = node->region.half_dim;
+
+   double x = (center.x - half_dim) * width;
+   double y = (center.y - half_dim) * height;
+   
+   cairo_rectangle(cr, x, y, 2*half_dim * width, 2*half_dim * height);
+   cairo_stroke(cr);
+
+   if (node->nw != NULL) {
+      draw_quadtree(cr, node->nw);
+      draw_quadtree(cr, node->ne);
+      draw_quadtree(cr, node->sw);
+      draw_quadtree(cr, node->se);
+   }
+}