validate_subtree(quadtree_find_parent(node));
}
-static int quadtree_compare(struct quadtree *a, struct quadtree *b)
+static int quadtree_compare_coord(struct vector *a, struct vector *b)
{
int up, left;
- up = b->pos.y < a->pos.y;
- left = b->pos.x < a->pos.x;
+ up = b->y < a->y;
+ left = b->x < a->x;
if (up && left)
return 0;
return 3;
}
+static int quadtree_compare(struct quadtree *a, struct quadtree *b)
+{
+ return quadtree_compare_coord(&a->pos, &b->pos);
+}
+
+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;
+}
+
/**
* quadtree_add - add a node to a quadtree
* @parent: parent 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);
return new;
}
-/*
- * _qt_optimal_del - Delete nodes from a detached subtree in optimal order
- *
- * All nodes are moved under the new parent node
- */
-static void _qt_optimal_del(struct quadtree *parent, struct quadtree *node,
- struct quadtree_ops *ops, int parents)
+static double max_by_dir(double a, double b, int direction)
{
- struct quadtree *child[4] = {0};
- int i, children = 0, max_c, max_i;
+ switch (direction) {
+ case 0: /* up */
+ case 1: /* right */
+ return MAX(a, b);
+ case 2: /* down */
+ case 3: /* left */
+ return MIN(a, b);
+ }
- for (i = 0; i < 4; i++) {
- if (!node->child[i]) {
- child[i] = 0;
- continue;
- }
+ trap();
+ return 0;
+}
- child[children] = node->child[i];
- node->child[i] = NULL;
- children++;
+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;
}
- /*
- * 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.
- */
+ trap();
+ return 0;
+}
- if (node->children <= parents / 4) {
- if (debug) {
- printf("%d: Moving node %p away from parent %p\n",
- __LINE__, node, parent);
- fflush(stdout);
- }
- validate_tree(parent);
- quadtree_add(parent, node, ops);
- node = NULL;
- parents /= 4;
+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: /* left */
+ ch1 = node->child[1];
+ ch2 = node->child[3];
+ break;
+ case 2: /* down */
+ ch1 = node->child[2];
+ ch2 = node->child[3];
+ break;
+ case 3: /* right */
+ ch1 = node->child[0];
+ ch2 = node->child[2];
+ break;
+ default:
+ trap();
}
- while(children) {
- max_c = -1;
- max_i = -1;
- for (i = 0; i < 4; i++) {
- if (!child[i])
- continue;
+ 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 (max_c < child[i]->children) {
- max_c = child[i]->children;
- max_i = i;
- }
- }
+ if (ch1)
+ return maxv_by_dir(ch1, direction);
+ if (ch2)
+ return maxv_by_dir(ch2, direction);
- _qt_optimal_del(parent, child[max_i], ops, parents / 4);
- child[max_i] = 0;
- children--;
- }
+ return maxv_by_dir(node, direction);
+}
- 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);
- }
+/*
+ * Stores the upper right and lower left corner coordinates to the
+ * corner vectors
+ */
+static void quadtree_get_tree_dimensions(struct quadtree *node,
+ struct vector *corner)
+{
+ corner[0].y = quadtree_get_max_dimension(node, 0);
+ corner[0].x = quadtree_get_max_dimension(node, 1);
+ corner[1].y = quadtree_get_max_dimension(node, 2);
+ corner[1].x = quadtree_get_max_dimension(node, 3);
}
-static int add_to_relocation_list(struct quadtree *node,
- struct list_head **head)
+static int is_within(struct vector *pos, struct vector *corner)
{
- int nodes = 0;
+ 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;
+}
- if (node->child[0])
- nodes += add_to_relocation_list(node->child[0], head);
- if (node->child[1])
- nodes += add_to_relocation_list(node->child[1], head);
+static struct quadtree *quadtree_find_nearest(struct quadtree *tree,
+ struct vector *pos,
+ struct vector *corner)
+{
+ int ret;
- if (debug) {
- printf("Adding node %p to relocation list %p\n",
- node, *head);
- fflush(stdout);
- }
+ if (!is_within(pos, corner))
+ return NULL;
- if (!*head) {
- *head = &node->list;
- INIT_LIST_HEAD(*head);
- }
+ while (1) {
+ ret = quadtree_compare_coord(&tree->pos, pos);
- list_add(&node->list, *head);
- nodes++;
+ if (tree->child[ret]) {
+ tree = tree->child[ret];
+ continue;
+ }
- if (node->child[2])
- nodes += add_to_relocation_list(node->child[2], head);
- if (node->child[3])
- nodes += add_to_relocation_list(node->child[3], head);
+ return tree;
+ }
+}
- return nodes;
+static void get_middle_point(struct vector *corner, struct vector *middle)
+{
+ middle->x = (corner[0].x + corner[1].x) / 2;
+ middle->y = (corner[0].y + corner[1].y) / 2;
}
-static void relocate_from_list(struct quadtree *start, struct quadtree *end,
- int nodes, struct quadtree *parent,
- struct quadtree_ops *ops)
+static void quadtree_split_by_node(struct quadtree *node,
+ struct vector *corner, int dir)
{
- struct quadtree *tmp = start, *node;
- int i, d = nodes / 2;
+ switch (dir) {
+ case QUADTREE_UPLEFT:
+ corner[0].x = node->pos.x;
+ corner[1].y = node->pos.y;
+ break;
+ case QUADTREE_UPRIGHT:
+ corner[1] = node->pos;
+ break;
+ case QUADTREE_DOWNRIGHT:
+ corner[0].y = node->pos.y;
+ corner[1].x = node->pos.x;
+ break;
+ case QUADTREE_DOWNLEFT:
+ corner[0] = node->pos;
+ break;
+ default:
+ trap();
+ }
+}
- for (; nodes > 0; nodes--) {
- if (debug) {
- printf("stepping %d, %d nodes left\n", d, nodes);
- fflush(stdout);
+/*
+ * 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 */
+ for (i = 0; i < 4; i++) {
+ if (node->parent->child[i] == node) {
+ node->parent->child[i] = 0;
+ break;
}
+ }
- for (i = 0; i < d; i++)
- tmp = list_to_tree(tmp->list.next);
+ if (i == 4)
+ trap();
- if (debug) {
- printf("Now moving node %p, next %p , prev %p\n",
- tmp,
- list_to_tree(tmp->list.prev),
- list_to_tree(tmp->list.next));
- fflush(stdout);
- }
- node = tmp;
- tmp = list_to_tree(tmp->list.next);
- list_del(&node->list);
- quadtree_add(parent, node, ops);
+ for (i = 0; i < 4; i++) {
+ if (!node->child[i])
+ continue;
- d /= 2;
- if (d == 0)
- d = nodes / 2;
+ n = node->child[i];
+ _quadtree_del(n, parent);
+ _quadtree_add(parent, n);
}
- return;
+ return 0;
+}
- for (i = 0; i < nodes / 2; i++)
- tmp = list_to_tree(tmp->list.next);
+/**
+ * Move everything under @tree node under @parent node, everything
+ * else except the @tree node itself
+ */
+static void 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;
+ int ret;
- node = tmp;
+ get_middle_point(corner_orig, &mid);
+ t = quadtree_find_nearest(tree, &mid, corner_orig);
- tmp = list_to_tree(node->list.prev);
+ if (!t)
+ return;
- if (debug) {
- printf("%d nodes to move, now at %d\n", nodes, i);
- printf("Moving node %p\n", node);
- fflush(stdout);
+ /* oops, we don't want to remove this node, let's pick another
+ * one
+ */
+ if (t == tree) {
+ ret = quadtree_compare_coord(&tree->pos, &mid);
+
+ if (tree->child[ret]) {
+ t = quadtree_find_nearest(tree->child[ret], &mid,
+ corner_orig);
+ } else {
+ /*
+ * No suitable match from the right
+ * direction. Try to find a second nearest
+ * node.
+ */
+ struct vector sub;
+ struct quadtree *node;
+ double dist = 0, dist2;
+ int i;
+
+ t = NULL;
+ for (i = 0; i < 4; i++) {
+ if (!tree->child[i])
+ continue;
+ if (!t) {
+ t = quadtree_find_nearest(
+ tree->child[i], &mid,
+ corner_orig);
+ vector_sub(&mid, &t->pos, &sub);
+ dist = vector_abs(&sub);
+ }
+ node = quadtree_find_nearest(tree->child[i],
+ &mid, corner_orig);
+ vector_sub(&mid, &node->pos, &sub);
+ dist2 = vector_abs(&sub);
+
+ if (dist2 < dist) {
+ t = node;
+ dist = dist2;
+ }
+ }
+ }
}
- list_del(&node->list);
- quadtree_add(parent, node, ops);
- node = list_to_tree(tmp->list.next);
- if (nodes / 2 - 1 > 0)
- relocate_from_list(start, tmp, nodes / 2 - 1, parent, ops);
+ /*
+ * Now we have the t node which contains the object of the
+ * spatial middle coordinates of the tree.
+ */
+
+ _quadtree_del(t, tree);
+ quadtree_add(parent, t, ops);
+
+ /*
+ * Now split the search rectangle in quadtres and do the same
+ * with all of the quarters.
+ */
- if (nodes > 2)
- relocate_from_list(node, end, nodes - nodes / 2 - 1, parent,
- ops);
+ corner[0] = corner_orig[0];
+ corner[1] = corner_orig[1];
+ quadtree_split_by_node(t, corner, QUADTREE_UPLEFT);
+ optimally_move_tree(tree, parent, corner, ops);
+
+ corner[0] = corner_orig[0];
+ corner[1] = corner_orig[1];
+ quadtree_split_by_node(t, corner, QUADTREE_UPRIGHT);
+ optimally_move_tree(tree, parent, corner, ops);
+
+ corner[0] = corner_orig[0];
+ corner[1] = corner_orig[1];
+ quadtree_split_by_node(t, corner, QUADTREE_DOWNLEFT);
+ optimally_move_tree(tree, parent, corner, ops);
+
+ corner[0] = corner_orig[0];
+ corner[1] = corner_orig[1];
+ quadtree_split_by_node(t, corner, QUADTREE_DOWNRIGHT);
+ optimally_move_tree(tree, parent, corner, ops);
}
/*
struct quadtree *quadtree_del(struct quadtree *node,
struct quadtree_ops *ops)
{
- struct quadtree *parent = 0, *start, *end;
- struct list_head *head = NULL;
- int i, nodes = 0;
+ struct quadtree *parent = NULL;
+ struct vector corner[2];
+ int i;
validate_tree(node);
printf("Relocating %ld children\n", node->children);
fflush(stdout);
}
+
+ /* Get the outer limits of the area belongin to the node */
+ quadtree_get_tree_dimensions(node, corner);
+
/*
* We are deleting the root node. This means we have to select
* a new root node and reconstruct the entire tree under it
if (!node->child[i])
continue;
- if (!parent) {
- parent = node->child[i];
- parent->parent = 0;
- continue;
- }
-
- nodes += add_to_relocation_list(node->child[i], &head);
+ parent = node->child[i];
+ parent->parent = 0;
}
+ } else {
+ /*
+ * 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.
+ */
- start = list_to_tree(head);
- end = list_to_tree(head->prev);
-
- relocate_from_list(start, end, nodes, parent, ops);
+ for (i = 0; i < 4; i++) {
+ if (node->parent->child[i] == node) {
+ node->parent->child[i] = 0;
+ break;
+ }
+ }
+ if (i == 4)
+ trap();
- return parent;
- }
+ parent = node->parent;
- /*
- * 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.
- */
+ /*
+ * The sub branch is now detached from the main
+ * tree. Fix the stats.
+ */
+ quadtree_recalculate_parent_stats(node->parent, ops);
- for (i = 0; i < 4; i++) {
- if (node->parent->child[i] == node) {
- node->parent->child[i] = 0;
- break;
- }
+ parent = quadtree_find_parent(parent);
}
- if (i == 4)
- trap();
-
- parent = node->parent;
/*
- * 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;
-
- nodes += add_to_relocation_list(node->child[i], &head);
- }
-
- start = list_to_tree(head);
- end = list_to_tree(head->prev);
-
- relocate_from_list(start, end, nodes, parent, ops);
- }
+ quadtree_get_tree_dimensions(node, corner);
+ optimally_move_tree(node, parent, corner, ops);
validate_tree(parent);
return parent;