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 /*
 193  * Recursively walk through the tree and propagate changes made to the
 194  * given node up until the highest parent.
 195  */
 196 static void quadtree_recalculate_parent_stats(struct quadtree *node,
 197                                                      struct quadtree_ops *ops)
 198 {
 199         int i;
 200
 201         while (node) {
 202                 node->depth = 0;
 203                 node->children = 0;
 204
 205                 for (i = 0; i < 4; i++) {
 206                         if (!node->child[i])
 207                                 continue;
 208
 209                         node->depth = MAX(node->depth,
 210                                           node->child[i]->depth + 1);
 211                         node->children += node->child[i]->children + 1;
 212                 }
 213
 214                 /* The space covered by the parent node's corner
 215                  * points needs be as wide as its widest child node's
 216                  * corners.
 217                  */
 218 #define CHILD_CORNER_SAFE(node, ch_idx, cor_idx, axis)               \
 219                 (node)->child[ch_idx] ?                              \
 220                         (node)->child[ch_idx]->corner[cor_idx].axis : \
 221                         (node)->pos.axis
 222
 223                 node->corner[0].x = MIN(CHILD_CORNER_SAFE(node, 0, 0, x),
 224                                         CHILD_CORNER_SAFE(node, 2, 0, x));
 225                 node->corner[0].y = MIN(CHILD_CORNER_SAFE(node, 0, 0, y),
 226                                         CHILD_CORNER_SAFE(node, 1, 0, y));
 227                 node->corner[1].x = MAX(CHILD_CORNER_SAFE(node, 1, 1, x),
 228                                         CHILD_CORNER_SAFE(node, 3, 1, x));
 229                 node->corner[1].y = MAX(CHILD_CORNER_SAFE(node, 2, 1, y),
 230                                         CHILD_CORNER_SAFE(node, 3, 1, y));
 231 #undef CHILD_CORNER_SAFE
 232
 233                 if (ops->recalculate_stats)
 234                         ops->recalculate_stats(node);
 235
 236                 node = node->parent;
 237         }
 238 }
 239
 240 /**
 241  * quadtree_add - add a node to a quadtree
 242  * @parent: parent node
 243  * @new: the new node to be added
 244  *
 245  * Add a node to a quadtree. The tree is kept in order, the new node
 246  * is placed in the end of appropriate branch.
 247  *
 248  * The case of nodes sharing identical coordinates is not taken into
 249  * account at all.
 250  */
 251
 252 struct quadtree *quadtree_add(struct quadtree *parent, struct quadtree *new,
 253                               struct quadtree_ops *ops)
 254 {
 255         if (parent == new)
 256                 trap();
 257
 258         validate_tree(parent);
 259
 260         _quadtree_add(parent, new);
 261
 262         if (debug) {
 263                 printf("adding node %p to parent %p\n", new, parent);
 264                 fflush(stdout);
 265         }
 266
 267         quadtree_recalculate_parent_stats(new, ops);
 268
 269         validate_tree(new);
 270
 271         return new;
 272 }
 273
 274 static double max_by_dir(double a, double b, int direction)
 275 {
 276         switch (direction) {
 277         case 0: /* up */
 278         case 3: /* left */
 279                 return MIN(a, b);
 280         case 1: /* right */
 281         case 2: /* down */
 282                 return MAX(a, b);
 283         }
 284
 285         trap();
 286         return 0;
 287 }
 288
 289 static double maxv_by_dir(struct quadtree *a, int direction)
 290 {
 291         switch (direction) {
 292         case 0: /* up */
 293         case 2: /* down */
 294                 return a->pos.y;
 295         case 1: /* right */
 296         case 3: /* left */
 297                 return a->pos.x;
 298         }
 299
 300         trap();
 301         return 0;
 302 }
 303
 304 static double quadtree_get_max_dimension(struct quadtree *node, int direction)
 305 {
 306         struct quadtree *ch1 = NULL, *ch2 = NULL;
 307         double a, b;
 308
 309         switch (direction) {
 310         case 0: /* up */
 311                 ch1 = node->child[0];
 312                 ch2 = node->child[1];
 313                 break;
 314         case 1: /* right */
 315                 ch1 = node->child[1];
 316                 ch2 = node->child[3];
 317                 break;
 318         case 2: /* down */
 319                 ch1 = node->child[2];
 320                 ch2 = node->child[3];
 321                 break;
 322         case 3: /* left */
 323                 ch1 = node->child[0];
 324                 ch2 = node->child[2];
 325                 break;
 326         default:
 327                 trap();
 328         }
 329
 330         if (ch1 && ch2) {
 331                 a = quadtree_get_max_dimension(ch1, direction);
 332                 b = quadtree_get_max_dimension(ch2, direction);
 333                 return max_by_dir(a, b, direction);
 334         }
 335
 336         if (ch1) {
 337                 a = quadtree_get_max_dimension(ch1, direction);
 338                 b = maxv_by_dir(ch1, direction);
 339                 return max_by_dir(a, b, direction);
 340         }
 341         if (ch2) {
 342                 a = quadtree_get_max_dimension(ch2, direction);
 343                 b = maxv_by_dir(ch2, direction);
 344                 return max_by_dir(a, b, direction);
 345         }
 346
 347         return maxv_by_dir(node, direction);
 348 }
 349
 350 /*
 351  * Stores the lower right and upper left corner coordinates to the
 352  * corner vectors
 353  */
 354 static void quadtree_get_tree_dimensions(struct quadtree *node,
 355                                         struct vector *corner)
 356 {
 357         corner[0].y = quadtree_get_max_dimension(node, 2);
 358         corner[0].x = quadtree_get_max_dimension(node, 1);
 359         corner[1].y = quadtree_get_max_dimension(node, 0);
 360         corner[1].x = quadtree_get_max_dimension(node, 3);
 361
 362         if (debug) {
 363                 printf("\nInitial Search rectangle (%f %f) (%f %f), (%f %f)\n",
 364                         corner[0].x, corner[0].y,
 365                         corner[1].x, corner[1].y,
 366                         node->pos.x, node->pos.y);
 367                 fflush(stdout);
 368         }
 369
 370         if ((corner[0].x < corner[1].x) ||
 371             (corner[0].y < corner[1].y))
 372                 trap();
 373 }
 374
 375 static int is_within(struct vector *pos, struct vector *corner)
 376 {
 377         if ((pos->x >= corner[1].x) && (pos->x <= corner[0].x) &&
 378             (pos->y >= corner[1].y) && (pos->y <= corner[0].y))
 379                 return 1;
 380         return 0;
 381 }
 382
 383 static int quadrants_to_search(struct quadtree *node, struct vector *corner)
 384 {
 385         int direction = 0, i;
 386         int up = 0, left = 0, right = 0, down = 0;
 387
 388         for (i = 0; i < 2; i++) {
 389                 if (corner[i].x <= node->pos.x)
 390                         left = 1;
 391                 else if (corner[i].x >= node->pos.x)
 392                         right = 1;
 393                 if (corner[i].y <= node->pos.y)
 394                         up = 1;
 395                 else if (corner[i].y >= node->pos.y)
 396                         down = 1;
 397         }
 398
 399         if (left || up)
 400                 direction |= QUADTREE_UPLEFT;
 401         if (right || up)
 402                 direction |= QUADTREE_UPRIGHT;
 403         if (left || down)
 404                 direction |= QUADTREE_DOWNLEFT;
 405         if (right || down)
 406                 direction |= QUADTREE_DOWNRIGHT;
 407
 408         return direction;
 409 }
 410
 411 static struct quadtree *quadtree_find_nearest(struct quadtree *tree,
 412                                         struct vector *pos,
 413                                         struct vector *corner)
 414 {
 415         struct vector tmp;
 416         struct quadtree *nearest, *node;
 417         double distance = 0, dist;
 418         int i, directions;
 419
 420         if (!is_within(pos, corner)) {
 421                 if (debug) {
 422                         printf("No nearest to be found from (%f %f) (%f %f) "
 423                                 "pos (%f %f)\n",
 424                                 corner[0].x, corner[0].y,
 425                                 corner[1].x, corner[1].y,
 426                                 pos->x, pos->y);
 427                         fflush(stdout);
 428                 }
 429                 return NULL;
 430         }
 431
 432         if (is_within(&tree->pos, corner)) {
 433                 vector_sub(pos, &tree->pos, &tmp);
 434                 distance = vector_abs(&tmp);
 435                 nearest = tree;
 436         } else {
 437                 nearest = NULL;
 438         }
 439
 440         directions = quadrants_to_search(tree, corner);
 441
 442         for (i = 0; i < 4; i++) {
 443                 if (!tree->child[i])
 444                         continue;
 445
 446 /*
 447                 if (!(directions & (1 << i)))
 448                         continue;
 449 */
 450                 node = quadtree_find_nearest(tree->child[i], pos, corner);
 451
 452                 if (!node)
 453                         continue;
 454
 455                 vector_sub(pos, &node->pos, &tmp);
 456                 dist = vector_abs(&tmp);
 457
 458                 if (!nearest || dist < distance) {
 459                         nearest = node;
 460                         distance = dist;
 461
 462                 }
 463         }
 464
 465         if (nearest && !is_within(&nearest->pos, corner)) {
 466                 if (debug)
 467                         printf("Node %p (%f %f) is not within "
 468                                 "search window (%f %f) (%f %f)\n",
 469                                 nearest, nearest->pos.x, nearest->pos.y,
 470                                 corner[0].x, corner[0].y,
 471                                 corner[1].x, corner[1].y);
 472                 return NULL;
 473         }
 474
 475         return nearest;
 476 }
 477
 478 static struct quadtree *quadtree_find_nearest_noparent(struct quadtree *tree,
 479                                                 struct vector *pos,
 480                                                 struct vector *corner)
 481 {
 482         struct quadtree *nearest;
 483         struct vector sub;
 484         struct quadtree *node;
 485         double dist = 0, dist2;
 486         int i;
 487
 488         nearest = quadtree_find_nearest(tree, pos, corner);
 489
 490         if (nearest != tree)
 491                 goto out;
 492
 493         if (debug)
 494                 printf("Avoiding parent or NULL node %p\n", nearest);
 495
 496         /*
 497          * oops, we don't want to pick the parent node, let's choose
 498          * another one
 499          */
 500         nearest = NULL;
 501         for (i = 0; i < 4; i++) {
 502                 if (!tree->child[i])
 503                         continue;
 504
 505                 if (!nearest) {
 506                         nearest = quadtree_find_nearest(tree->child[i], pos,
 507                                                         corner);
 508
 509                         if (nearest == NULL)
 510                                 continue;
 511
 512                         vector_sub(pos, &nearest->pos, &sub);
 513                         dist = vector_abs(&sub);
 514                         continue;
 515                 }
 516                 node = quadtree_find_nearest(tree->child[i],
 517                                         pos, corner);
 518
 519                 if (node == NULL)
 520                         continue;
 521
 522                 vector_sub(pos, &node->pos, &sub);
 523                 dist2 = vector_abs(&sub);
 524
 525                 if (dist2 < dist) {
 526                         nearest = node;
 527                         dist = dist2;
 528                 }
 529         }
 530
 531 out:
 532         return nearest;
 533 }
 534
 535 static void get_middle_point(struct vector *corner, struct vector *middle)
 536 {
 537         middle->x = (corner[0].x + corner[1].x) / (double)2;
 538         middle->y = (corner[0].y + corner[1].y) / (double)2;
 539 }
 540
 541 static int quadtree_split_by_node(struct quadtree *node,
 542                                 struct vector *corner, int dir)
 543 {
 544         if (!is_within(&node->pos, corner)) {
 545                 if (debug)
 546                         printf("Not within search rectangle\n");
 547                 return 0;
 548         }
 549
 550         switch (dir) {
 551         case QUADTREE_UPLEFT:
 552                 corner[0] = node->pos;
 553                 break;
 554         case QUADTREE_UPRIGHT:
 555                 corner[0].y = node->pos.y;
 556                 corner[1].x = node->pos.x;
 557                 break;
 558         case QUADTREE_DOWNRIGHT:
 559                 corner[1] = node->pos;
 560                 break;
 561         case QUADTREE_DOWNLEFT:
 562                 corner[0].x = node->pos.x;
 563                 corner[1].y = node->pos.y;
 564                 break;
 565         default:
 566                 trap();
 567         }
 568
 569         if (debug)
 570                 printf("Search rectangle is now (%f %f) (%f %f), (%f %f)\n",
 571                         corner[0].x, corner[0].y,
 572                         corner[1].x, corner[1].y,
 573                         node->pos.x, node->pos.y);
 574
 575         if ((corner[0].x < corner[1].x) ||
 576             (corner[0].y < corner[1].y))
 577                 trap();
 578
 579         return 1;
 580 }
 581
 582 /*
 583  * Quickly detach a node from a tree. Move all child nodes under
 584  * @parent node.
 585  */
 586 static int _quadtree_del(struct quadtree *node, struct quadtree *parent)
 587 {
 588         struct quadtree *n;
 589         int i;
 590
 591         /* Detach from the tree */
 592         if (node->parent)
 593                 for (i = 0; i < 4; i++) {
 594                         if (node->parent->child[i] == node) {
 595                                 node->parent->child[i] = 0;
 596                                 break;
 597                         }
 598                 }
 599
 600         if (i == 4)
 601                 trap();
 602
 603
 604         for (i = 0; i < 4; i++) {
 605                 if (!node->child[i])
 606                         continue;
 607
 608                 n = node->child[i];
 609                 _quadtree_del(n, parent);
 610                 _quadtree_add(parent, n);
 611         }
 612
 613         return 0;
 614 }
 615
 616 /**
 617  * Move everything under @tree node to @parent node, everything
 618  * else except the @tree node itself
 619  */
 620 static int optimally_move_tree(struct quadtree *tree, struct quadtree *parent,
 621                         struct vector *corner_orig,
 622                         struct quadtree_ops *ops)
 623 {
 624         struct vector corner[2], mid;
 625         struct quadtree *t, *tmp;
 626         int moved = 0;
 627
 628         get_middle_point(corner_orig, &mid);
 629         t = quadtree_find_nearest_noparent(tree, &mid, corner_orig);
 630
 631         if (!t) {
 632                 if (debug)
 633                         printf("Cannot find nearest node\n");
 634
 635                 goto out;
 636         }
 637
 638         /*
 639          * Now we have the t node which contains the object of the
 640          * spatial middle coordinates of the tree.
 641          */
 642
 643         if (debug) {
 644                 printf("Relocating node %p (%f %f) under parent %p\n", t,
 645                         t->pos.x, t->pos.y, parent);
 646                 printf("There are %ld child nodes left\n", tree->children);
 647                 fflush(stdout);
 648         }
 649         _quadtree_del(t, tree);
 650         quadtree_add(parent, t, ops);
 651
 652         moved++;
 653         tree->children--;
 654         if (!tree->children)
 655                 goto out;
 656
 657         /*
 658          * Now split the search rectangle in quadtres and do the same
 659          * with all of the quarters.
 660          */
 661
 662         corner[0] = corner_orig[0];
 663         corner[1] = corner_orig[1];
 664         if (quadtree_split_by_node(t, corner, QUADTREE_UPLEFT))
 665                 moved += optimally_move_tree(tree, parent, corner, ops);
 666
 667         corner[0] = corner_orig[0];
 668         corner[1] = corner_orig[1];
 669         if (quadtree_split_by_node(t, corner, QUADTREE_UPRIGHT))
 670                 moved += optimally_move_tree(tree, parent, corner, ops);
 671
 672         corner[0] = corner_orig[0];
 673         corner[1] = corner_orig[1];
 674         if (quadtree_split_by_node(t, corner, QUADTREE_DOWNLEFT))
 675                 moved += optimally_move_tree(tree, parent, corner, ops);
 676
 677         corner[0] = corner_orig[0];
 678         corner[1] = corner_orig[1];
 679         if (quadtree_split_by_node(t, corner, QUADTREE_DOWNRIGHT))
 680                 moved += optimally_move_tree(tree, parent, corner, ops);
 681
 682         get_middle_point(corner_orig, &mid);
 683         tmp = quadtree_find_nearest(tree, &mid, corner_orig);
 684         if (tmp && tmp != tree)
 685                 trap();
 686
 687 out:
 688         if (debug)
 689                 printf("Now moved %d nodes, %ld left\n", moved, tree->children);
 690
 691         return moved;
 692 }
 693
 694 /*
 695  * quadtree_del - Detach a node from the tree
 696  *
 697  * Return value: The new root node of the tree. If we are detaching
 698  * anything but the root node of the entire tree, the returned root
 699  * value will be the original root of the tree.
 700  */
 701 struct quadtree *quadtree_del(struct quadtree *node,
 702                               struct quadtree_ops *ops)
 703 {
 704         struct quadtree *parent = NULL;
 705         struct vector corner[2];
 706         int i, children = node->children, moved;
 707
 708         validate_tree(node);
 709
 710         if (debug) {
 711                 printf("Deleting node %p under parent %p\n",
 712                                node, node->parent);
 713                 printf("Relocating %ld children\n", node->children);
 714                 fflush(stdout);
 715         }
 716
 717         if (!node->parent) {
 718                 /*
 719                  * We are deleting the root node. This means we have
 720                  * to select a new root node and reconstruct the
 721                  * entire tree under it again.
 722                  */
 723                 if (debug) {
 724                         printf("Deleting root node\n");
 725                         fflush(stdout);
 726                 }
 727                 for (i = 0; i < 4; i++) {
 728                         if (!node->child[i])
 729                                 continue;
 730
 731                         parent = node->child[i];
 732                         _quadtree_del(parent, node);
 733                         parent->parent = 0;
 734                         quadtree_recalculate_parent_stats(parent, ops);
 735                         break;
 736                 }
 737         } else {
 738                 /*
 739                  * The node has a parent. Detach the node from it and
 740                  * relocate the children.
 741                  */
 742
 743                 for (i = 0; i < 4; i++) {
 744                         if (node->parent->child[i] == node) {
 745                                 node->parent->child[i] = 0;
 746                                 break;
 747                         }
 748                 }
 749                 if (i == 4)
 750                         trap();
 751
 752                 parent = node->parent;
 753
 754                 /*
 755                  * The sub branch is now detached from the main
 756                  * tree. Fix the stats.
 757                  */
 758                 quadtree_recalculate_parent_stats(node->parent, ops);
 759         }
 760
 761         validate_tree(parent);
 762
 763         if (!node->children)
 764                 goto out;
 765         /*
 766          * Now we are ready to prepare for relocating the nodes under
 767          * the parent.
 768          */
 769
 770         quadtree_get_tree_dimensions(node, corner);
 771         moved = optimally_move_tree(node, parent, corner, ops);
 772
 773         if (debug) {
 774                 if (moved != children) {
 775                         printf("Got %d children but %d were moved\n",
 776                                 children, moved);
 777                         printf("nearest children left:\n");
 778                         for (i = 0 ; i < 4; i++)
 779                                 if (node->child[i])
 780                                         printf("   node %d %p, (%f %f)\n",
 781                                                 i, node->child[i],
 782                                                 node->child[i]->pos.x,
 783                                                 node->child[i]->pos.y);
 784                         fflush(stdout);
 785                         trap();
 786                 }
 787                 printf("Delete done, returning parent %p\n", parent);
 788                 fflush(stdout);
 789         }
 790
 791 out:
 792         validate_tree(parent);
 793         return quadtree_find_parent(parent);
 794 }
 795
 796 static void check_for_crossed_subnodes(struct quadtree *node,
 797                                 struct vector *limit, struct quadtree_ops *ops)
 798 {
 799         int direction = 0, i;
 800         int up = 0, left = 0, right = 0, down = 0;
 801
 802         for (i = 0; i < 2; i++) {
 803                 if (limit[i].x < node->pos.x)
 804                         left = 1;
 805                 else
 806                         right = 1;
 807                 if (limit[i].y < node->pos.y)
 808                         up = 1;
 809                 else
 810                         down = 1;
 811         }
 812
 813         if (left || up)
 814                 direction |= QUADTREE_UPLEFT;
 815         if (right || up)
 816                 direction |= QUADTREE_UPRIGHT;
 817         if (left || down)
 818                 direction |= QUADTREE_DOWNLEFT;
 819         if (right || down)
 820                 direction |= QUADTREE_DOWNRIGHT;
 821         if ((left && right) || (up && down))
 822                 direction |= QUADTREE_SELF;
 823
 824         if ((direction & QUADTREE_UPLEFT) && node->child[0])
 825                 check_for_crossed_subnodes(node->child[0], limit, ops);
 826
 827         if ((direction & QUADTREE_UPRIGHT) && node->child[1])
 828                 check_for_crossed_subnodes(node->child[0], limit, ops);
 829
 830         if ((direction & QUADTREE_DOWNLEFT) && node->child[2])
 831                 check_for_crossed_subnodes(node->child[0], limit, ops);
 832
 833         if ((direction & QUADTREE_DOWNRIGHT) && node->child[3])
 834                 check_for_crossed_subnodes(node->child[0], limit, ops);
 835
 836         if (direction & QUADTREE_SELF) {
 837                 struct quadtree *parent;
 838
 839                 parent = quadtree_del(node, ops);
 840                 quadtree_add(parent, node, ops);
 841         }
 842 }
 843
 844 struct quadtree *quadtree_move(struct quadtree *node, struct vector new_pos,
 845                         struct quadtree_ops *ops)
 846 {
 847         struct quadtree *parent, *tree_parent;
 848         int modify;
 849
 850         /* Check if we have crossed any of the parents */
 851         parent = node->parent;
 852         while (parent) {
 853                 if (node->pos.x < parent->pos.x && new_pos.x > parent->pos.x)
 854                         modify = 1;
 855                 if (node->pos.x > parent->pos.x && new_pos.x < parent->pos.x)
 856                         modify = 1;
 857                 if (node->pos.y < parent->pos.y && new_pos.y > parent->pos.y)
 858                         modify = 1;
 859                 if (node->pos.y > parent->pos.y && new_pos.y < parent->pos.y)
 860                         modify = 1;
 861
 862                 if (!modify) {
 863                         parent = parent->parent;
 864                         continue;
 865                 }
 866
 867                 /*
 868                  * If the node has crossed the boundaries, remove it
 869                  * from the tree and add it again to it. It is then
 870                  * guaranteed to be in the correct position of the
 871                  * tree.
 872                  */
 873                 tree_parent = quadtree_del(node, ops);
 874                 node->pos = new_pos;
 875                 quadtree_add(tree_parent, node, ops);
 876                 return tree_parent;
 877         }
 878
 879         /* Move the node into its new location */
 880         node->pos = new_pos;
 881
 882         if(node->children) {
 883                 /*
 884                  * Now, search the subtree for any children that are
 885                  * located in wrong place and move them into correct
 886                  * place within the tree.
 887                  */
 888                 struct vector limit[2];
 889
 890                 limit[0] = node->pos;
 891                 limit[1] = new_pos;
 892                 check_for_crossed_subnodes(node, limit, ops);
 893         }
 894
 895         return quadtree_find_parent(node);
 896
 897 }
 898
 899 static int _walk_tree(struct quadtree *head, const struct quadtree_iterator *it)
 900 {
 901         int direction, count = 0;
 902
 903         direction = it->direction(head, (struct quadtree_iterator *)it);
 904
 905         if ((direction & QUADTREE_UPLEFT) && head->child[0])
 906                 count += _walk_tree(head->child[0], it);
 907
 908         if ((direction & QUADTREE_UPRIGHT) && head->child[1])
 909                 count += _walk_tree(head->child[1], it);
 910
 911         if ((direction & QUADTREE_DOWNLEFT) && head->child[2])
 912                 count += _walk_tree(head->child[2], it);
 913
 914         if ((direction & QUADTREE_DOWNRIGHT) && head->child[3])
 915                 count += _walk_tree(head->child[3], it);
 916
 917         if ((direction & QUADTREE_SELF) && it->callback) {
 918                 it->callback(head, (struct quadtree_iterator *)it);
 919                 count++;
 920         }
 921
 922         return count;
 923 }
 924
 925 int walk_quadtree(const struct quadtree_iterator *it)
 926 {
 927         return _walk_tree(it->head, it);
 928 }