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