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;
  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                 children += node->child[i]->children + 1;
  82
  83                 validate_subtree(node->child[i]);
  84         }
  85
  86         if (node->depth < 0) {
  87                 printf("%s:%d Tree statistics inconsistency detected! "
  88                        "Negative depth: %ld\n",
  89                        __func__, __LINE__, node->depth);
  90         }
  91
  92         if (node->children != children) {
  93                 printf("%s:%d Tree statistics inconsistency detected! "
  94                        "child count mismatch. Expected %ld, got %ld\n",
  95                        __func__, __LINE__, children, node->children);
  96                 fflush(stdout);
  97                 trap();
  98         }
  99
 100         for (i = 0; i < 4 && node->children; i++) {
 101                 if (!node->child[i])
 102                         continue;
 103
 104                 if (node->depth == node->child[i]->depth + 1)
 105                         break;
 106
 107                 if (node->child[i]->depth > node->depth) {
 108                         printf("%s:%d Tree statistics inconsistency detected! "
 109                                "child depth mismatch %ld > %ld\n",
 110                                __func__, __LINE__, node->child[i]->depth,
 111                                node->depth);
 112                 }
 113         }
 114
 115         if (i == 4) {
 116                 printf("%s:%d Tree statistics inconsistency detected! "
 117                        "child depth mismatch.",
 118                        __func__, __LINE__);
 119                 fflush(stdout);
 120                 trap();
 121         }
 122 }
 123
 124 static void validate_tree(const struct quadtree *node)
 125 {
 126         if (debug)
 127                 validate_subtree(quadtree_find_parent(node));
 128 }
 129
 130 /**
 131  * quadtree_add - add a node to a quadtree
 132  * @parent: parent node
 133  * @new: the new node to be added
 134  *
 135  * Add a node to a quadtree. The tree is kept in order, the new node
 136  * is placed in the end of appropriate branch.
 137  *
 138  * The case of nodes sharing identical coordinates is not taken into
 139  * account at all.
 140  */
 141
 142 struct quadtree *quadtree_add(struct quadtree *parent, struct quadtree *new,
 143                               struct quadtree_ops *ops)
 144 {
 145         int ret, i;
 146         if (parent == new)
 147                 trap();
 148
 149         validate_tree(parent);
 150
 151         ret = quadtree_compare(parent, new);
 152
 153         if (ret < 0 || ret >= 4) {
 154                 printf("Invalid comparison result of %d\n", ret);
 155                 return 0;
 156         }
 157
 158         if (parent->child[ret])
 159                 return quadtree_add(parent->child[ret], new, ops);
 160
 161         parent->child[ret] = new;
 162         new->parent = parent;
 163         for (i = 0; i < 4; i++)
 164                 new->child[i] = 0;
 165
 166         if (debug) {
 167                 printf("adding node %p to parent %p\n", new, parent);
 168                 fflush(stdout);
 169         }
 170
 171         quadtree_recalculate_parent_stats(new, ops);
 172
 173         validate_tree(new);
 174
 175         return new;
 176 }
 177
 178 /*
 179  * _qt_optimal_del - Delete nodes from a detached subtree in optimal order
 180  *
 181  * All nodes are moved under the new parent node
 182  */
 183 static void _qt_optimal_del(struct quadtree *parent, struct quadtree *node,
 184                        struct quadtree_ops *ops, int parents)
 185 {
 186         struct quadtree *child[4] = {0};
 187         int i, children = 0, max_c, max_i;
 188
 189         for (i = 0; i < 4; i++) {
 190                 if (!node->child[i]) {
 191                         child[i] = 0;
 192                         continue;
 193                 }
 194
 195                 child[children] = node->child[i];
 196                 node->child[i] = NULL;
 197                 children++;
 198         }
 199
 200         /*
 201          * Try to remove the nodes in such order that the node which
 202          * has child number one quarter of the parent's child numer
 203          * gets deleted first.
 204          */
 205
 206         if (node->children <= parents / 4) {
 207                 if (debug) {
 208                         printf("%d: Moving node %p away from parent %p\n",
 209                                __LINE__, node, parent);
 210                         fflush(stdout);
 211                 }
 212                 validate_tree(parent);
 213                 quadtree_add(parent, node, ops);
 214                 node = NULL;
 215                 parents /= 4;
 216         }
 217
 218         while(children) {
 219                 max_c = -1;
 220                 max_i = -1;
 221                 for (i = 0; i < 4; i++) {
 222                         if (!child[i])
 223                                 continue;
 224
 225                         if (max_c < child[i]->children) {
 226                                 max_c = child[i]->children;
 227                                 max_i = i;
 228                         }
 229                 }
 230
 231                 _qt_optimal_del(parent, child[max_i], ops, parents / 4);
 232                 child[max_i] = 0;
 233                 children--;
 234         }
 235
 236         if (node) {
 237                 validate_tree(parent);
 238                 if (debug) {
 239                         printf("%d: Moving node %p away from parent %p\n",
 240                                __LINE__, node, parent);
 241                         fflush(stdout);
 242                 }
 243                 quadtree_add(parent, node, ops);
 244         }
 245 }
 246
 247 /*
 248  * quadtree_del - Detach a node from the tree
 249  *
 250  * Return value: The new root node of the tree. If we are detaching
 251  * anything but the root node of the entire tree, the returned root
 252  * value will be the original root of the tree.
 253  */
 254 struct quadtree *quadtree_del(struct quadtree *node,
 255                               struct quadtree_ops *ops)
 256 {
 257         struct quadtree *parent = 0;
 258         int i;
 259
 260         if (debug) {
 261                 printf("Deleting node %p under parent %p\n",
 262                                node, node->parent);
 263                 fflush(stdout);
 264         }
 265         /*
 266          * We are deleting the root node. This means we have to select
 267          * a new root node and reconstruct the entire tree under it
 268          * again.
 269          */
 270         if (!node->parent) {
 271                 for (i = 0; i < 4; i++) {
 272                         if (!node->child[i])
 273                                 continue;
 274
 275                         if (!parent) {
 276                                 parent = node->child[i];
 277                                 parent->parent = 0;
 278                                 continue;
 279                         }
 280                         _qt_optimal_del(parent, node->child[i], ops,
 281                                         node->children);
 282                         node->child[i] = 0;
 283                 }
 284
 285                 return parent;
 286         }
 287
 288         /*
 289          * We are not deleting the parent. Detach the node from the
 290          * parent abd relocate the children. The node will be then
 291          * detached from the tree.
 292          */
 293
 294         for (i = 0; i < 4; i++) {
 295                 if (node->parent->child[i] == node) {
 296                         node->parent->child[i] = 0;
 297                         break;
 298                 }
 299         }
 300         if (i == 4)
 301                 trap();
 302
 303         parent = node->parent;
 304
 305         /*
 306          * The sub branch is now detached from the main tree. Fix the
 307          * stats.
 308          */
 309         quadtree_recalculate_parent_stats(node->parent, ops);
 310
 311         parent = quadtree_find_parent(parent);
 312
 313         if (node->children) {
 314                 for (i = 0; i < 4; i++) {
 315                         if (!node->child[i])
 316                                 continue;
 317
 318                         _qt_optimal_del(parent, node->child[i], ops,
 319                                         node->children);
 320                 }
 321         }
 322
 323         validate_tree(parent);
 324         return parent;
 325 }
 326
 327
 328 static int _walk_tree(struct quadtree *head, const struct quadtree_iterator *it)
 329 {
 330         int direction, count = 0;
 331
 332         direction = it->direction(head, (struct quadtree_iterator *)it);
 333
 334         if ((direction & QUADTREE_UPLEFT) && head->child[0])
 335                 count += _walk_tree(head->child[0], it);
 336
 337         if ((direction & QUADTREE_UPRIGHT) && head->child[1])
 338                 count += _walk_tree(head->child[1], it);
 339
 340         if ((direction & QUADTREE_DOWNLEFT) && head->child[2])
 341                 count += _walk_tree(head->child[2], it);
 342
 343         if ((direction & QUADTREE_DOWNRIGHT) && head->child[3])
 344                 count += _walk_tree(head->child[3], it);
 345
 346         if ((direction & QUADTREE_SELF) && it->callback) {
 347                 it->callback(head, (struct quadtree_iterator *)it);
 348                 count++;
 349         }
 350
 351         return count;
 352 }
 353
 354 int walk_quadtree(const struct quadtree_iterator *it)
 355 {
 356         return _walk_tree(it->head, it);
 357 }