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