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 /**
 103  * quadtree_add - add a node to a quadtree
 104  * @parent: parent node
 105  * @new: the new node to be added
 106  * @compare: pointer to a comparison function
 107  *
 108  * Add a node to a quadtree. The tree is kept in order, the new node
 109  * is placed in the end of appropriate branch. The compare function is
 110  * used to compare the new node against a branch in the tree. The
 111  * comparison function must have following return value depending on
 112  * the position of node a compared to the position of node b:
 113  *
 114  * 0: upper left
 115  * 1: upper right
 116  * 2: lower left
 117  * 3: lower right
 118  *
 119  * The case of nodes sharing identical coordinates is not taken into
 120  * account at all.
 121  */
 122
 123 struct quadtree *quadtree_add(struct quadtree *parent, struct quadtree *new,
 124                               struct quadtree_ops *ops)
 125 {
 126         int ret, i;
 127         if (parent == new)
 128                 trap();
 129
 130         validate_tree(parent);
 131
 132         ret = ops->compare(parent, new);
 133
 134         if (ret < 0 || ret >= 4) {
 135                 printf("Invalid comparison result of %d\n", ret);
 136                 return 0;
 137         }
 138
 139         if (parent->child[ret])
 140                 return quadtree_add(parent->child[ret], new, ops);
 141
 142         parent->child[ret] = new;
 143         new->parent = parent;
 144         for (i = 0; i < 4; i++)
 145                 new->child[i] = 0;
 146
 147         if (debug) {
 148                 printf("adding node %p to parent %p\n", new, parent);
 149                 fflush(stdout);
 150         }
 151
 152         quadtree_recalculate_parent_stats(new, ops);
 153
 154         validate_tree(new);
 155
 156         return new;
 157 }
 158
 159 /*
 160  * _quadtree_reposition_reqursively - Move everything under and
 161  * including a given node under the new root node
 162  */
 163
 164 static void
 165 _quadtree_reposition_reqursively(struct quadtree *root,
 166                                  struct quadtree *node,
 167                                  struct quadtree_ops *ops)
 168 {
 169         int i;
 170
 171         validate_tree(root);
 172
 173         /* First remove all children, if any */
 174
 175         for (i = 0; i < 4; i++) {
 176                 if (!node->child[i])
 177                         continue;
 178
 179                 if (node->child[i] == node ||
 180                     node->child[i] == node->parent)
 181                         trap();
 182
 183                 _quadtree_reposition_reqursively(root, node->child[i], ops);
 184                 node->child[i] = 0;
 185         }
 186
 187         /* There are no children left, reset stats */
 188         node->depth = 0;
 189         node->children = 0;
 190
 191         /* Then remove this node under the new root. */
 192         quadtree_add(root, node, ops);
 193 }
 194
 195 /*
 196  * quadtree_del - Detach a node from the tree
 197  *
 198  * Return value: The new root node of the tree. If we are detaching
 199  * anything but the root node of the entire tree, the returned root
 200  * value will be the original root of the tree.
 201  */
 202 struct quadtree *quadtree_del(struct quadtree *node,
 203                               struct quadtree_ops *ops)
 204 {
 205         struct quadtree *parent = 0;
 206         int i;
 207
 208         if (debug) {
 209                 printf("Deleting node %p under parent %p\n",
 210                                node, node->parent);
 211                 fflush(stdout);
 212         }
 213         /*
 214          * We are deleting the root node. This means we have to select
 215          * a new root node and reconstruct the entire tree under it
 216          * again.
 217          */
 218         if (!node->parent) {
 219                 for (i = 0; i < 4; i++) {
 220                         if (!node->child[i])
 221                                 continue;
 222
 223                         if (!parent) {
 224                                 parent = node->child[i];
 225                                 parent->parent = 0;
 226                                 continue;
 227                         }
 228                         _quadtree_reposition_reqursively(parent,
 229                                                          node->child[i],
 230                                                          ops);
 231                         node->child[i] = 0;
 232                 }
 233
 234                 return parent;
 235         }
 236
 237         /*
 238          * We are not deleting the parent. Detach the node from the
 239          * parent abd relocate the children. The node will be then
 240          * detached from the tree.
 241          */
 242
 243         for (i = 0; i < 4; i++) {
 244                 if (node->parent->child[i] == node) {
 245                         node->parent->child[i] = 0;
 246                         break;
 247                 }
 248         }
 249         if (i == 4) {
 250                 printf("%s:%d Fatal! Tree inconsistency detected\n",
 251                        __func__, __LINE__);
 252                 trap();
 253         }
 254
 255         /* Fix parent stats */
 256         quadtree_recalculate_parent_stats(node->parent, ops);
 257
 258         /*
 259          * The sub branch is now detached from the main tree. Continue
 260          * relocating the detached branch.
 261          */
 262
 263         for (i = 0; i < 4; i++) {
 264                 if (!node->child[i])
 265                         continue;
 266
 267                 _quadtree_reposition_reqursively(node->parent, node->child[i],
 268                                                  ops);
 269                 node->child[i] = 0;
 270         }
 271
 272         parent = quadtree_find_parent(node);
 273         node->parent = 0;
 274
 275         node->children = 0;
 276         node->depth = 0;
 277
 278         validate_tree(parent);
 279         return parent;
 280 }
 281
 282
 283 static int _walk_tree(struct quadtree *head, const struct quadtree_iterator *it)
 284 {
 285         int direction, count = 0;
 286
 287         direction = it->direction(head, (struct quadtree_iterator *)it);
 288
 289         if ((direction & QUADTREE_UPLEFT) && head->child[0])
 290                 count += _walk_tree(head->child[0], it);
 291
 292         if ((direction & QUADTREE_UPRIGHT) && head->child[1])
 293                 count += _walk_tree(head->child[1], it);
 294
 295         if ((direction & QUADTREE_DOWNLEFT) && head->child[2])
 296                 count += _walk_tree(head->child[2], it);
 297
 298         if ((direction & QUADTREE_DOWNRIGHT) && head->child[3])
 299                 count += _walk_tree(head->child[3], it);
 300
 301         if ((direction & QUADTREE_SELF) && it->callback) {
 302                 it->callback(head, (struct quadtree_iterator *)it);
 303                 count++;
 304         }
 305
 306         return count;
 307 }
 308
 309 int walk_quadtree(const struct quadtree_iterator *it)
 310 {
 311         return _walk_tree(it->head, it);
 312 }