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 }
 170
 171 /**
 172  * quadtree_add - add a node to a quadtree
 173  * @parent: parent node
 174  * @new: the new node to be added
 175  *
 176  * Add a node to a quadtree. The tree is kept in order, the new node
 177  * is placed in the end of appropriate branch.
 178  *
 179  * The case of nodes sharing identical coordinates is not taken into
 180  * account at all.
 181  */
 182
 183 struct quadtree *quadtree_add(struct quadtree *parent, struct quadtree *new,
 184                               struct quadtree_ops *ops)
 185 {
 186         int ret, i;
 187         if (parent == new)
 188                 trap();
 189
 190         validate_tree(parent);
 191
 192         ret = quadtree_compare(parent, new);
 193
 194         if (ret < 0 || ret >= 4) {
 195                 printf("Invalid comparison result of %d\n", ret);
 196                 return 0;
 197         }
 198
 199         if (parent->child[ret])
 200                 return quadtree_add(parent->child[ret], new, ops);
 201
 202         parent->child[ret] = new;
 203         new->parent = parent;
 204         for (i = 0; i < 4; i++)
 205                 new->child[i] = 0;
 206
 207         if (debug) {
 208                 printf("adding node %p to parent %p\n", new, parent);
 209                 fflush(stdout);
 210         }
 211
 212         quadtree_recalculate_parent_stats(new, ops);
 213
 214         validate_tree(new);
 215
 216         return new;
 217 }
 218
 219 /*
 220  * _qt_optimal_del - Delete nodes from a detached subtree in optimal order
 221  *
 222  * All nodes are moved under the new parent node
 223  */
 224 static void _qt_optimal_del(struct quadtree *parent, struct quadtree *node,
 225                        struct quadtree_ops *ops, int parents)
 226 {
 227         struct quadtree *child[4] = {0};
 228         int i, children = 0, max_c, max_i;
 229
 230         for (i = 0; i < 4; i++) {
 231                 if (!node->child[i]) {
 232                         child[i] = 0;
 233                         continue;
 234                 }
 235
 236                 child[children] = node->child[i];
 237                 node->child[i] = NULL;
 238                 children++;
 239         }
 240
 241         /*
 242          * Try to remove the nodes in such order that the node which
 243          * has child number one quarter of the parent's child numer
 244          * gets deleted first.
 245          */
 246
 247         if (node->children <= parents / 4) {
 248                 if (debug) {
 249                         printf("%d: Moving node %p away from parent %p\n",
 250                                __LINE__, node, parent);
 251                         fflush(stdout);
 252                 }
 253                 validate_tree(parent);
 254                 quadtree_add(parent, node, ops);
 255                 node = NULL;
 256                 parents /= 4;
 257         }
 258
 259         while(children) {
 260                 max_c = -1;
 261                 max_i = -1;
 262                 for (i = 0; i < 4; i++) {
 263                         if (!child[i])
 264                                 continue;
 265
 266                         if (max_c < child[i]->children) {
 267                                 max_c = child[i]->children;
 268                                 max_i = i;
 269                         }
 270                 }
 271
 272                 _qt_optimal_del(parent, child[max_i], ops, parents / 4);
 273                 child[max_i] = 0;
 274                 children--;
 275         }
 276
 277         if (node) {
 278                 validate_tree(parent);
 279                 if (debug) {
 280                         printf("%d: Moving node %p away from parent %p\n",
 281                                __LINE__, node, parent);
 282                         fflush(stdout);
 283                 }
 284                 quadtree_add(parent, node, ops);
 285         }
 286 }
 287
 288 /*
 289  * quadtree_del - Detach a node from the tree
 290  *
 291  * Return value: The new root node of the tree. If we are detaching
 292  * anything but the root node of the entire tree, the returned root
 293  * value will be the original root of the tree.
 294  */
 295 struct quadtree *quadtree_del(struct quadtree *node,
 296                               struct quadtree_ops *ops)
 297 {
 298         struct quadtree *parent = 0;
 299         int i;
 300
 301         if (debug) {
 302                 printf("Deleting node %p under parent %p\n",
 303                                node, node->parent);
 304                 fflush(stdout);
 305         }
 306         /*
 307          * We are deleting the root node. This means we have to select
 308          * a new root node and reconstruct the entire tree under it
 309          * again.
 310          */
 311         if (!node->parent) {
 312                 for (i = 0; i < 4; i++) {
 313                         if (!node->child[i])
 314                                 continue;
 315
 316                         if (!parent) {
 317                                 parent = node->child[i];
 318                                 parent->parent = 0;
 319                                 continue;
 320                         }
 321                         _qt_optimal_del(parent, node->child[i], ops,
 322                                         node->children);
 323                         node->child[i] = 0;
 324                 }
 325
 326                 return parent;
 327         }
 328
 329         /*
 330          * We are not deleting the parent. Detach the node from the
 331          * parent abd relocate the children. The node will be then
 332          * detached from the tree.
 333          */
 334
 335         for (i = 0; i < 4; i++) {
 336                 if (node->parent->child[i] == node) {
 337                         node->parent->child[i] = 0;
 338                         break;
 339                 }
 340         }
 341         if (i == 4)
 342                 trap();
 343
 344         parent = node->parent;
 345
 346         /*
 347          * The sub branch is now detached from the main tree. Fix the
 348          * stats.
 349          */
 350         quadtree_recalculate_parent_stats(node->parent, ops);
 351
 352         parent = quadtree_find_parent(parent);
 353
 354         if (node->children) {
 355                 for (i = 0; i < 4; i++) {
 356                         if (!node->child[i])
 357                                 continue;
 358
 359                         _qt_optimal_del(parent, node->child[i], ops,
 360                                         node->children);
 361                 }
 362         }
 363
 364         validate_tree(parent);
 365         return parent;
 366 }
 367
 368
 369 static int _walk_tree(struct quadtree *head, const struct quadtree_iterator *it)
 370 {
 371         int direction, count = 0;
 372
 373         direction = it->direction(head, (struct quadtree_iterator *)it);
 374
 375         if ((direction & QUADTREE_UPLEFT) && head->child[0])
 376                 count += _walk_tree(head->child[0], it);
 377
 378         if ((direction & QUADTREE_UPRIGHT) && head->child[1])
 379                 count += _walk_tree(head->child[1], it);
 380
 381         if ((direction & QUADTREE_DOWNLEFT) && head->child[2])
 382                 count += _walk_tree(head->child[2], it);
 383
 384         if ((direction & QUADTREE_DOWNRIGHT) && head->child[3])
 385                 count += _walk_tree(head->child[3], it);
 386
 387         if ((direction & QUADTREE_SELF) && it->callback) {
 388                 it->callback(head, (struct quadtree_iterator *)it);
 389                 count++;
 390         }
 391
 392         return count;
 393 }
 394
 395 int walk_quadtree(const struct quadtree_iterator *it)
 396 {
 397         return _walk_tree(it->head, it);
 398 }