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         for (i = 0; i < 4; i++) {
  25                 if (!node->child[i])
  26                         continue;
  27
  28                 if (node->child[i]->parent != node) {
  29                         printf("%s:%d Fatal! Tree inconsistency detected "
  30                                "at child %d in node %p\n",
  31                                __func__, __LINE__, i, node);
  32                         trap();
  33                 }
  34
  35                 if (node->parent) {
  36                         if (node->child[i] == node->parent) {
  37                                 printf("%s:%d Fatal! Tree loop detected "
  38                                        "at child %d in node %p\n",
  39                                        __func__, __LINE__, i, node);
  40                                 trap();
  41                         }
  42                 }
  43
  44                 children += node->child[i]->children + 1;
  45
  46                 validate_subtree(node->child[i]);
  47         }
  48
  49         if (node->depth < 0) {
  50                 printf("%s:%d Tree statistics inconsistency detected! "
  51                        "Negative depth: %ld\n",
  52                        __func__, __LINE__, node->depth);
  53         }
  54
  55         if (node->children != children) {
  56                 printf("%s:%d Tree statistics inconsistency detected! "
  57                        "child count mismatch. Expected %ld, got %ld\n",
  58                        __func__, __LINE__, children, node->children);
  59                 trap();
  60         }
  61
  62         for (i = 0; i < 4 && node->children; i++) {
  63                 if (!node->child[i])
  64                         continue;
  65
  66                 if (node->depth == node->child[i]->depth + 1)
  67                         break;
  68
  69                 if (node->child[i]->depth > node->depth) {
  70                         printf("%s:%d Tree statistics inconsistency detected! "
  71                                "child depth mismatch %ld > %ld\n",
  72                                __func__, __LINE__, node->child[i]->depth,
  73                                node->depth);
  74                 }
  75         }
  76
  77         if (i == 4) {
  78                 printf("%s:%d Tree statistics inconsistency detected! "
  79                        "child depth mismatch.",
  80                        __func__, __LINE__);
  81                 trap();
  82         }
  83 }
  84
  85 static void validate_tree(const struct quadtree *node)
  86 {
  87         if (debug)
  88                 validate_subtree(quadtree_find_parent(node));
  89 }
  90
  91 /**
  92  * quadtree_add - add a node to a quadtree
  93  * @parent: parent node
  94  * @new: the new node to be added
  95  * @compare: pointer to a comparison function
  96  *
  97  * Add a node to a quadtree. The tree is kept in order, the new node
  98  * is placed in the end of appropriate branch. The compare function is
  99  * used to compare the new node against a branch in the tree. The
 100  * comparison function must have following return value depending on
 101  * the position of node a compared to the position of node b:
 102  *
 103  * 0: upper left
 104  * 1: upper right
 105  * 2: lower left
 106  * 3: lower right
 107  *
 108  * The case of nodes sharing identical coordinates is not taken into
 109  * account at all.
 110  */
 111
 112 struct quadtree *quadtree_add(struct quadtree *parent,
 113                               struct quadtree *new,
 114                               int (*compare)(struct quadtree *a,
 115                                              struct quadtree *b))
 116 {
 117         int ret;
 118         if (parent == new)
 119                 trap();
 120
 121         validate_tree(parent);
 122
 123         ret = compare(parent, new);
 124
 125         if (ret < 0 || ret >= 4) {
 126                 printf("Invalid comparison result of %d\n", ret);
 127                 return 0;
 128         }
 129
 130         if (parent->child[ret])
 131                 return quadtree_add(parent->child[ret], new, compare);
 132
 133         parent->child[ret] = new;
 134         new->parent = parent;
 135
 136         quadtree_recalculate_parent_stats(parent);
 137
 138         validate_tree(new);
 139
 140         return new;
 141 }
 142
 143 /*
 144  * _quadtree_reposition_reqursively - Move everything under and
 145  * including a given node under the new root node
 146  */
 147
 148 static void
 149 _quadtree_reposition_reqursively(struct quadtree *root,
 150                                  struct quadtree *node,
 151                                  int (*compare)(struct quadtree *a,
 152                                                 struct quadtree *b))
 153 {
 154         int i;
 155
 156         validate_tree(node);
 157
 158         /* First remove all children, if any */
 159
 160         for (i = 0; i < 4; i++) {
 161                 if (!node->child[i])
 162                         continue;
 163
 164                 if (node->child[i] == node ||
 165                     node->child[i] == node->parent)
 166                         trap();
 167
 168                 _quadtree_reposition_reqursively(root, node->child[i], compare);
 169                 node->child[i] = 0;
 170         }
 171
 172         /* There are no children left, reset stats */
 173         node->depth = 0;
 174         node->children = 0;
 175
 176         /* Then remove this node under the new root. */
 177         quadtree_add(root, node, compare);
 178 }
 179
 180 /*
 181  * quadtree_del - Detach a node from the tree
 182  *
 183  * Return value: The new root node of the tree. If we are detaching
 184  * anything but the root node of the entire tree, the returned root
 185  * value will be the original root of the tree.
 186  */
 187 struct quadtree *quadtree_del(struct quadtree *node,
 188                               int (*compare)(struct quadtree *a,
 189                                              struct quadtree *b))
 190 {
 191         struct quadtree *parent = 0;
 192         int i;
 193
 194         /*
 195          * We are deleting the root node. This means we have to select
 196          * a new root node and reconstruct the entire tree under it
 197          * again.
 198          */
 199         if (!node->parent) {
 200                 for (i = 0; i < 4; i++) {
 201                         if (!node->child[i])
 202                                 continue;
 203
 204                         if (!parent) {
 205                                 parent = node->child[i];
 206                                 continue;
 207                         }
 208                         _quadtree_reposition_reqursively(parent,
 209                                                          node->child[i],
 210                                                          compare);
 211                         node->child[i] = 0;
 212                 }
 213
 214                 return parent;
 215         }
 216
 217         /*
 218          * We are not deleting the parent. Detach the node from the
 219          * parent abd relocate the children. The node will be then
 220          * detached from the tree.
 221          */
 222
 223         for (i = 0; i < 4; i++) {
 224                 if (node->parent->child[i] == node) {
 225                         node->parent->child[i] = 0;
 226                         break;
 227                 }
 228         }
 229         if (i == 4) {
 230                 printf("%s:%d Fatal! Tree inconsistency detected\n",
 231                        __func__, __LINE__);
 232                 trap();
 233         }
 234
 235         /* Fix parent stats */
 236         quadtree_recalculate_parent_stats(node->parent);
 237
 238         /*
 239          * The sub branch is now detached from the main tree. Continue
 240          * relocating the detached branch.
 241          */
 242
 243         for (i = 0; i < 4; i++) {
 244                 if (!node->child[i])
 245                         continue;
 246
 247                 _quadtree_reposition_reqursively(node->parent, node->child[i],
 248                                                  compare);
 249                 node->child[i] = 0;
 250         }
 251
 252         parent = quadtree_find_parent(node);
 253         node->parent = 0;
 254
 255         node->children = 0;
 256         node->depth = 0;
 257
 258         validate_tree(parent);
 259         return parent;
 260 }
 261
 262
 263 static int _walk_tree(struct quadtree *head, const struct quadtree_iterator *it)
 264 {
 265         int direction, count = 0;
 266
 267         direction = it->direction(head, (struct quadtree_iterator *)it);
 268
 269         if ((direction & QUADTREE_UPLEFT) && head->child[0])
 270                 count += _walk_tree(head->child[0], it);
 271
 272         if ((direction & QUADTREE_UPRIGHT) && head->child[1])
 273                 count += _walk_tree(head->child[1], it);
 274
 275         if ((direction & QUADTREE_DOWNLEFT) && head->child[2])
 276                 count += _walk_tree(head->child[2], it);
 277
 278         if ((direction & QUADTREE_DOWNRIGHT) && head->child[3])
 279                 count += _walk_tree(head->child[3], it);
 280
 281         if ((direction & QUADTREE_SELF) && it->callback) {
 282                 it->callback(head, (struct quadtree_iterator *)it);
 283                 count++;
 284         }
 285
 286         return count;
 287 }
 288
 289 int walk_quadtree(const struct quadtree_iterator *it)
 290 {
 291         return _walk_tree(it->head, it);
 292 }