12 static void _trap(int line)
15 printf("Trapped from line %d, use debugger to get backtrace\n",
22 #define trap() _trap(__LINE__)
24 static int quadtree_compare_coord(const struct vector *a,
25 const struct vector *b)
41 static int quadtree_compare(const struct quadtree *a, const struct quadtree *b)
43 return quadtree_compare_coord(&a->pos, &b->pos);
46 static void validate_subtree(const struct quadtree *node)
53 printf("Attempted to validate a null pointer\n");
58 for (i = 0; i < 4; i++) {
62 if (node->child[i]->parent != node) {
63 printf("%s:%d Fatal! Tree inconsistency detected at "
64 "child %d %p in node %p, incorrent parent %p\n",
65 __func__, __LINE__, i, node->child[i], node,
66 node->child[i]->parent);
72 if (node->child[i] == node->parent) {
73 printf("%s:%d Fatal! Tree loop detected "
74 "at child %d in node %p\n",
75 __func__, __LINE__, i, node);
81 dir = quadtree_compare(node, node->child[i]);
84 printf("%s:%d Fatal! Spatial inconsistency detected "
85 "at child %d in node %p\n"
86 "parent: (%f %f), child (%f %f), "
88 __func__, __LINE__, i, node,
89 node->pos.x, node->pos.y,
90 node->child[i]->pos.x, node->child[i]->pos.y,
96 children += node->child[i]->children + 1;
98 validate_subtree(node->child[i]);
101 if (node->depth < 0) {
102 printf("%s:%d Tree statistics inconsistency detected! "
103 "Negative depth: %ld\n",
104 __func__, __LINE__, node->depth);
107 if (node->children != children) {
108 printf("%s:%d Tree statistics inconsistency detected! "
109 "child count mismatch. Expected %ld, got %ld\n",
110 __func__, __LINE__, children, node->children);
115 for (i = 0; i < 4 && node->children; i++) {
119 if (node->depth == node->child[i]->depth + 1)
122 if (node->child[i]->depth > node->depth) {
123 printf("%s:%d Tree statistics inconsistency detected! "
124 "child depth mismatch %ld > %ld\n",
125 __func__, __LINE__, node->child[i]->depth,
131 printf("%s:%d Tree statistics inconsistency detected! "
132 "child depth mismatch.",
139 static void validate_tree(const struct quadtree *node)
147 return validate_subtree(node);
149 for (i = 0; i < 4; i++)
150 if (node->parent->child[i] == node)
154 printf("%s:%d Tree inconsistency detected! "
155 "Wrong parent %p for node %p\n",
156 __func__, __LINE__, node->parent, node);
161 validate_tree(node->parent);
164 void _quadtree_validate_tree(const struct quadtree *node)
169 static struct quadtree *_quadtree_add(struct quadtree *parent,
170 struct quadtree *new)
174 ret = quadtree_compare(parent, new);
176 if (ret < 0 || ret >= 4) {
177 printf("Invalid comparison result of %d\n", ret);
181 if (parent->child[ret])
182 return _quadtree_add(parent->child[ret], new);
184 parent->child[ret] = new;
185 new->parent = parent;
186 for (i = 0; i < 4; i++)
193 * Recursively walk through the tree and propagate changes made to the
194 * given node up until the highest parent.
196 static void quadtree_recalculate_parent_stats(struct quadtree *node,
197 struct quadtree_ops *ops)
205 for (i = 0; i < 4; i++) {
209 node->depth = MAX(node->depth,
210 node->child[i]->depth + 1);
211 node->children += node->child[i]->children + 1;
214 /* The space covered by the parent node's corner
215 * points needs be as wide as its widest child node's
218 #define CHILD_CORNER_SAFE(node, ch_idx, cor_idx, axis) \
219 (node)->child[ch_idx] ? \
220 (node)->child[ch_idx]->corner[cor_idx].axis : \
223 node->corner[0].x = MIN(CHILD_CORNER_SAFE(node, 0, 0, x),
224 CHILD_CORNER_SAFE(node, 2, 0, x));
225 node->corner[0].y = MIN(CHILD_CORNER_SAFE(node, 0, 0, y),
226 CHILD_CORNER_SAFE(node, 1, 0, y));
227 node->corner[1].x = MAX(CHILD_CORNER_SAFE(node, 1, 1, x),
228 CHILD_CORNER_SAFE(node, 3, 1, x));
229 node->corner[1].y = MAX(CHILD_CORNER_SAFE(node, 2, 1, y),
230 CHILD_CORNER_SAFE(node, 3, 1, y));
231 #undef CHILD_CORNER_SAFE
233 if (ops->recalculate_stats)
234 ops->recalculate_stats(node);
241 * quadtree_add - add a node to a quadtree
242 * @parent: parent node
243 * @new: the new node to be added
245 * Add a node to a quadtree. The tree is kept in order, the new node
246 * is placed in the end of appropriate branch.
248 * The case of nodes sharing identical coordinates is not taken into
252 struct quadtree *quadtree_add(struct quadtree *parent, struct quadtree *new,
253 struct quadtree_ops *ops)
258 validate_tree(parent);
260 _quadtree_add(parent, new);
263 printf("adding node %p to parent %p\n", new, parent);
267 quadtree_recalculate_parent_stats(new, ops);
274 static double max_by_dir(double a, double b, int direction)
289 static double maxv_by_dir(struct quadtree *a, int direction)
304 static double quadtree_get_max_dimension(struct quadtree *node, int direction)
306 struct quadtree *ch1 = NULL, *ch2 = NULL;
311 ch1 = node->child[0];
312 ch2 = node->child[1];
315 ch1 = node->child[1];
316 ch2 = node->child[3];
319 ch1 = node->child[2];
320 ch2 = node->child[3];
323 ch1 = node->child[0];
324 ch2 = node->child[2];
331 a = quadtree_get_max_dimension(ch1, direction);
332 b = quadtree_get_max_dimension(ch2, direction);
333 return max_by_dir(a, b, direction);
337 a = quadtree_get_max_dimension(ch1, direction);
338 b = maxv_by_dir(ch1, direction);
339 return max_by_dir(a, b, direction);
342 a = quadtree_get_max_dimension(ch2, direction);
343 b = maxv_by_dir(ch2, direction);
344 return max_by_dir(a, b, direction);
347 return maxv_by_dir(node, direction);
351 * Stores the lower right and upper left corner coordinates to the
354 static void quadtree_get_tree_dimensions(struct quadtree *node,
355 struct vector *corner)
357 corner[0].y = quadtree_get_max_dimension(node, 2);
358 corner[0].x = quadtree_get_max_dimension(node, 1);
359 corner[1].y = quadtree_get_max_dimension(node, 0);
360 corner[1].x = quadtree_get_max_dimension(node, 3);
363 printf("\nInitial Search rectangle (%f %f) (%f %f), (%f %f)\n",
364 corner[0].x, corner[0].y,
365 corner[1].x, corner[1].y,
366 node->pos.x, node->pos.y);
370 if ((corner[0].x < corner[1].x) ||
371 (corner[0].y < corner[1].y))
375 static int is_within(struct vector *pos, struct vector *corner)
377 if ((pos->x >= corner[1].x) && (pos->x <= corner[0].x) &&
378 (pos->y >= corner[1].y) && (pos->y <= corner[0].y))
383 static int quadrants_to_search(struct quadtree *node, struct vector *corner)
385 int direction = 0, i;
386 int up = 0, left = 0, right = 0, down = 0;
388 for (i = 0; i < 2; i++) {
389 if (corner[i].x <= node->pos.x)
391 else if (corner[i].x >= node->pos.x)
393 if (corner[i].y <= node->pos.y)
395 else if (corner[i].y >= node->pos.y)
400 direction |= QUADTREE_UPLEFT;
402 direction |= QUADTREE_UPRIGHT;
404 direction |= QUADTREE_DOWNLEFT;
406 direction |= QUADTREE_DOWNRIGHT;
411 static struct quadtree *quadtree_find_nearest(struct quadtree *tree,
413 struct vector *corner)
416 struct quadtree *nearest, *node;
417 double distance = 0, dist;
420 if (!is_within(pos, corner)) {
422 printf("No nearest to be found from (%f %f) (%f %f) "
424 corner[0].x, corner[0].y,
425 corner[1].x, corner[1].y,
432 if (is_within(&tree->pos, corner)) {
433 vector_sub(pos, &tree->pos, &tmp);
434 distance = vector_abs(&tmp);
440 directions = quadrants_to_search(tree, corner);
442 for (i = 0; i < 4; i++) {
447 if (!(directions & (1 << i)))
450 node = quadtree_find_nearest(tree->child[i], pos, corner);
455 vector_sub(pos, &node->pos, &tmp);
456 dist = vector_abs(&tmp);
458 if (!nearest || dist < distance) {
465 if (nearest && !is_within(&nearest->pos, corner)) {
467 printf("Node %p (%f %f) is not within "
468 "search window (%f %f) (%f %f)\n",
469 nearest, nearest->pos.x, nearest->pos.y,
470 corner[0].x, corner[0].y,
471 corner[1].x, corner[1].y);
478 static struct quadtree *quadtree_find_nearest_noparent(struct quadtree *tree,
480 struct vector *corner)
482 struct quadtree *nearest;
484 struct quadtree *node;
485 double dist = 0, dist2;
488 nearest = quadtree_find_nearest(tree, pos, corner);
494 printf("Avoiding parent or NULL node %p\n", nearest);
497 * oops, we don't want to pick the parent node, let's choose
501 for (i = 0; i < 4; i++) {
506 nearest = quadtree_find_nearest(tree->child[i], pos,
512 vector_sub(pos, &nearest->pos, &sub);
513 dist = vector_abs(&sub);
516 node = quadtree_find_nearest(tree->child[i],
522 vector_sub(pos, &node->pos, &sub);
523 dist2 = vector_abs(&sub);
535 static void get_middle_point(struct vector *corner, struct vector *middle)
537 middle->x = (corner[0].x + corner[1].x) / (double)2;
538 middle->y = (corner[0].y + corner[1].y) / (double)2;
541 static int quadtree_split_by_node(struct quadtree *node,
542 struct vector *corner, int dir)
544 if (!is_within(&node->pos, corner)) {
546 printf("Not within search rectangle\n");
551 case QUADTREE_UPLEFT:
552 corner[0] = node->pos;
554 case QUADTREE_UPRIGHT:
555 corner[0].y = node->pos.y;
556 corner[1].x = node->pos.x;
558 case QUADTREE_DOWNRIGHT:
559 corner[1] = node->pos;
561 case QUADTREE_DOWNLEFT:
562 corner[0].x = node->pos.x;
563 corner[1].y = node->pos.y;
570 printf("Search rectangle is now (%f %f) (%f %f), (%f %f)\n",
571 corner[0].x, corner[0].y,
572 corner[1].x, corner[1].y,
573 node->pos.x, node->pos.y);
575 if ((corner[0].x < corner[1].x) ||
576 (corner[0].y < corner[1].y))
583 * Quickly detach a node from a tree. Move all child nodes under
586 static int _quadtree_del(struct quadtree *node, struct quadtree *parent)
591 /* Detach from the tree */
593 for (i = 0; i < 4; i++) {
594 if (node->parent->child[i] == node) {
595 node->parent->child[i] = 0;
604 for (i = 0; i < 4; i++) {
609 _quadtree_del(n, parent);
610 _quadtree_add(parent, n);
617 * Move everything under @tree node to @parent node, everything
618 * else except the @tree node itself
620 static int optimally_move_tree(struct quadtree *tree, struct quadtree *parent,
621 struct vector *corner_orig,
622 struct quadtree_ops *ops)
624 struct vector corner[2], mid;
625 struct quadtree *t, *tmp;
628 get_middle_point(corner_orig, &mid);
629 t = quadtree_find_nearest_noparent(tree, &mid, corner_orig);
633 printf("Cannot find nearest node\n");
639 * Now we have the t node which contains the object of the
640 * spatial middle coordinates of the tree.
644 printf("Relocating node %p (%f %f) under parent %p\n", t,
645 t->pos.x, t->pos.y, parent);
646 printf("There are %ld child nodes left\n", tree->children);
649 _quadtree_del(t, tree);
650 quadtree_add(parent, t, ops);
658 * Now split the search rectangle in quadtres and do the same
659 * with all of the quarters.
662 corner[0] = corner_orig[0];
663 corner[1] = corner_orig[1];
664 if (quadtree_split_by_node(t, corner, QUADTREE_UPLEFT))
665 moved += optimally_move_tree(tree, parent, corner, ops);
667 corner[0] = corner_orig[0];
668 corner[1] = corner_orig[1];
669 if (quadtree_split_by_node(t, corner, QUADTREE_UPRIGHT))
670 moved += optimally_move_tree(tree, parent, corner, ops);
672 corner[0] = corner_orig[0];
673 corner[1] = corner_orig[1];
674 if (quadtree_split_by_node(t, corner, QUADTREE_DOWNLEFT))
675 moved += optimally_move_tree(tree, parent, corner, ops);
677 corner[0] = corner_orig[0];
678 corner[1] = corner_orig[1];
679 if (quadtree_split_by_node(t, corner, QUADTREE_DOWNRIGHT))
680 moved += optimally_move_tree(tree, parent, corner, ops);
682 get_middle_point(corner_orig, &mid);
683 tmp = quadtree_find_nearest(tree, &mid, corner_orig);
684 if (tmp && tmp != tree)
689 printf("Now moved %d nodes, %ld left\n", moved, tree->children);
695 * quadtree_del - Detach a node from the tree
697 * Return value: The new root node of the tree. If we are detaching
698 * anything but the root node of the entire tree, the returned root
699 * value will be the original root of the tree.
701 struct quadtree *quadtree_del(struct quadtree *node,
702 struct quadtree_ops *ops)
704 struct quadtree *parent = NULL;
705 struct vector corner[2];
706 int i, children = node->children, moved;
711 printf("Deleting node %p under parent %p\n",
713 printf("Relocating %ld children\n", node->children);
719 * We are deleting the root node. This means we have
720 * to select a new root node and reconstruct the
721 * entire tree under it again.
724 printf("Deleting root node\n");
727 for (i = 0; i < 4; i++) {
731 parent = node->child[i];
732 _quadtree_del(parent, node);
734 quadtree_recalculate_parent_stats(parent, ops);
739 * The node has a parent. Detach the node from it and
740 * relocate the children.
743 for (i = 0; i < 4; i++) {
744 if (node->parent->child[i] == node) {
745 node->parent->child[i] = 0;
752 parent = node->parent;
755 * The sub branch is now detached from the main
756 * tree. Fix the stats.
758 quadtree_recalculate_parent_stats(node->parent, ops);
761 validate_tree(parent);
766 * Now we are ready to prepare for relocating the nodes under
770 quadtree_get_tree_dimensions(node, corner);
771 moved = optimally_move_tree(node, parent, corner, ops);
774 if (moved != children) {
775 printf("Got %d children but %d were moved\n",
777 printf("nearest children left:\n");
778 for (i = 0 ; i < 4; i++)
780 printf(" node %d %p, (%f %f)\n",
782 node->child[i]->pos.x,
783 node->child[i]->pos.y);
787 printf("Delete done, returning parent %p\n", parent);
792 validate_tree(parent);
793 return quadtree_find_parent(parent);
796 static void check_for_crossed_subnodes(struct quadtree *node,
797 struct vector *limit, struct quadtree_ops *ops)
799 int direction = 0, i;
800 int up = 0, left = 0, right = 0, down = 0;
802 for (i = 0; i < 2; i++) {
803 if (limit[i].x < node->pos.x)
807 if (limit[i].y < node->pos.y)
814 direction |= QUADTREE_UPLEFT;
816 direction |= QUADTREE_UPRIGHT;
818 direction |= QUADTREE_DOWNLEFT;
820 direction |= QUADTREE_DOWNRIGHT;
821 if ((left && right) || (up && down))
822 direction |= QUADTREE_SELF;
824 if ((direction & QUADTREE_UPLEFT) && node->child[0])
825 check_for_crossed_subnodes(node->child[0], limit, ops);
827 if ((direction & QUADTREE_UPRIGHT) && node->child[1])
828 check_for_crossed_subnodes(node->child[0], limit, ops);
830 if ((direction & QUADTREE_DOWNLEFT) && node->child[2])
831 check_for_crossed_subnodes(node->child[0], limit, ops);
833 if ((direction & QUADTREE_DOWNRIGHT) && node->child[3])
834 check_for_crossed_subnodes(node->child[0], limit, ops);
836 if (direction & QUADTREE_SELF) {
837 struct quadtree *parent;
839 parent = quadtree_del(node, ops);
840 quadtree_add(parent, node, ops);
844 struct quadtree *quadtree_move(struct quadtree *node, struct vector new_pos,
845 struct quadtree_ops *ops)
847 struct quadtree *parent, *tree_parent;
850 /* Check if we have crossed any of the parents */
851 parent = node->parent;
853 if (node->pos.x < parent->pos.x && new_pos.x > parent->pos.x)
855 if (node->pos.x > parent->pos.x && new_pos.x < parent->pos.x)
857 if (node->pos.y < parent->pos.y && new_pos.y > parent->pos.y)
859 if (node->pos.y > parent->pos.y && new_pos.y < parent->pos.y)
863 parent = parent->parent;
868 * If the node has crossed the boundaries, remove it
869 * from the tree and add it again to it. It is then
870 * guaranteed to be in the correct position of the
873 tree_parent = quadtree_del(node, ops);
875 quadtree_add(tree_parent, node, ops);
876 quadtree_recalculate_parent_stats(node, ops);
880 /* Move the node into its new location */
885 * Now, search the subtree for any children that are
886 * located in wrong place and move them into correct
887 * place within the tree.
889 struct vector limit[2];
891 limit[0] = node->pos;
893 check_for_crossed_subnodes(node, limit, ops);
896 quadtree_recalculate_parent_stats(node, ops);
897 return quadtree_find_parent(node);
901 static int _walk_tree(struct quadtree *head, const struct quadtree_iterator *it)
903 int direction, count = 0;
905 direction = it->direction(head, (struct quadtree_iterator *)it);
907 if ((direction & QUADTREE_UPLEFT) && head->child[0])
908 count += _walk_tree(head->child[0], it);
910 if ((direction & QUADTREE_UPRIGHT) && head->child[1])
911 count += _walk_tree(head->child[1], it);
913 if ((direction & QUADTREE_DOWNLEFT) && head->child[2])
914 count += _walk_tree(head->child[2], it);
916 if ((direction & QUADTREE_DOWNRIGHT) && head->child[3])
917 count += _walk_tree(head->child[3], it);
919 if ((direction & QUADTREE_SELF) && it->callback) {
920 it->callback(head, (struct quadtree_iterator *)it);
927 int walk_quadtree(const struct quadtree_iterator *it)
929 return _walk_tree(it->head, it);