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