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