git.itanic.dy.fi Git - sdl-planets/blob - quadtree.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3
   4 #include "quadtree.h"
   5
   6 #ifdef DEBUG
   7 #define debug 1
   8 #else
   9 #define debug 0
  10 #endif
  11
  12 static void _trap(int line)
  13 {
  14         if (debug) {
  15                 printf("Trapped from line %d, use debugger to get backtrace\n",
  16                         line);
  17                 fflush(stdout);
  18                 exit(1);
  19         }
  20 }
  21
  22 #define trap() _trap(__LINE__)
  23
  24 static int quadtree_compare_coord(const struct vector *a,
  25                                 const struct vector *b)
  26 {
  27         int up, left;
  28
  29         up = b->y < a->y;
  30         left = b->x < a->x;
  31
  32         if (up && left)
  33                 return 0;
  34         if (up && !left)
  35                 return 1;
  36         if (left)
  37                 return 2;
  38         return 3;
  39 }
  40
  41 static int quadtree_compare(const struct quadtree *a, const struct quadtree *b)
  42 {
  43         return quadtree_compare_coord(&a->pos, &b->pos);
  44 }
  45
  46 static void validate_subtree(const struct quadtree *node)
  47 {
  48         int i, dir;
  49         long int children;
  50         children = 0;
  51
  52         if (!node) {
  53                 printf("Attempted to validate a null pointer\n");
  54                 fflush(stdout);
  55                 trap();
  56         }
  57
  58         for (i = 0; i < 4; i++) {
  59                 if (!node->child[i])
  60                         continue;
  61
  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);
  67                         fflush(stdout);
  68                         trap();
  69                 }
  70
  71                 if (node->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);
  76                                 fflush(stdout);
  77                                 trap();
  78                         }
  79                 }
  80
  81                 dir = quadtree_compare(node, node->child[i]);
  82
  83                 if (dir != i) {
  84                         printf("%s:%d Fatal! Spatial inconsistency detected "
  85                                 "at child %d in node %p\n"
  86                                 "parent: (%f %f), child (%f %f), "
  87                                 "dir %d != %d\n",
  88                                 __func__, __LINE__, i, node,
  89                                 node->pos.x, node->pos.y,
  90                                 node->child[i]->pos.x, node->child[i]->pos.y,
  91                                 dir, i);
  92                         fflush(stdout);
  93                         trap();
  94                 }
  95
  96                 children += node->child[i]->children + 1;
  97
  98                 validate_subtree(node->child[i]);
  99         }
 100
 101         if (node->depth < 0) {
 102                 printf("%s:%d Tree statistics inconsistency detected! "
 103                        "Negative depth: %ld\n",
 104                        __func__, __LINE__, node->depth);
 105         }
 106
 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);
 111                 fflush(stdout);
 112                 trap();
 113         }
 114
 115         for (i = 0; i < 4 && node->children; i++) {
 116                 if (!node->child[i])
 117                         continue;
 118
 119                 if (node->depth == node->child[i]->depth + 1)
 120                         break;
 121
 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,
 126                                node->depth);
 127                 }
 128         }
 129
 130         if (i == 4) {
 131                 printf("%s:%d Tree statistics inconsistency detected! "
 132                        "child depth mismatch.",
 133                        __func__, __LINE__);
 134                 fflush(stdout);
 135                 trap();
 136         }
 137 }
 138
 139 static void validate_tree(const struct quadtree *node)
 140 {
 141         int i;
 142
 143         if (!debug)
 144                 return;
 145
 146         if (!node->parent)
 147                 return validate_subtree(node);
 148
 149         for (i = 0; i < 4; i++)
 150                 if (node->parent->child[i] == node)
 151                         break;
 152
 153         if (i == 4) {
 154                 printf("%s:%d Tree inconsistency detected! "
 155                         "Wrong parent %p for node %p\n",
 156                         __func__, __LINE__, node->parent, node);
 157                 fflush(stdout);
 158                 trap();
 159         }
 160
 161         validate_tree(node->parent);
 162 }
 163
 164 void _quadtree_validate_tree(const struct quadtree *node)
 165 {
 166         validate_tree(node);
 167 }
 168
 169 static struct quadtree *_quadtree_add(struct quadtree *parent,
 170                                 struct quadtree *new)
 171 {
 172         int ret, i;
 173
 174         ret = quadtree_compare(parent, new);
 175
 176         if (ret < 0 || ret >= 4) {
 177                 printf("Invalid comparison result of %d\n", ret);
 178                 return 0;
 179         }
 180
 181         if (parent->child[ret])
 182                 return _quadtree_add(parent->child[ret], new);
 183
 184         parent->child[ret] = new;
 185         new->parent = parent;
 186         for (i = 0; i < 4; i++)
 187                 new->child[i] = 0;
 188
 189         return new;
 190 }
 191
 192 static void _recalculate_node_area_stats(struct quadtree *node)
 193 {
 194                 /* The space covered by the parent node's corner
 195                  * points needs be as wide as its widest child node's
 196                  * corners.
 197                  */
 198 #define CHILD_CORNER_SAFE(node, ch_idx, cor_idx, axis)          \
 199         ((node)->child[ch_idx] ?                                \
 200                 (node)->child[ch_idx]->corner[cor_idx].axis :   \
 201                 (node)->pos.axis)
 202
 203                 node->corner[0].x = MIN(CHILD_CORNER_SAFE(node, 0, 0, x),
 204                                         CHILD_CORNER_SAFE(node, 2, 0, x));
 205                 node->corner[0].y = MIN(CHILD_CORNER_SAFE(node, 0, 0, y),
 206                                         CHILD_CORNER_SAFE(node, 1, 0, y));
 207                 node->corner[1].x = MAX(CHILD_CORNER_SAFE(node, 1, 1, x),
 208                                         CHILD_CORNER_SAFE(node, 3, 1, x));
 209                 node->corner[1].y = MAX(CHILD_CORNER_SAFE(node, 2, 1, y),
 210                                         CHILD_CORNER_SAFE(node, 3, 1, y));
 211 #undef CHILD_CORNER_SAFE
 212 }
 213
 214 static void recalculate_parent_area_stats(struct quadtree *node)
 215 {
 216         struct vector old_corner[2];
 217
 218         while (node) {
 219                 memcpy(&old_corner, &node->corner, sizeof(old_corner));
 220
 221                 _recalculate_node_area_stats(node);
 222
 223                 /*
 224                  * Stop propagating the changes up in the tree if
 225                  * nothing has changed
 226                  */
 227                 if (memcmp(&old_corner, &node->corner, sizeof(old_corner)))
 228                         node = node->parent;
 229                 else
 230                         node = NULL;
 231         }
 232 }
 233
 234 /*
 235  * Recursively walk through the tree and propagate changes made to the
 236  * given node up until the highest parent.
 237  */
 238 static void quadtree_recalculate_parent_stats(struct quadtree *node,
 239                                                      struct quadtree_ops *ops)
 240 {
 241         int i;
 242
 243         while (node) {
 244                 node->depth = 0;
 245                 node->children = 0;
 246
 247                 for (i = 0; i < 4; i++) {
 248                         if (!node->child[i])
 249                                 continue;
 250
 251                         node->depth = MAX(node->depth,
 252                                           node->child[i]->depth + 1);
 253                         node->children += node->child[i]->children + 1;
 254                 }
 255
 256                 _recalculate_node_area_stats(node);
 257
 258                 if (ops->recalculate_stats)
 259                         ops->recalculate_stats(node);
 260
 261                 node = node->parent;
 262         }
 263 }
 264
 265 /**
 266  * quadtree_add - add a node to a quadtree
 267  * @parent: parent node
 268  * @new: the new node to be added
 269  *
 270  * Add a node to a quadtree. The tree is kept in order, the new node
 271  * is placed in the end of appropriate branch.
 272  *
 273  * The case of nodes sharing identical coordinates is not taken into
 274  * account at all.
 275  */
 276
 277 struct quadtree *quadtree_add(struct quadtree *parent, struct quadtree *new,
 278                               struct quadtree_ops *ops)
 279 {
 280         if (parent == new)
 281                 trap();
 282
 283         validate_tree(parent);
 284
 285         _quadtree_add(parent, new);
 286
 287         if (debug) {
 288                 printf("adding node %p to parent %p\n", new, parent);
 289                 fflush(stdout);
 290         }
 291
 292         quadtree_recalculate_parent_stats(new, ops);
 293
 294         validate_tree(new);
 295
 296         return new;
 297 }
 298
 299 static int is_within(struct vector *pos, struct vector *corner)
 300 {
 301         if ((pos->x >= corner[1].x) && (pos->x <= corner[0].x) &&
 302             (pos->y >= corner[1].y) && (pos->y <= corner[0].y))
 303                 return 1;
 304         return 0;
 305 }
 306
 307 static int rectangle_and_circle_overlap(struct vector *corner,
 308                                         struct vector *pos, double radius)
 309 {
 310         int vertically_overlapping = 0, horizontally_overlapping = 0;
 311
 312         if ((pos->x < corner[1].x && pos->x + radius > corner[0].x) ||
 313             (pos->x > corner[0].x && pos->x - radius < corner[1].x))
 314                 horizontally_overlapping = 1;
 315
 316         if ((pos->y < corner[1].y && pos->y + radius > corner[0].y) ||
 317             (pos->y > corner[0].y && pos->y - radius < corner[1].y))
 318                 vertically_overlapping = 1;
 319
 320         return horizontally_overlapping && vertically_overlapping;
 321 }
 322
 323 static int quadrants_to_search(struct quadtree *node, struct vector *pos,
 324                         double dist)
 325 {
 326         int direction = 0;
 327
 328         if (dist <= 0)
 329                 return 0xf;
 330
 331         if (node->child[0] &&
 332                 rectangle_and_circle_overlap(node->child[0]->corner, pos, dist))
 333                 direction |= QUADTREE_UPLEFT;
 334
 335         if (node->child[1] &&
 336                 rectangle_and_circle_overlap(node->child[1]->corner, pos, dist))
 337                 direction |= QUADTREE_UPRIGHT;
 338
 339         if (node->child[2] &&
 340                 rectangle_and_circle_overlap(node->child[2]->corner, pos, dist))
 341                 direction |= QUADTREE_DOWNLEFT;
 342
 343         if (node->child[3] &&
 344                 rectangle_and_circle_overlap(node->child[3]->corner, pos, dist))
 345                 direction |= QUADTREE_DOWNRIGHT;
 346
 347         return direction;
 348 }
 349
 350 static struct quadtree *quadtree_find_nearest(struct quadtree *tree,
 351                                         struct vector *pos,
 352                                         struct vector *corner,
 353                                         struct quadtree *nearest,
 354                                         int depth)
 355 {
 356         struct vector tmp;
 357         struct quadtree *node;
 358         double distance = 0, dist;
 359         int i, directions;
 360
 361         if (nearest == NULL)
 362                 nearest = tree;
 363
 364         vector_sub(&nearest->pos, pos, &tmp);
 365         distance = vector_abs_squared(&tmp);
 366
 367         if (tree != nearest) {
 368                 vector_sub(&tree->pos, pos, &tmp);
 369                 dist = vector_abs_squared(&tmp);
 370                 if (dist < distance) {
 371                         nearest = tree;
 372                         distance = dist;
 373                 }
 374         }
 375
 376         if (!is_within(&nearest->pos, corner)) {
 377                 if (debug)
 378                         printf("Discarding nearest %p "
 379                                 "as outside of search window\n", nearest);
 380                 nearest = NULL;
 381                 distance = -1;
 382         }
 383
 384         directions = quadrants_to_search(tree, pos, distance);
 385         if (debug)
 386                 printf("%d: Directions: %x\n", depth, directions);
 387         for (i = 0; i < 4; i++) {
 388                 if (!tree->child[i])
 389                         continue;
 390
 391                 if (!(directions & (1 << i)))
 392                         continue;
 393
 394
 395                 node = quadtree_find_nearest(tree->child[i], pos, corner,
 396                                         nearest, depth + 1);
 397
 398                 if (!node)
 399                         continue;
 400
 401                 if (node != nearest) {
 402                         vector_sub(&node->pos, pos, &tmp);
 403                         dist = vector_abs_squared(&tmp);
 404                         if (dist < distance || !nearest) {
 405                                 nearest = node;
 406                                 distance = dist;
 407                         }
 408                         /*
 409                          * Update the directions where to search for
 410                          * since those may not be valid any more
 411                          */
 412                         directions = quadrants_to_search(tree, pos, distance);
 413                         continue;
 414                 }
 415         }
 416
 417         if (nearest && !is_within(&nearest->pos, corner)) {
 418                 if (debug)
 419                         printf("%d: Node %p (%f %f) is not within "
 420                                 "search window (%f %f) (%f %f)\n", depth,
 421                                 nearest, nearest->pos.x, nearest->pos.y,
 422                                 corner[0].x, corner[0].y,
 423                                 corner[1].x, corner[1].y);
 424                 return NULL;
 425         }
 426         if (debug)
 427                 printf("%d: Node %p (%f %f) is nearest node, dist %f\n",
 428                         depth, nearest,
 429                         nearest ? nearest->pos.x : -1,
 430                         nearest ? nearest->pos.y : -1,
 431                         nearest ? distance : -1);
 432         return nearest;
 433 }
 434
 435 static struct quadtree *quadtree_find_nearest_noparent(struct quadtree *tree,
 436                                                 struct vector *pos,
 437                                                 struct vector *corner)
 438 {
 439         struct quadtree *nearest = NULL;
 440         struct vector sub;
 441         struct quadtree *node;
 442         double dist = 0, dist2;
 443         int i;
 444
 445         nearest = quadtree_find_nearest(tree, pos, corner, nearest, 0);
 446
 447         if (nearest != tree)
 448                 goto out;
 449
 450         if (debug)
 451                 printf("Avoiding parent or NULL node %p\n", nearest);
 452
 453         /*
 454          * oops, we don't want to pick the parent node, let's choose
 455          * another one
 456          */
 457         nearest = NULL;
 458         for (i = 0; i < 4; i++) {
 459                 if (!tree->child[i])
 460                         continue;
 461
 462                 if (!nearest) {
 463                         nearest = quadtree_find_nearest(tree->child[i], pos,
 464                                                         corner, nearest, 0);
 465
 466                         if (nearest == NULL)
 467                                 continue;
 468
 469                         vector_sub(pos, &nearest->pos, &sub);
 470                         dist = vector_abs_squared(&sub);
 471                         continue;
 472                 }
 473                 node = quadtree_find_nearest(tree->child[i], pos, corner,
 474                                         nearest, 0);
 475
 476                 if (node == NULL)
 477                         continue;
 478
 479                 vector_sub(pos, &node->pos, &sub);
 480                 dist2 = vector_abs_squared(&sub);
 481
 482                 if (dist2 < dist) {
 483                         nearest = node;
 484                         dist = dist2;
 485                 }
 486         }
 487
 488 out:
 489         return nearest;
 490 }
 491
 492 static void get_middle_point(struct vector *corner, struct vector *middle)
 493 {
 494         middle->x = (corner[0].x + corner[1].x) / (double)2;
 495         middle->y = (corner[0].y + corner[1].y) / (double)2;
 496 }
 497
 498 static int quadtree_split_by_node(struct quadtree *node,
 499                                 struct vector *corner, int dir)
 500 {
 501         if (!is_within(&node->pos, corner)) {
 502                 if (debug)
 503                         printf("Not within search rectangle\n");
 504                 return 0;
 505         }
 506
 507         if (debug)
 508                 printf("Search rectangle was before (%f %f) (%f %f), (%f %f)\n",
 509                         corner[0].x, corner[0].y,
 510                         corner[1].x, corner[1].y,
 511                         node->pos.x, node->pos.y);
 512
 513         switch (dir) {
 514         case QUADTREE_UPLEFT:
 515                 corner[0] = node->pos;
 516                 break;
 517         case QUADTREE_UPRIGHT:
 518                 corner[0].y = node->pos.y;
 519                 corner[1].x = node->pos.x;
 520                 break;
 521         case QUADTREE_DOWNRIGHT:
 522                 corner[1] = node->pos;
 523                 break;
 524         case QUADTREE_DOWNLEFT:
 525                 corner[0].x = node->pos.x;
 526                 corner[1].y = node->pos.y;
 527                 break;
 528         default:
 529                 trap();
 530         }
 531
 532         if (debug)
 533                 printf("Search rectangle is now (%f %f) (%f %f), (%f %f)\n",
 534                         corner[0].x, corner[0].y,
 535                         corner[1].x, corner[1].y,
 536                         node->pos.x, node->pos.y);
 537
 538         if ((corner[0].x < corner[1].x) ||
 539             (corner[0].y < corner[1].y))
 540                 trap();
 541
 542         return 1;
 543 }
 544
 545 /**
 546  * Quickly detach a node from a tree. Move all child nodes under
 547  * @parent node.
 548  */
 549 static int _quadtree_del(struct quadtree *node, struct quadtree *parent)
 550 {
 551         struct quadtree *n;
 552         int i;
 553
 554         /* Detach from the tree */
 555         if (node->parent)
 556                 for (i = 0; i < 4; i++) {
 557                         if (node->parent->child[i] == node) {
 558                                 node->parent->child[i] = 0;
 559                                 break;
 560                         }
 561                 }
 562
 563         if (i == 4)
 564                 trap();
 565
 566         for (i = 0; i < 4; i++) {
 567                 if (!node->child[i])
 568                         continue;
 569
 570                 n = node->child[i];
 571                 _quadtree_del(n, parent);
 572                 _quadtree_add(parent, n);
 573                 recalculate_parent_area_stats(n);
 574         }
 575
 576         return 0;
 577 }
 578
 579 /**
 580  * Move everything under @tree node to @parent node, everything
 581  * else except the @tree node itself
 582  */
 583 static int optimally_move_tree(struct quadtree *tree, struct quadtree *parent,
 584                         struct vector *corner_orig,
 585                         struct quadtree_ops *ops)
 586 {
 587         struct vector corner[2], mid;
 588         struct quadtree *t;
 589         int moved = 0;
 590
 591         get_middle_point(corner_orig, &mid);
 592         t = quadtree_find_nearest_noparent(tree, &mid, corner_orig);
 593
 594         if (!t) {
 595                 if (debug)
 596                         printf("Cannot find nearest node\n");
 597
 598                 goto out;
 599         }
 600
 601         /*
 602          * Now we have the t node which contains the object of the
 603          * spatial middle coordinates of the tree.
 604          */
 605
 606         if (debug) {
 607                 printf("Relocating node %p (%f %f) under parent %p\n", t,
 608                         t->pos.x, t->pos.y, parent);
 609                 printf("There are %ld child nodes left\n", tree->children);
 610                 fflush(stdout);
 611         }
 612         _quadtree_del(t, tree);
 613         quadtree_add(parent, t, ops);
 614
 615         moved++;
 616         tree->children--;
 617         if (!tree->children)
 618                 goto out;
 619
 620         /*
 621          * Now split the search rectangle in quadtres and do the same
 622          * with all of the quarters.
 623          */
 624
 625         corner[0] = corner_orig[0];
 626         corner[1] = corner_orig[1];
 627         if (quadtree_split_by_node(t, corner, QUADTREE_UPLEFT))
 628                 moved += optimally_move_tree(tree, parent, corner, ops);
 629
 630         corner[0] = corner_orig[0];
 631         corner[1] = corner_orig[1];
 632         if (quadtree_split_by_node(t, corner, QUADTREE_UPRIGHT))
 633                 moved += optimally_move_tree(tree, parent, corner, ops);
 634
 635         corner[0] = corner_orig[0];
 636         corner[1] = corner_orig[1];
 637         if (quadtree_split_by_node(t, corner, QUADTREE_DOWNLEFT))
 638                 moved += optimally_move_tree(tree, parent, corner, ops);
 639
 640         corner[0] = corner_orig[0];
 641         corner[1] = corner_orig[1];
 642         if (quadtree_split_by_node(t, corner, QUADTREE_DOWNRIGHT))
 643                 moved += optimally_move_tree(tree, parent, corner, ops);
 644
 645 out:
 646         if (debug)
 647                 printf("Now moved %d nodes, %ld left\n", moved, tree->children);
 648
 649         return moved;
 650 }
 651
 652 /*
 653  * quadtree_del - Detach a node from the tree
 654  *
 655  * Return value: The new root node of the tree. If we are detaching
 656  * anything but the root node of the entire tree, the returned root
 657  * value will be the original root of the tree.
 658  */
 659 struct quadtree *quadtree_del(struct quadtree *node,
 660                               struct quadtree_ops *ops)
 661 {
 662         struct quadtree *parent = NULL;
 663         struct vector corner[2];
 664         int i, children = node->children, moved;
 665
 666         validate_tree(node);
 667
 668         if (debug) {
 669                 printf("Deleting node %p under parent %p\n",
 670                                node, node->parent);
 671                 printf("Relocating %ld children\n", node->children);
 672                 fflush(stdout);
 673         }
 674
 675         if (!node->parent) {
 676                 /*
 677                  * We are deleting the root node. This means we have
 678                  * to select a new root node and reconstruct the
 679                  * entire tree under it again.
 680                  */
 681                 if (debug) {
 682                         printf("Deleting root node\n");
 683                         fflush(stdout);
 684                 }
 685                 for (i = 0; i < 4; i++) {
 686                         if (!node->child[i])
 687                                 continue;
 688
 689                         parent = node->child[i];
 690                         _quadtree_del(parent, node);
 691                         parent->parent = 0;
 692                         quadtree_recalculate_parent_stats(parent, ops);
 693                         break;
 694                 }
 695         } else {
 696                 /*
 697                  * The node has a parent. Detach the node from it and
 698                  * relocate the children.
 699                  */
 700
 701                 for (i = 0; i < 4; i++) {
 702                         if (node->parent->child[i] == node) {
 703                                 node->parent->child[i] = 0;
 704                                 break;
 705                         }
 706                 }
 707                 if (i == 4)
 708                         trap();
 709
 710                 parent = node->parent;
 711                 node->parent = NULL;
 712
 713                 /*
 714                  * The sub branch is now detached from the main
 715                  * tree. Fix the stats.
 716                  */
 717                 quadtree_recalculate_parent_stats(parent, ops);
 718         }
 719
 720         validate_tree(parent);
 721
 722         if (!node->children)
 723                 goto out;
 724         /*
 725          * Now we are ready to prepare for relocating the nodes under
 726          * the parent.
 727          */
 728
 729         corner[0] = node->corner[1];
 730         corner[1] = node->corner[0];
 731         if (debug) {
 732                 printf("\nInitial Search rectangle (%f %f) (%f %f), (%f %f)\n",
 733                         corner[0].x, corner[0].y,
 734                         corner[1].x, corner[1].y,
 735                         node->pos.x, node->pos.y);
 736                 fflush(stdout);
 737         }
 738         moved = optimally_move_tree(node, parent, corner, ops);
 739
 740         if (debug) {
 741                 if (moved != children) {
 742                         printf("Got %d children but %d were moved\n",
 743                                 children, moved);
 744                         printf("nearest children left:\n");
 745                         for (i = 0 ; i < 4; i++)
 746                                 if (node->child[i])
 747                                         printf("   node %d %p, (%f %f)\n",
 748                                                 i, node->child[i],
 749                                                 node->child[i]->pos.x,
 750                                                 node->child[i]->pos.y);
 751                         fflush(stdout);
 752                         trap();
 753                 }
 754                 printf("Delete done, returning parent %p\n", parent);
 755                 fflush(stdout);
 756         }
 757
 758 out:
 759         validate_tree(parent);
 760         return quadtree_find_parent(parent);
 761 }
 762
 763 static int check_for_crossed_subnodes(struct quadtree *node,
 764                                 struct quadtree *skip,
 765                                 struct vector *limit, struct quadtree_ops *ops)
 766 {
 767         int direction = 0, i;
 768         int up = 0, left = 0, right = 0, down = 0;
 769         int cross = 0;
 770
 771         for (i = 0; i < 2; i++) {
 772                 if (limit[i].x < node->pos.x)
 773                         left = 1;
 774                 else
 775                         right = 1;
 776                 if (limit[i].y < node->pos.y)
 777                         up = 1;
 778                 else
 779                         down = 1;
 780         }
 781
 782         if (left || up)
 783                 direction |= QUADTREE_UPLEFT;
 784         if (right || up)
 785                 direction |= QUADTREE_UPRIGHT;
 786         if (left || down)
 787                 direction |= QUADTREE_DOWNLEFT;
 788         if (right || down)
 789                 direction |= QUADTREE_DOWNRIGHT;
 790         if ((left && right) || (up && down))
 791                 direction |= QUADTREE_SELF;
 792
 793
 794         /*
 795          * We need to skip the topmost node (the one that was actually
 796          * moved), otherwise we would trigger rebalance every time
 797          */
 798         if (node == skip)
 799                 goto child_check;
 800
 801         if (direction & QUADTREE_SELF) {
 802                 struct quadtree *parent;
 803
 804                 if (debug)
 805                         printf("Rebalanced because crossed child %p: "
 806                                 "node (%f %f) limits (%f %f) (%f %f)\n",
 807                                 node,
 808                                 node->pos.x, node->pos.y,
 809                                 limit[0].x, limit[0].y,
 810                                 limit[1].x, limit[1].y);
 811                 return 1;
 812                 parent = quadtree_del(node, ops);
 813                 quadtree_add(parent, node, ops);
 814         }
 815
 816 child_check:
 817         if ((direction & QUADTREE_UPLEFT) && node->child[0])
 818                 cross += check_for_crossed_subnodes(node->child[0], skip,
 819                                                 limit, ops);
 820
 821         if (cross)
 822                 goto out;
 823
 824         if ((direction & QUADTREE_UPRIGHT) && node->child[1])
 825                 cross += check_for_crossed_subnodes(node->child[1], skip,
 826                                                 limit, ops);
 827
 828         if (cross)
 829                 goto out;
 830
 831         if ((direction & QUADTREE_DOWNLEFT) && node->child[2])
 832                 cross += check_for_crossed_subnodes(node->child[2], skip,
 833                                                 limit, ops);
 834
 835         if (cross)
 836                 goto out;
 837
 838         if ((direction & QUADTREE_DOWNRIGHT) && node->child[3])
 839                 cross += check_for_crossed_subnodes(node->child[3], skip,
 840                                                 limit, ops);
 841 out:
 842         return cross;
 843 }
 844
 845 struct quadtree *quadtree_move(struct quadtree *node, struct vector new_pos,
 846                         struct quadtree_ops *ops)
 847 {
 848         struct quadtree *parent, *tree_parent;
 849         int modify = 0;
 850         int parents = 0, all_parents = 0;
 851
 852         if (debug) {
 853                 parent = node->parent;
 854                 while (parent) {
 855                         parent = parent->parent;
 856                         all_parents++;
 857                 }
 858         }
 859
 860         /* Check if we have crossed any of the parents */
 861         parent = node->parent;
 862         while (parent) {
 863                 parents++;
 864                 if (node->pos.x < parent->pos.x && new_pos.x > parent->pos.x)
 865                         modify |= 1;
 866                 if (node->pos.x > parent->pos.x && new_pos.x < parent->pos.x)
 867                         modify |= 2;
 868                 if (node->pos.y < parent->pos.y && new_pos.y > parent->pos.y)
 869                         modify |= 4;
 870                 if (node->pos.y > parent->pos.y && new_pos.y < parent->pos.y)
 871                         modify |= 8;
 872
 873                 if (!modify) {
 874                         parent = parent->parent;
 875                         continue;
 876                 }
 877
 878                 if (debug)
 879                         printf("Rebalanced because crossed parent: node %p, "
 880                                 "%d/%d %x "
 881                                 "old (%f %f) new (%f %f) parent (%f %f)\n",
 882                                 node, parents, all_parents, modify,
 883                                 node->pos.x, node->pos.y,
 884                                 new_pos.x, new_pos.y,
 885                                 parent->pos.x, parent->pos.y);
 886                 /*
 887                  * If the node has crossed the boundaries, remove it
 888                  * from the tree and add it again to it. It is then
 889                  * guaranteed to be in the correct position of the
 890                  * tree.
 891                  */
 892                 validate_tree(node);
 893                 tree_parent = quadtree_del(node, ops);
 894                 node->pos = new_pos;
 895                 quadtree_add(tree_parent, node, ops);
 896                 validate_tree(node);
 897
 898                 return tree_parent;
 899         }
 900
 901         if (node->children) {
 902                 /*
 903                  * Now, search the subtree for any children that are
 904                  * located in wrong place and move them into correct
 905                  * place within the tree.
 906                  */
 907                 struct vector limit[2];
 908
 909                 limit[0] = node->pos;
 910                 limit[1] = new_pos;
 911                 modify = check_for_crossed_subnodes(node, node, limit, ops);
 912         }
 913
 914         /* Move the node into its new location */
 915
 916         validate_tree(node);
 917         if (modify)
 918                 tree_parent = quadtree_del(node, ops);
 919         node->pos = new_pos;
 920         if (!modify)
 921                 recalculate_parent_area_stats(node);
 922         if (modify)
 923                 quadtree_add(tree_parent, node, ops);
 924         validate_tree(node);
 925         return quadtree_find_parent(node);
 926
 927 }
 928
 929 static int _walk_tree(struct quadtree *head, const struct quadtree_iterator *it)
 930 {
 931         int direction, count = 0;
 932
 933         direction = it->direction(head, (struct quadtree_iterator *)it);
 934
 935         if ((direction & QUADTREE_UPLEFT) && head->child[0])
 936                 count += _walk_tree(head->child[0], it);
 937
 938         if ((direction & QUADTREE_UPRIGHT) && head->child[1])
 939                 count += _walk_tree(head->child[1], it);
 940
 941         if ((direction & QUADTREE_DOWNLEFT) && head->child[2])
 942                 count += _walk_tree(head->child[2], it);
 943
 944         if ((direction & QUADTREE_DOWNRIGHT) && head->child[3])
 945                 count += _walk_tree(head->child[3], it);
 946
 947         if ((direction & QUADTREE_SELF) && it->callback) {
 948                 it->callback(head, (struct quadtree_iterator *)it);
 949                 count++;
 950         }
 951
 952         return count;
 953 }
 954
 955 int walk_quadtree(const struct quadtree_iterator *it)
 956 {
 957         return _walk_tree(it->head, it);
 958 }