From 31f872434427ad22546e1f461ff2e565fecc6396 Mon Sep 17 00:00:00 2001
From: Timo Kokkonen <kaapeli@itanic.dy.fi>
Date: Tue, 7 Dec 2010 22:13:46 +0200
Subject: [PATCH] quadtree: Implement proper rebalancing

Select nodes in predetermined spatial order that will produce well
balanced tree.

Signed-off-by: Timo Kokkonen <kaapeli@itanic.dy.fi>
---
 quadtree.c | 582 +++++++++++++++++++++++++++++++++++++++++++----------
 1 file changed, 481 insertions(+), 101 deletions(-)

diff --git a/quadtree.c b/quadtree.c
index da3b43d..5e57afd 100644
--- a/quadtree.c
+++ b/quadtree.c
@@ -166,6 +166,27 @@ void _quadtree_validate_tree(const struct quadtree *node)
 	validate_tree(node);
 }
 
+static struct quadtree *_quadtree_add(struct quadtree *parent,
+				struct quadtree *new)
+{
+	int ret, i;
+
+	ret = quadtree_compare(parent, new);
+
+	if (ret < 0 || ret >= 4) {
+		printf("Invalid comparison result of %d\n", ret);
+		return 0;
+	}
+
+	if (parent->child[ret])
+		return _quadtree_add(parent->child[ret], new);
+
+	parent->child[ret] = new;
+	new->parent = parent;
+	for (i = 0; i < 4; i++)
+		new->child[i] = 0;
+
+	return new;
 }
 
 /**
@@ -183,26 +204,12 @@ void _quadtree_validate_tree(const struct quadtree *node)
 struct quadtree *quadtree_add(struct quadtree *parent, struct quadtree *new,
 			      struct quadtree_ops *ops)
 {
-	int ret, i;
 	if (parent == new)
 		trap();
 
 	validate_tree(parent);
 
-	ret = quadtree_compare(parent, new);
-
-	if (ret < 0 || ret >= 4) {
-		printf("Invalid comparison result of %d\n", ret);
-		return 0;
-	}
-
-	if (parent->child[ret])
-		return quadtree_add(parent->child[ret], new, ops);
-
-	parent->child[ret] = new;
-	new->parent = parent;
-	for (i = 0; i < 4; i++)
-		new->child[i] = 0;
+	_quadtree_add(parent, new);
 
 	if (debug) {
 		printf("adding node %p to parent %p\n", new, parent);
@@ -216,73 +223,424 @@ struct quadtree *quadtree_add(struct quadtree *parent, struct quadtree *new,
 	return new;
 }
 
+static double max_by_dir(double a, double b, int direction)
+{
+	switch (direction) {
+	case 0: /* up */
+	case 3: /* left */
+		return MIN(a, b);
+	case 1: /* right */
+	case 2: /* down */
+		return MAX(a, b);
+	}
+
+	trap();
+	return 0;
+}
+
+static double maxv_by_dir(struct quadtree *a, int direction)
+{
+	switch (direction) {
+	case 0: /* up */
+	case 2: /* down */
+		return a->pos.y;
+	case 1: /* right */
+	case 3: /* left */
+		return a->pos.x;
+	}
+
+	trap();
+	return 0;
+}
+
+static double quadtree_get_max_dimension(struct quadtree *node, int direction)
+{
+	struct quadtree *ch1 = NULL, *ch2 = NULL;
+	double a, b;
+
+	switch (direction) {
+	case 0: /* up */
+		ch1 = node->child[0];
+		ch2 = node->child[1];
+		break;
+	case 1: /* right */
+		ch1 = node->child[1];
+		ch2 = node->child[3];
+		break;
+	case 2: /* down */
+		ch1 = node->child[2];
+		ch2 = node->child[3];
+		break;
+	case 3: /* left */
+		ch1 = node->child[0];
+		ch2 = node->child[2];
+		break;
+	default:
+		trap();
+	}
+
+	if (ch1 && ch2) {
+		a = quadtree_get_max_dimension(ch1, direction);
+		b = quadtree_get_max_dimension(ch2, direction);
+		return max_by_dir(a, b, direction);
+	}
+
+	if (ch1) {
+		a = quadtree_get_max_dimension(ch1, direction);
+		b = maxv_by_dir(ch1, direction);
+		return max_by_dir(a, b, direction);
+	}
+	if (ch2) {
+		a = quadtree_get_max_dimension(ch2, direction);
+		b = maxv_by_dir(ch2, direction);
+		return max_by_dir(a, b, direction);
+	}
+
+	return maxv_by_dir(node, direction);
+}
+
 /*
- * _qt_optimal_del - Delete nodes from a detached subtree in optimal order
- *
- * All nodes are moved under the new parent node
+ * Stores the lower right and upper left corner coordinates to the
+ * corner vectors
  */
-static void _qt_optimal_del(struct quadtree *parent, struct quadtree *node,
-		       struct quadtree_ops *ops, int parents)
+static void quadtree_get_tree_dimensions(struct quadtree *node,
+					struct vector *corner)
 {
-	struct quadtree *child[4] = {0};
-	int i, children = 0, max_c, max_i;
+	corner[0].y = quadtree_get_max_dimension(node, 2);
+	corner[0].x = quadtree_get_max_dimension(node, 1);
+	corner[1].y = quadtree_get_max_dimension(node, 0);
+	corner[1].x = quadtree_get_max_dimension(node, 3);
+
+	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);
+	}
+
+	if ((corner[0].x < corner[1].x) ||
+	    (corner[0].y < corner[1].y))
+		trap();
+}
+
+static int is_within(struct vector *pos, struct vector *corner)
+{
+	if ((pos->x >= corner[1].x) && (pos->x <= corner[0].x) &&
+	    (pos->y >= corner[1].y) && (pos->y <= corner[0].y))
+		return 1;
+	return 0;
+}
+
+static int quadrants_to_search(struct quadtree *node, struct vector *corner)
+{
+	int direction = 0, i;
+	int up = 0, left = 0, right = 0, down = 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 (left || up)
+		direction |= QUADTREE_UPLEFT;
+	if (right || up)
+		direction |= QUADTREE_UPRIGHT;
+	if (left || down)
+		direction |= QUADTREE_DOWNLEFT;
+	if (right || down)
+		direction |= QUADTREE_DOWNRIGHT;
+
+	return direction;
+}
+
+static struct quadtree *quadtree_find_nearest(struct quadtree *tree,
+					struct vector *pos,
+					struct vector *corner)
+{
+	struct vector tmp;
+	struct quadtree *nearest, *node;
+	double distance, dist;
+	int ret, 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);
+		nearest = tree;
+	} else {
+		nearest = NULL;
+	}
+
+	directions = quadrants_to_search(tree, corner);
 
 	for (i = 0; i < 4; i++) {
-		if (!node->child[i]) {
-			child[i] = 0;
+		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);
+
+		if (!nearest || dist < distance) {
+			nearest = node;
+			distance = dist;
+
 		}
+	}
 
-		child[children] = node->child[i];
-		node->child[i] = NULL;
-		children++;
+	if (nearest && !is_within(&nearest->pos, corner)) {
+		if (debug)
+			printf("Node %p (%f %f) is not within "
+				"search window (%f %f) (%f %f)\n",
+				nearest, nearest->pos.x, nearest->pos.y,
+				corner[0].x, corner[0].y,
+				corner[1].x, corner[1].y);
+		return NULL;
 	}
 
+	return nearest;
+}
+
+static struct quadtree *quadtree_find_nearest_noparent(struct quadtree *tree,
+						struct vector *pos,
+						struct vector *corner)
+{
+	struct quadtree *nearest;
+	struct vector sub;
+	struct quadtree *node;
+	double dist = 0, dist2;
+	int i;
+
+	nearest = quadtree_find_nearest(tree, pos, corner);
+
+	if (nearest != tree)
+		goto out;
+
+	if (debug)
+		printf("Avoiding parent or NULL node %p\n", nearest);
+
 	/*
-	 * Try to remove the nodes in such order that the node which
-	 * has child number one quarter of the parent's child numer
-	 * gets deleted first.
+	 * oops, we don't want to pick the parent node, let's choose
+	 * another one
 	 */
+	nearest = NULL;
+	for (i = 0; i < 4; i++) {
+		if (!tree->child[i])
+			continue;
 
-	if (node->children <= parents / 4) {
-		if (debug) {
-			printf("%d: Moving node %p away from parent %p\n",
-			       __LINE__, node, parent);
-			fflush(stdout);
+		if (!nearest) {
+			nearest = quadtree_find_nearest(tree->child[i], pos,
+							corner);
+
+			if (nearest == NULL)
+				continue;
+
+			vector_sub(pos, &nearest->pos, &sub);
+			dist = vector_abs(&sub);
+			continue;
+		}
+		node = quadtree_find_nearest(tree->child[i],
+					pos, corner);
+
+		if (node == NULL)
+			continue;
+
+		vector_sub(pos, &node->pos, &sub);
+		dist2 = vector_abs(&sub);
+
+		if (dist2 < dist) {
+			nearest = node;
+			dist = dist2;
 		}
-		validate_tree(parent);
-		quadtree_add(parent, node, ops);
-		node = NULL;
-		parents /= 4;
 	}
 
-	while(children) {
-		max_c = -1;
-		max_i = -1;
-		for (i = 0; i < 4; i++) {
-			if (!child[i])
-				continue;
+out:
+	return nearest;
+}
+
+static void get_middle_point(struct vector *corner, struct vector *middle)
+{
+	middle->x = (corner[0].x + corner[1].x) / (double)2;
+	middle->y = (corner[0].y + corner[1].y) / (double)2;
+}
+
+static int quadtree_split_by_node(struct quadtree *node,
+				struct vector *corner, int dir)
+{
+	if (!is_within(&node->pos, corner)) {
+		if (debug)
+			printf("Not within search rectangle\n");
+		return 0;
+	}
+
+	switch (dir) {
+	case QUADTREE_UPLEFT:
+		corner[0] = node->pos;
+		break;
+	case QUADTREE_UPRIGHT:
+		corner[0].y = node->pos.y;
+		corner[1].x = node->pos.x;
+		break;
+	case QUADTREE_DOWNRIGHT:
+		corner[1] = node->pos;
+		break;
+	case QUADTREE_DOWNLEFT:
+		corner[0].x = node->pos.x;
+		corner[1].y = node->pos.y;
+		break;
+	default:
+		trap();
+	}
+
+	if (debug)
+		printf("Search rectangle is now (%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);
+
+	if ((corner[0].x < corner[1].x) ||
+	    (corner[0].y < corner[1].y))
+		trap();
 
-			if (max_c < child[i]->children) {
-				max_c = child[i]->children;
-				max_i = i;
+	return 1;
+}
+
+/*
+ * Quickly detach a node from a tree. Move all child nodes under
+ * @parent node.
+ */
+static int _quadtree_del(struct quadtree *node, struct quadtree *parent)
+{
+	struct quadtree *n;
+	int i;
+
+	/* Detach from the tree */
+	if (node->parent)
+		for (i = 0; i < 4; i++) {
+			if (node->parent->child[i] == node) {
+				node->parent->child[i] = 0;
+				break;
 			}
 		}
 
-		_qt_optimal_del(parent, child[max_i], ops, parents / 4);
-		child[max_i] = 0;
-		children--;
+	if (i == 4)
+		trap();
+
+
+	for (i = 0; i < 4; i++) {
+		if (!node->child[i])
+			continue;
+
+		n = node->child[i];
+		_quadtree_del(n, parent);
+		_quadtree_add(parent, n);
 	}
 
-	if (node) {
-		validate_tree(parent);
-		if (debug) {
-			printf("%d: Moving node %p away from parent %p\n",
-			       __LINE__, node, parent);
-			fflush(stdout);
-		}
-		quadtree_add(parent, node, ops);
+	return 0;
+}
+
+/**
+ * Move everything under @tree node to @parent node, everything
+ * else except the @tree node itself
+ */
+static int optimally_move_tree(struct quadtree *tree, struct quadtree *parent,
+			struct vector *corner_orig,
+			struct quadtree_ops *ops)
+{
+	struct vector corner[2], mid;
+	struct quadtree *t, *tmp;
+	int moved = 0;
+
+	get_middle_point(corner_orig, &mid);
+	t = quadtree_find_nearest_noparent(tree, &mid, corner_orig);
+
+	if (!t) {
+		if (debug)
+			printf("Cannot find nearest node\n");
+
+		goto out;
 	}
+
+	/*
+	 * Now we have the t node which contains the object of the
+	 * spatial middle coordinates of the tree.
+	 */
+
+	if (debug) {
+		printf("Relocating node %p (%f %f) under parent %p\n", t,
+			t->pos.x, t->pos.y, parent);
+		printf("There are %ld child nodes left\n", tree->children);
+		fflush(stdout);
+	}
+	_quadtree_del(t, tree);
+	quadtree_add(parent, t, ops);
+
+	moved++;
+	tree->children--;
+	if (!tree->children)
+		goto out;
+
+	/*
+	 * Now split the search rectangle in quadtres and do the same
+	 * with all of the quarters.
+	 */
+
+	corner[0] = corner_orig[0];
+	corner[1] = corner_orig[1];
+	if (quadtree_split_by_node(t, corner, QUADTREE_UPLEFT))
+		moved += optimally_move_tree(tree, parent, corner, ops);
+
+	corner[0] = corner_orig[0];
+	corner[1] = corner_orig[1];
+	if (quadtree_split_by_node(t, corner, QUADTREE_UPRIGHT))
+		moved += optimally_move_tree(tree, parent, corner, ops);
+
+	corner[0] = corner_orig[0];
+	corner[1] = corner_orig[1];
+	if (quadtree_split_by_node(t, corner, QUADTREE_DOWNLEFT))
+		moved += optimally_move_tree(tree, parent, corner, ops);
+
+	corner[0] = corner_orig[0];
+	corner[1] = corner_orig[1];
+	if (quadtree_split_by_node(t, corner, QUADTREE_DOWNRIGHT))
+		moved += optimally_move_tree(tree, parent, corner, ops);
+
+	get_middle_point(corner_orig, &mid);
+	tmp = quadtree_find_nearest(tree, &mid, corner_orig);
+	if (tmp && tmp != tree)
+		trap();
+
+out:
+	if (debug)
+		printf("Now moved %d nodes, %ld left\n", moved, tree->children);
+
+	return moved;
 }
 
 /*
@@ -295,74 +653,96 @@ static void _qt_optimal_del(struct quadtree *parent, struct quadtree *node,
 struct quadtree *quadtree_del(struct quadtree *node,
 			      struct quadtree_ops *ops)
 {
-	struct quadtree *parent = 0;
-	int i;
+	struct quadtree *parent = NULL;
+	struct vector corner[2];
+	int i, children = node->children, moved;
+
+	validate_tree(node);
 
 	if (debug) {
 		printf("Deleting node %p under parent %p\n",
 			       node, node->parent);
+		printf("Relocating %ld children\n", node->children);
 		fflush(stdout);
 	}
-	/*
-	 * We are deleting the root node. This means we have to select
-	 * a new root node and reconstruct the entire tree under it
-	 * again.
-	 */
+
 	if (!node->parent) {
+		/*
+		 * We are deleting the root node. This means we have
+		 * to select a new root node and reconstruct the
+		 * entire tree under it again.
+		 */
+		if (debug) {
+			printf("Deleting root node\n");
+			fflush(stdout);
+		}
 		for (i = 0; i < 4; i++) {
 			if (!node->child[i])
 				continue;
 
-			if (!parent) {
-				parent = node->child[i];
-				parent->parent = 0;
-				continue;
-			}
-			_qt_optimal_del(parent, node->child[i], ops,
-					node->children);
-			node->child[i] = 0;
+			parent = node->child[i];
+			_quadtree_del(parent, node);
+			parent->parent = 0;
+			quadtree_recalculate_parent_stats(parent, ops);
+			break;
 		}
+	} else {
+		/*
+		 * The node has a parent. Detach the node from it and
+		 * relocate the children.
+		 */
 
-		return parent;
-	}
+		for (i = 0; i < 4; i++) {
+			if (node->parent->child[i] == node) {
+				node->parent->child[i] = 0;
+				break;
+			}
+		}
+		if (i == 4)
+			trap();
 
-	/*
-	 * We are not deleting the parent. Detach the node from the
-	 * parent abd relocate the children. The node will be then
-	 * detached from the tree.
-	 */
+		parent = node->parent;
 
-	for (i = 0; i < 4; i++) {
-		if (node->parent->child[i] == node) {
-			node->parent->child[i] = 0;
-			break;
-		}
+		/*
+		 * The sub branch is now detached from the main
+		 * tree. Fix the stats.
+		 */
+		quadtree_recalculate_parent_stats(node->parent, ops);
 	}
-	if (i == 4)
-		trap();
 
-	parent = node->parent;
+	validate_tree(parent);
 
+	if (!node->children)
+		goto out;
 	/*
-	 * The sub branch is now detached from the main tree. Fix the
-	 * stats.
+	 * Now we are ready to prepare for relocating the nodes under
+	 * the parent.
 	 */
-	quadtree_recalculate_parent_stats(node->parent, ops);
-
-	parent = quadtree_find_parent(parent);
 
-	if (node->children) {
-		for (i = 0; i < 4; i++) {
-			if (!node->child[i])
-				continue;
+	quadtree_get_tree_dimensions(node, corner);
+	moved = optimally_move_tree(node, parent, corner, ops);
 
-			_qt_optimal_del(parent, node->child[i], ops,
-					node->children);
+	if (debug) {
+		if (moved != children) {
+			printf("Got %d children but %d were moved\n",
+				children, moved);
+			printf("nearest children left:\n");
+			for (i = 0 ; i < 4; i++)
+				if (node->child[i])
+					printf("   node %d %p, (%f %f)\n",
+						i, node->child[i],
+						node->child[i]->pos.x,
+						node->child[i]->pos.y);
+			fflush(stdout);
+			trap();
 		}
+		printf("Delete done, returning parent %p\n", parent);
+		fflush(stdout);
 	}
 
+out:
 	validate_tree(parent);
-	return parent;
+	return quadtree_find_parent(parent);
 }
 
 
-- 
2.44.0