git.meshlink.io Git - catta/blob - prioq.c

   1 #include "prioq.h"
   2
   3 flxPrioQueue* flx_prio_queue_new(gint (*compare) (gpointer a, gpointer b)) {
   4     flxPrioQueue *q;
   5     g_assert(compare);
   6
   7     q = g_new(flxPrioQueue, 1);
   8     q->root = q->last = NULL;
   9     q->n_nodes = 0;
  10     q->compare = compare;
  11     return q;
  12 }
  13
  14 void flx_prio_queue_free(flxPrioQueue *q) {
  15     g_assert(q);
  16
  17     while (q->last)
  18         flx_prio_queue_remove(q, q->last);
  19
  20     g_assert(!q->n_nodes);
  21     g_free(q);
  22 }
  23
  24 static flxPrioQueueNode* get_node_at_xy(flxPrioQueue *q, guint x, guint y) {
  25     guint r;
  26     flxPrioQueueNode *n;
  27     g_assert(q);
  28
  29     n = q->root;
  30     g_assert(n);
  31
  32     for (r = 0; r < y; r++) {
  33         g_assert(n);
  34
  35         if ((x >> (y-r-1)) & 1)
  36             n = n->right;
  37         else
  38             n = n->left;
  39     }
  40
  41     g_assert(n->x == x);
  42     g_assert(n->y == y);
  43
  44     return n;
  45 }
  46
  47 static void exchange_nodes(flxPrioQueue *q, flxPrioQueueNode *a, flxPrioQueueNode *b) {
  48     flxPrioQueueNode *l, *r, *p, *ap, *an, *bp, *bn;
  49     gint t;
  50     g_assert(q);
  51     g_assert(a);
  52     g_assert(b);
  53     g_assert(a != b);
  54
  55     /* Swap positions */
  56     t = a->x; a->x = b->x; b->x = t;
  57     t = a->y; a->y = b->y; b->y = t;
  58
  59     if (a->parent == b) {
  60         /* B is parent of A */
  61
  62         p = b->parent;
  63         b->parent = a;
  64
  65         if ((a->parent = p)) {
  66             if (a->parent->left == b)
  67                 a->parent->left = a;
  68             else
  69                 a->parent->right = a;
  70         } else
  71             q->root = a;
  72
  73         if (b->left == a) {
  74             if ((b->left = a->left))
  75                 b->left->parent = b;
  76             a->left = b;
  77
  78             r = a->right;
  79             if ((a->right = b->right))
  80                 a->right->parent = a;
  81             if ((b->right = r))
  82                 b->right->parent = b;
  83
  84         } else {
  85             if ((b->right = a->right))
  86                 b->right->parent = b;
  87             a->right = b;
  88
  89             l = a->left;
  90             if ((a->left = b->left))
  91                 a->left->parent = a;
  92             if ((b->left = l))
  93                 b->left->parent = b;
  94         }
  95     } else if (b->parent == a) {
  96         /* A ist parent of B */
  97
  98         p = a->parent;
  99         a->parent = b;
 100
 101         if ((b->parent = p)) {
 102             if (b->parent->left == a)
 103                 b->parent->left = b;
 104             else
 105                 b->parent->right = b;
 106         } else
 107             q->root = b;
 108
 109         if (a->left == b) {
 110             if ((a->left = b->left))
 111                 a->left->parent = a;
 112             b->left = a;
 113
 114             r = a->right;
 115             if ((a->right = b->right))
 116                 a->right->parent = a;
 117             if ((b->right = r))
 118                 b->right->parent = b;
 119         } else {
 120             if ((a->right = b->right))
 121                 a->right->parent = a;
 122             b->right = a;
 123
 124             l = a->left;
 125             if ((a->left = b->left))
 126                 a->left->parent = a;
 127             if ((b->left = l))
 128                 b->left->parent = b;
 129         }
 130     } else {
 131         /* Swap parents */
 132         p = a->parent;
 133
 134         if ((a->parent = b->parent)) {
 135             if (a->parent->left == b)
 136                 a->parent->left = a;
 137             else
 138                 a->parent->right = a;
 139         } else
 140             q->root = a;
 141
 142         if ((b->parent = p)) {
 143             if (b->parent->left == a)
 144                 b->parent->left = b;
 145             else
 146                 b->parent->right = b;
 147         } else
 148             q->root = b;
 149
 150         /* Swap children */
 151         l = a->left;
 152         r = a->right;
 153
 154         if ((a->left = b->left))
 155             a->left->parent = a;
 156
 157         if ((b->left = l))
 158             b->left->parent = b;
 159
 160         if ((a->right = b->right))
 161             a->right->parent = a;
 162
 163         if ((b->right = r))
 164             b->right->parent = b;
 165     }
 166
 167     /* Swap siblings */
 168     ap = a->prev; an = a->next;
 169     bp = b->prev; bn = b->next;
 170
 171     if (a->next == b) {
 172         /* A is predecessor of B */
 173         a->prev = b;
 174         b->next = a;
 175
 176         if ((a->next = bn))
 177             a->next->prev = a;
 178         else
 179             q->last = a;
 180
 181         if ((b->prev = ap))
 182             b->prev->next = b;
 183
 184     } else if (b->next == a) {
 185         /* B is predecessor of A */
 186         a->next = b;
 187         b->prev = a;
 188
 189         if ((a->prev = bp))
 190             a->prev->next = a;
 191
 192         if ((b->next = an))
 193             b->next->prev = b;
 194         else
 195             q->last = b;
 196
 197     } else {
 198         /* A is no neighbour of B */
 199
 200         if ((a->prev = bp))
 201             a->prev->next = a;
 202
 203         if ((a->next = bn))
 204             a->next->prev = a;
 205         else
 206             q->last = a;
 207
 208         if ((b->prev = ap))
 209             b->prev->next = b;
 210
 211         if ((b->next = an))
 212             b->next->prev = b;
 213         else
 214             q->last = b;
 215     }
 216 }
 217
 218 /* Move a node to the correct position */
 219 static void shuffle_node(flxPrioQueue *q, flxPrioQueueNode *n) {
 220     g_assert(q);
 221     g_assert(n);
 222
 223     /* Move up until the position is OK */
 224     while (n->parent && q->compare(n->parent->data, n->data) > 0)
 225         exchange_nodes(q, n, n->parent);
 226
 227     /* Move down until the position is OK */
 228     for (;;) {
 229         flxPrioQueueNode *min;
 230
 231         if (!(min = n->left)) {
 232             /* No children */
 233             g_assert(!n->right);
 234             break;
 235         }
 236
 237         if (n->right && q->compare(n->right->data, min->data) < 0)
 238             min = n->right;
 239
 240         /* min now contains the smaller one of our two children */
 241
 242         if (q->compare(n->data, min->data) <= 0)
 243             /* Order OK */
 244             break;
 245
 246         exchange_nodes(q, n, min);
 247     }
 248 }
 249
 250 flxPrioQueueNode* flx_prio_queue_put(flxPrioQueue *q, gpointer data) {
 251     flxPrioQueueNode *n;
 252     g_assert(q);
 253
 254     n = g_new(flxPrioQueueNode, 1);
 255     n->queue = q;
 256     n->data = data;
 257
 258     if (q->last) {
 259         g_assert(q->root);
 260         g_assert(q->n_nodes);
 261
 262         n->y = q->last->y;
 263         n->x = q->last->x+1;
 264
 265         if (n->x >= ((guint) 1 << n->y)) {
 266             n->x = 0;
 267             n->y++;
 268         }
 269
 270         q->last->next = n;
 271         n->prev = q->last;
 272
 273         g_assert(n->y > 0);
 274         n->parent = get_node_at_xy(q, n->x/2, n->y-1);
 275
 276         if (n->x & 1)
 277             n->parent->right = n;
 278         else
 279             n->parent->left = n;
 280     } else {
 281         g_assert(!q->root);
 282         g_assert(!q->n_nodes);
 283
 284         n->y = n->x = 0;
 285         q->root = n;
 286         n->prev = n->parent = NULL;
 287     }
 288
 289     n->next = n->left = n->right = NULL;
 290     q->last = n;
 291     q->n_nodes++;
 292
 293     shuffle_node(q, n);
 294
 295     return n;
 296 }
 297
 298 void flx_prio_queue_remove(flxPrioQueue *q, flxPrioQueueNode *n) {
 299     g_assert(q);
 300     g_assert(n);
 301
 302     if (n != q->last) {
 303         flxPrioQueueNode *replacement = q->last;
 304         exchange_nodes(q, replacement, n);
 305         flx_prio_queue_remove(q, q->last);
 306         shuffle_node(q, replacement);
 307         return;
 308     }
 309
 310     g_assert(n == q->last);
 311     g_assert(!n->next);
 312     g_assert(!n->left);
 313     g_assert(!n->right);
 314
 315     q->last = n->prev;
 316
 317     if (n->prev)
 318         n->prev->next = NULL;
 319     else
 320         g_assert(!n->parent);
 321
 322     if (n->parent) {
 323         g_assert(n->prev);
 324         if (n->parent->left == n) {
 325             g_assert(n->parent->right == NULL);
 326             n->parent->left = NULL;
 327         } else {
 328             g_assert(n->parent->right == n);
 329             g_assert(n->parent->left != NULL);
 330             n->parent->right = NULL;
 331         }
 332     } else {
 333         g_assert(q->root == n);
 334         g_assert(!n->prev);
 335         q->root = NULL;
 336     }
 337
 338     q->n_nodes--;
 339
 340     g_free(n);
 341 }