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;
}
/**
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);
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;
}
/*
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);
}
projects / sdl-planets / commitdiff