6 long int quadtree_rebalance_events;
7 long int quadtree_rebalanced_nodes;
15 static void _trap(int line)
18 printf("Trapped from line %d, use debugger to get backtrace\n",
25 #define trap() _trap(__LINE__)
27 static int quadtree_compare_coord(const struct vector *a,
28 const struct vector *b)
44 static int quadtree_compare(const struct quadtree *a, const struct quadtree *b)
46 return quadtree_compare_coord(&a->pos, &b->pos);
49 static void validate_subtree(const struct quadtree *node)
56 printf("Attempted to validate a null pointer\n");
61 for (i = 0; i < 4; i++) {
65 if (node->child[i]->parent != node) {
66 printf("%s:%d Fatal! Tree inconsistency detected at "
67 "child %d %p in node %p, incorrent parent %p\n",
68 __func__, __LINE__, i, node->child[i], node,
69 node->child[i]->parent);
75 if (node->child[i] == node->parent) {
76 printf("%s:%d Fatal! Tree loop detected "
77 "at child %d in node %p\n",
78 __func__, __LINE__, i, node);
84 dir = quadtree_compare(node, node->child[i]);
87 printf("%s:%d Fatal! Spatial inconsistency detected "
88 "at child %d in node %p\n"
89 "parent: (%f %f), child (%f %f), "
91 __func__, __LINE__, i, node,
92 node->pos.x, node->pos.y,
93 node->child[i]->pos.x, node->child[i]->pos.y,
99 children += node->child[i]->children + 1;
101 validate_subtree(node->child[i]);
104 if (node->depth < 0) {
105 printf("%s:%d Tree statistics inconsistency detected! "
106 "Negative depth: %ld\n",
107 __func__, __LINE__, node->depth);
110 if (node->children != children) {
111 printf("%s:%d Tree statistics inconsistency detected! "
112 "child count mismatch. Expected %ld, got %ld\n",
113 __func__, __LINE__, children, node->children);
118 for (i = 0; i < 4 && node->children; i++) {
122 if (node->depth == node->child[i]->depth + 1)
125 if (node->child[i]->depth > node->depth) {
126 printf("%s:%d Tree statistics inconsistency detected! "
127 "child depth mismatch %ld > %ld\n",
128 __func__, __LINE__, node->child[i]->depth,
134 printf("%s:%d Tree statistics inconsistency detected! "
135 "child depth mismatch.",
142 static void validate_tree(const struct quadtree *node)
150 return validate_subtree(node);
152 for (i = 0; i < 4; i++)
153 if (node->parent->child[i] == node)
157 printf("%s:%d Tree inconsistency detected! "
158 "Wrong parent %p for node %p\n",
159 __func__, __LINE__, node->parent, node);
164 validate_tree(node->parent);
167 void _quadtree_validate_tree(const struct quadtree *node)
172 static struct quadtree *_quadtree_add(struct quadtree *parent,
173 struct quadtree *new)
177 ret = quadtree_compare(parent, new);
179 if (ret < 0 || ret >= 4) {
180 printf("Invalid comparison result of %d\n", ret);
184 if (parent->child[ret])
185 return _quadtree_add(parent->child[ret], new);
187 parent->child[ret] = new;
188 new->parent = parent;
189 for (i = 0; i < 4; i++)
195 static void _recalculate_node_area_stats(struct quadtree *node)
197 /* The space covered by the parent node's corner
198 * points needs be as wide as its widest child node's
201 #define CHILD_CORNER_SAFE(node, ch_idx, cor_idx, axis) \
202 ((node)->child[ch_idx] ? \
203 (node)->child[ch_idx]->corner[cor_idx].axis : \
206 node->corner[0].x = MIN(CHILD_CORNER_SAFE(node, 0, 0, x),
207 CHILD_CORNER_SAFE(node, 2, 0, x));
208 node->corner[0].y = MIN(CHILD_CORNER_SAFE(node, 0, 0, y),
209 CHILD_CORNER_SAFE(node, 1, 0, y));
210 node->corner[1].x = MAX(CHILD_CORNER_SAFE(node, 1, 1, x),
211 CHILD_CORNER_SAFE(node, 3, 1, x));
212 node->corner[1].y = MAX(CHILD_CORNER_SAFE(node, 2, 1, y),
213 CHILD_CORNER_SAFE(node, 3, 1, y));
214 #undef CHILD_CORNER_SAFE
217 static void recalculate_parent_area_stats(struct quadtree *node)
219 struct vector old_corner[2];
222 memcpy(&old_corner, &node->corner, sizeof(old_corner));
224 _recalculate_node_area_stats(node);
227 * Stop propagating the changes up in the tree if
228 * nothing has changed
230 if (memcmp(&old_corner, &node->corner, sizeof(old_corner)))
238 * Recursively walk through the tree and propagate changes made to the
239 * given node up until the highest parent.
241 static void quadtree_recalculate_parent_stats(struct quadtree *node,
242 struct quadtree_ops *ops)
250 for (i = 0; i < 4; i++) {
254 node->depth = MAX(node->depth,
255 node->child[i]->depth + 1);
256 node->children += node->child[i]->children + 1;
259 _recalculate_node_area_stats(node);
261 if (ops->recalculate_stats)
262 ops->recalculate_stats(node);
269 * quadtree_add - add a node to a quadtree
270 * @parent: parent node
271 * @new: the new node to be added
273 * Add a node to a quadtree. The tree is kept in order, the new node
274 * is placed in the end of appropriate branch.
276 * The case of nodes sharing identical coordinates is not taken into
280 struct quadtree *quadtree_add(struct quadtree *parent, struct quadtree *new,
281 struct quadtree_ops *ops)
286 validate_tree(parent);
288 _quadtree_add(parent, new);
291 printf("adding node %p to parent %p\n", new, parent);
295 quadtree_recalculate_parent_stats(new, ops);
302 static int is_within(struct vector *pos, struct vector *corner)
304 if ((pos->x >= corner[1].x) && (pos->x <= corner[0].x) &&
305 (pos->y >= corner[1].y) && (pos->y <= corner[0].y))
310 static int rectangle_and_circle_overlap(struct vector *corner,
311 struct vector *pos, double radius)
313 int vertically_overlapping = 0, horizontally_overlapping = 0;
315 if ((pos->x < corner[1].x && pos->x + radius > corner[0].x) ||
316 (pos->x > corner[0].x && pos->x - radius < corner[1].x))
317 horizontally_overlapping = 1;
319 if ((pos->y < corner[1].y && pos->y + radius > corner[0].y) ||
320 (pos->y > corner[0].y && pos->y - radius < corner[1].y))
321 vertically_overlapping = 1;
323 return horizontally_overlapping && vertically_overlapping;
326 static int quadrants_to_search(struct quadtree *node, struct vector *pos,
334 if (node->child[0] &&
335 rectangle_and_circle_overlap(node->child[0]->corner, pos, dist))
336 direction |= QUADTREE_UPLEFT;
338 if (node->child[1] &&
339 rectangle_and_circle_overlap(node->child[1]->corner, pos, dist))
340 direction |= QUADTREE_UPRIGHT;
342 if (node->child[2] &&
343 rectangle_and_circle_overlap(node->child[2]->corner, pos, dist))
344 direction |= QUADTREE_DOWNLEFT;
346 if (node->child[3] &&
347 rectangle_and_circle_overlap(node->child[3]->corner, pos, dist))
348 direction |= QUADTREE_DOWNRIGHT;
353 static struct quadtree *quadtree_find_nearest(struct quadtree *tree,
355 struct vector *corner,
356 struct quadtree *nearest,
360 struct quadtree *node;
361 double distance = 0, dist;
367 vector_sub(&nearest->pos, pos, &tmp);
368 distance = vector_abs_squared(&tmp);
370 if (tree != nearest) {
371 vector_sub(&tree->pos, pos, &tmp);
372 dist = vector_abs_squared(&tmp);
373 if (dist < distance) {
379 if (!is_within(&nearest->pos, corner)) {
381 printf("Discarding nearest %p "
382 "as outside of search window\n", nearest);
387 directions = quadrants_to_search(tree, pos, distance);
389 printf("%d: Directions: %x\n", depth, directions);
390 for (i = 0; i < 4; i++) {
394 if (!(directions & (1 << i)))
398 node = quadtree_find_nearest(tree->child[i], pos, corner,
404 if (node != nearest) {
405 vector_sub(&node->pos, pos, &tmp);
406 dist = vector_abs_squared(&tmp);
407 if (dist < distance || !nearest) {
412 * Update the directions where to search for
413 * since those may not be valid any more
415 directions = quadrants_to_search(tree, pos, distance);
420 if (nearest && !is_within(&nearest->pos, corner)) {
422 printf("%d: Node %p (%f %f) is not within "
423 "search window (%f %f) (%f %f)\n", depth,
424 nearest, nearest->pos.x, nearest->pos.y,
425 corner[0].x, corner[0].y,
426 corner[1].x, corner[1].y);
430 printf("%d: Node %p (%f %f) is nearest node, dist %f\n",
432 nearest ? nearest->pos.x : -1,
433 nearest ? nearest->pos.y : -1,
434 nearest ? distance : -1);
438 static struct quadtree *quadtree_find_nearest_noparent(struct quadtree *tree,
440 struct vector *corner)
442 struct quadtree *nearest = NULL;
444 struct quadtree *node;
445 double dist = 0, dist2;
448 nearest = quadtree_find_nearest(tree, pos, corner, nearest, 0);
454 printf("Avoiding parent or NULL node %p\n", nearest);
457 * oops, we don't want to pick the parent node, let's choose
461 for (i = 0; i < 4; i++) {
466 nearest = quadtree_find_nearest(tree->child[i], pos,
472 vector_sub(pos, &nearest->pos, &sub);
473 dist = vector_abs_squared(&sub);
476 node = quadtree_find_nearest(tree->child[i], pos, corner,
482 vector_sub(pos, &node->pos, &sub);
483 dist2 = vector_abs_squared(&sub);
495 static void get_middle_point(struct vector *corner, struct vector *middle)
497 middle->x = (corner[0].x + corner[1].x) / (double)2;
498 middle->y = (corner[0].y + corner[1].y) / (double)2;
501 static int quadtree_split_by_node(struct quadtree *node,
502 struct vector *corner, int dir)
504 if (!is_within(&node->pos, corner)) {
506 printf("Not within search rectangle\n");
511 printf("Search rectangle was before (%f %f) (%f %f), (%f %f)\n",
512 corner[0].x, corner[0].y,
513 corner[1].x, corner[1].y,
514 node->pos.x, node->pos.y);
517 case QUADTREE_UPLEFT:
518 corner[0] = node->pos;
520 case QUADTREE_UPRIGHT:
521 corner[0].y = node->pos.y;
522 corner[1].x = node->pos.x;
524 case QUADTREE_DOWNRIGHT:
525 corner[1] = node->pos;
527 case QUADTREE_DOWNLEFT:
528 corner[0].x = node->pos.x;
529 corner[1].y = node->pos.y;
536 printf("Search rectangle is now (%f %f) (%f %f), (%f %f)\n",
537 corner[0].x, corner[0].y,
538 corner[1].x, corner[1].y,
539 node->pos.x, node->pos.y);
541 if ((corner[0].x < corner[1].x) ||
542 (corner[0].y < corner[1].y))
549 * Quickly detach a node from a tree. Move all child nodes under
552 static int _quadtree_del(struct quadtree *node, struct quadtree *parent)
557 /* Detach from the tree */
559 for (i = 0; i < 4; i++) {
560 if (node->parent->child[i] == node) {
561 node->parent->child[i] = 0;
569 for (i = 0; i < 4; i++) {
574 _quadtree_del(n, parent);
575 _quadtree_add(parent, n);
576 recalculate_parent_area_stats(n);
583 * Move everything under @tree node to @parent node, everything
584 * else except the @tree node itself
586 static int optimally_move_tree(struct quadtree *tree, struct quadtree *parent,
587 struct vector *corner_orig,
588 struct quadtree_ops *ops)
590 struct vector corner[2], mid;
594 get_middle_point(corner_orig, &mid);
595 t = quadtree_find_nearest_noparent(tree, &mid, corner_orig);
599 printf("Cannot find nearest node\n");
605 * Now we have the t node which contains the object of the
606 * spatial middle coordinates of the tree.
610 printf("Relocating node %p (%f %f) under parent %p\n", t,
611 t->pos.x, t->pos.y, parent);
612 printf("There are %ld child nodes left\n", tree->children);
615 _quadtree_del(t, tree);
616 quadtree_add(parent, t, ops);
624 * Now split the search rectangle in quadtres and do the same
625 * with all of the quarters.
628 corner[0] = corner_orig[0];
629 corner[1] = corner_orig[1];
630 if (quadtree_split_by_node(t, corner, QUADTREE_UPLEFT))
631 moved += optimally_move_tree(tree, parent, corner, ops);
633 corner[0] = corner_orig[0];
634 corner[1] = corner_orig[1];
635 if (quadtree_split_by_node(t, corner, QUADTREE_UPRIGHT))
636 moved += optimally_move_tree(tree, parent, corner, ops);
638 corner[0] = corner_orig[0];
639 corner[1] = corner_orig[1];
640 if (quadtree_split_by_node(t, corner, QUADTREE_DOWNLEFT))
641 moved += optimally_move_tree(tree, parent, corner, ops);
643 corner[0] = corner_orig[0];
644 corner[1] = corner_orig[1];
645 if (quadtree_split_by_node(t, corner, QUADTREE_DOWNRIGHT))
646 moved += optimally_move_tree(tree, parent, corner, ops);
650 printf("Now moved %d nodes, %ld left\n", moved, tree->children);
656 * quadtree_del - Detach a node from the tree
658 * Return value: The new root node of the tree. If we are detaching
659 * anything but the root node of the entire tree, the returned root
660 * value will be the original root of the tree.
662 struct quadtree *quadtree_del(struct quadtree *node,
663 struct quadtree_ops *ops)
665 struct quadtree *parent = NULL;
666 struct vector corner[2];
667 int i, children = node->children, moved;
672 printf("Deleting node %p under parent %p\n",
674 printf("Relocating %ld children\n", node->children);
680 * We are deleting the root node. This means we have
681 * to select a new root node and reconstruct the
682 * entire tree under it again.
685 printf("Deleting root node\n");
688 for (i = 0; i < 4; i++) {
692 parent = node->child[i];
693 _quadtree_del(parent, node);
695 quadtree_recalculate_parent_stats(parent, ops);
700 * The node has a parent. Detach the node from it and
701 * relocate the children.
704 for (i = 0; i < 4; i++) {
705 if (node->parent->child[i] == node) {
706 node->parent->child[i] = 0;
713 parent = node->parent;
717 * The sub branch is now detached from the main
718 * tree. Fix the stats.
720 quadtree_recalculate_parent_stats(parent, ops);
723 validate_tree(parent);
728 * Now we are ready to prepare for relocating the nodes under
732 corner[0] = node->corner[1];
733 corner[1] = node->corner[0];
735 printf("\nInitial Search rectangle (%f %f) (%f %f), (%f %f)\n",
736 corner[0].x, corner[0].y,
737 corner[1].x, corner[1].y,
738 node->pos.x, node->pos.y);
741 moved = optimally_move_tree(node, parent, corner, ops);
744 if (moved != children) {
745 printf("Got %d children but %d were moved\n",
747 printf("nearest children left:\n");
748 for (i = 0 ; i < 4; i++)
750 printf(" node %d %p, (%f %f)\n",
752 node->child[i]->pos.x,
753 node->child[i]->pos.y);
757 printf("Delete done, returning parent %p\n", parent);
762 validate_tree(parent);
763 return quadtree_find_parent(parent);
766 static int check_for_crossed_subnodes(struct quadtree *node,
767 struct quadtree *skip,
768 struct vector *limit, struct quadtree_ops *ops)
770 int direction = 0, i;
771 int up = 0, left = 0, right = 0, down = 0;
774 for (i = 0; i < 2; i++) {
775 if (limit[i].x < node->pos.x)
779 if (limit[i].y < node->pos.y)
786 direction |= QUADTREE_UPLEFT;
788 direction |= QUADTREE_UPRIGHT;
790 direction |= QUADTREE_DOWNLEFT;
792 direction |= QUADTREE_DOWNRIGHT;
793 if ((left && right) || (up && down))
794 direction |= QUADTREE_SELF;
798 * We need to skip the topmost node (the one that was actually
799 * moved), otherwise we would trigger rebalance every time
804 if (direction & QUADTREE_SELF) {
805 struct quadtree *parent;
808 printf("Rebalanced because crossed child %p: "
809 "node (%f %f) limits (%f %f) (%f %f)\n",
811 node->pos.x, node->pos.y,
812 limit[0].x, limit[0].y,
813 limit[1].x, limit[1].y);
815 parent = quadtree_del(node, ops);
816 quadtree_add(parent, node, ops);
820 if ((direction & QUADTREE_UPLEFT) && node->child[0])
821 cross += check_for_crossed_subnodes(node->child[0], skip,
827 if ((direction & QUADTREE_UPRIGHT) && node->child[1])
828 cross += check_for_crossed_subnodes(node->child[1], skip,
834 if ((direction & QUADTREE_DOWNLEFT) && node->child[2])
835 cross += check_for_crossed_subnodes(node->child[2], skip,
841 if ((direction & QUADTREE_DOWNRIGHT) && node->child[3])
842 cross += check_for_crossed_subnodes(node->child[3], skip,
848 struct quadtree *quadtree_move(struct quadtree *node, struct vector new_pos,
849 struct quadtree_ops *ops)
851 struct quadtree *parent, *tree_parent;
853 int parents = 0, all_parents = 0;
856 parent = node->parent;
858 parent = parent->parent;
863 /* Check if we have crossed any of the parents */
864 parent = node->parent;
867 if (node->pos.x < parent->pos.x && new_pos.x > parent->pos.x)
869 if (node->pos.x > parent->pos.x && new_pos.x < parent->pos.x)
871 if (node->pos.y < parent->pos.y && new_pos.y > parent->pos.y)
873 if (node->pos.y > parent->pos.y && new_pos.y < parent->pos.y)
877 parent = parent->parent;
882 printf("Rebalanced because crossed parent: node %p, "
884 "old (%f %f) new (%f %f) parent (%f %f)\n",
885 node, parents, all_parents, modify,
886 node->pos.x, node->pos.y,
887 new_pos.x, new_pos.y,
888 parent->pos.x, parent->pos.y);
890 * If the node has crossed the boundaries, remove it
891 * from the tree and add it again to it. It is then
892 * guaranteed to be in the correct position of the
896 quadtree_rebalanced_nodes += node->children;
897 tree_parent = quadtree_del(node, ops);
899 quadtree_add(tree_parent, node, ops);
900 quadtree_rebalance_events++;
906 if (node->children) {
908 * Now, search the subtree for any children that are
909 * located in wrong place and move them into correct
910 * place within the tree.
912 struct vector limit[2];
914 limit[0] = node->pos;
916 modify = check_for_crossed_subnodes(node, node, limit, ops);
919 /* Move the node into its new location */
923 tree_parent = quadtree_del(node, ops);
926 recalculate_parent_area_stats(node);
928 quadtree_add(tree_parent, node, ops);
930 return quadtree_find_parent(node);
934 static int _walk_tree(struct quadtree *head, const struct quadtree_iterator *it)
936 int direction, count = 0;
938 direction = it->direction(head, (struct quadtree_iterator *)it);
940 if ((direction & QUADTREE_UPLEFT) && head->child[0])
941 count += _walk_tree(head->child[0], it);
943 if ((direction & QUADTREE_UPRIGHT) && head->child[1])
944 count += _walk_tree(head->child[1], it);
946 if ((direction & QUADTREE_DOWNLEFT) && head->child[2])
947 count += _walk_tree(head->child[2], it);
949 if ((direction & QUADTREE_DOWNRIGHT) && head->child[3])
950 count += _walk_tree(head->child[3], it);
952 if ((direction & QUADTREE_SELF) && it->callback) {
953 it->callback(head, (struct quadtree_iterator *)it);
960 int walk_quadtree(const struct quadtree_iterator *it)
962 return _walk_tree(it->head, it);