vcs.maemo.org Git - navit-package/blob - navit/fib-1.1/fib.c

   1 /*-
   2  * Copyright 1997-2003 John-Mark Gurney.
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  *
  14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  24  * SUCH DAMAGE.
  25  *
  26  *      $Id: fib.c,v 1.2 2007-07-04 22:44:39 martin-s Exp $
  27  *
  28  */
  29
  30 #include <fib.h>
  31 #include <fibpriv.h>
  32
  33 #include <limits.h>
  34 #include <stdlib.h>
  35
  36 #define swap(type, a, b)                \
  37                 do {                    \
  38                         type c;         \
  39                         c = a;          \
  40                         a = b;          \
  41                         b = c;          \
  42                 } while (0)             \
  43
  44 #define INT_BITS        (sizeof(int) * 8)
  45 static inline int
  46 ceillog2(unsigned int a)
  47 {
  48         int oa;
  49         int i;
  50         int b;
  51
  52         oa = a;
  53         b = INT_BITS / 2;
  54         i = 0;
  55         while (b) {
  56                 i = (i << 1);
  57                 if (a >= (1 << b)) {
  58                         a /= (1 << b);
  59                         i = i | 1;
  60                 } else
  61                         a &= (1 << b) - 1;
  62                 b /= 2;
  63         }
  64         if ((1 << i) == oa)
  65                 return i;
  66         else
  67                 return i + 1;
  68 }
  69
  70 /*
  71  * Private Heap Functions
  72  */
  73 static void
  74 fh_deleteel(struct fibheap *h, struct fibheap_el *x)
  75 {
  76         void *data;
  77         int key;
  78
  79         data = x->fhe_data;
  80         key = x->fhe_key;
  81
  82         if (!h->fh_keys)
  83                 fh_replacedata(h, x, h->fh_neginf);
  84         else
  85                 fh_replacekey(h, x, INT_MIN);
  86         if (fh_extractminel(h) != x) {
  87                 /*
  88                  * XXX - This should never happen as fh_replace should set it
  89                  * to min.
  90                  */
  91                 abort();
  92         }
  93
  94         x->fhe_data = data;
  95         x->fhe_key = key;
  96 }
  97
  98 static void
  99 fh_initheap(struct fibheap *new)
 100 {
 101         new->fh_cmp_fnct = NULL;
 102         new->fh_neginf = NULL;
 103         new->fh_n = 0;
 104         new->fh_Dl = -1;
 105         new->fh_cons = NULL;
 106         new->fh_min = NULL;
 107         new->fh_root = NULL;
 108         new->fh_keys = 0;
 109 #ifdef FH_STATS
 110         new->fh_maxn = 0;
 111         new->fh_ninserts = 0;
 112         new->fh_nextracts = 0;
 113 #endif
 114 }
 115
 116 static void
 117 fh_destroyheap(struct fibheap *h)
 118 {
 119         h->fh_cmp_fnct = NULL;
 120         h->fh_neginf = NULL;
 121         if (h->fh_cons != NULL)
 122                 free(h->fh_cons);
 123         h->fh_cons = NULL;
 124         free(h);
 125 }
 126
 127 /*
 128  * Public Heap Functions
 129  */
 130 struct fibheap *
 131 fh_makekeyheap()
 132 {
 133         struct fibheap *n;
 134
 135         if ((n = malloc(sizeof *n)) == NULL)
 136                 return NULL;
 137
 138         fh_initheap(n);
 139         n->fh_keys = 1;
 140
 141         return n;
 142 }
 143
 144 struct fibheap *
 145 fh_makeheap()
 146 {
 147         struct fibheap *n;
 148
 149         if ((n = malloc(sizeof *n)) == NULL)
 150                 return NULL;
 151
 152         fh_initheap(n);
 153
 154         return n;
 155 }
 156
 157 voidcmp
 158 fh_setcmp(struct fibheap *h, voidcmp fnct)
 159 {
 160         voidcmp oldfnct;
 161
 162         oldfnct = h->fh_cmp_fnct;
 163         h->fh_cmp_fnct = fnct;
 164
 165         return oldfnct;
 166 }
 167
 168 void *
 169 fh_setneginf(struct fibheap *h, void *data)
 170 {
 171         void *old;
 172
 173         old = h->fh_neginf;
 174         h->fh_neginf = data;
 175
 176         return old;
 177 }
 178
 179 struct fibheap *
 180 fh_union(struct fibheap *ha, struct fibheap *hb)
 181 {
 182         struct fibheap_el *x;
 183
 184         if (ha->fh_root == NULL || hb->fh_root == NULL) {
 185                 /* either one or both are empty */
 186                 if (ha->fh_root == NULL) {
 187                         fh_destroyheap(ha);
 188                         return hb;
 189                 } else {
 190                         fh_destroyheap(hb);
 191                         return ha;
 192                 }
 193         }
 194         ha->fh_root->fhe_left->fhe_right = hb->fh_root;
 195         hb->fh_root->fhe_left->fhe_right = ha->fh_root;
 196         x = ha->fh_root->fhe_left;
 197         ha->fh_root->fhe_left = hb->fh_root->fhe_left;
 198         hb->fh_root->fhe_left = x;
 199         ha->fh_n += hb->fh_n;
 200         /*
 201          * we probably should also keep stats on number of unions
 202          */
 203
 204         /* set fh_min if necessary */
 205         if (fh_compare(ha, hb->fh_min, ha->fh_min) < 0)
 206                 ha->fh_min = hb->fh_min;
 207
 208         fh_destroyheap(hb);
 209         return ha;
 210 }
 211
 212 void
 213 fh_deleteheap(struct fibheap *h)
 214 {
 215         /*
 216          * We could do this even faster by walking each binomial tree, but
 217          * this is simpler to code.
 218          */
 219         while (h->fh_min != NULL)
 220                 fhe_destroy(fh_extractminel(h));
 221
 222         fh_destroyheap(h);
 223 }
 224
 225 /*
 226  * Public Key Heap Functions
 227  */
 228 struct fibheap_el *
 229 fh_insertkey(struct fibheap *h, int key, void *data)
 230 {
 231         struct fibheap_el *x;
 232
 233         if ((x = fhe_newelem()) == NULL)
 234                 return NULL;
 235
 236         /* just insert on root list, and make sure it's not the new min */
 237         x->fhe_data = data;
 238         x->fhe_key = key;
 239
 240         fh_insertel(h, x);
 241
 242         return x;
 243 }
 244
 245 int
 246 fh_minkey(struct fibheap *h)
 247 {
 248         if (h->fh_min == NULL)
 249                 return INT_MIN;
 250         return h->fh_min->fhe_key;
 251 }
 252
 253 int
 254 fh_replacekey(struct fibheap *h, struct fibheap_el *x, int key)
 255 {
 256         int ret;
 257
 258         ret = x->fhe_key;
 259         (void)fh_replacekeydata(h, x, key, x->fhe_data);
 260
 261         return ret;
 262 }
 263
 264 #include <stdio.h>
 265
 266 void *
 267 fh_replacekeydata(struct fibheap *h, struct fibheap_el *x, int key, void *data)
 268 {
 269         void *odata;
 270         int okey;
 271         struct fibheap_el *y;
 272         int r;
 273
 274         odata = x->fhe_data;
 275         okey = x->fhe_key;
 276
 277         /*
 278          * we can increase a key by deleting and reinserting, that
 279          * requires O(lgn) time.
 280          */
 281         if ((r = fh_comparedata(h, key, data, x)) > 0) {
 282                 printf("fh_comparedata r=%d key=%d data=%p\n", r, key, data);
 283                 /* XXX - bad code! */
 284                 abort();
 285                 fh_deleteel(h, x);
 286
 287                 x->fhe_data = data;
 288                 x->fhe_key = key;
 289
 290                 fh_insertel(h, x);
 291
 292                 return odata;
 293         }
 294
 295         x->fhe_data = data;
 296         x->fhe_key = key;
 297
 298         /* because they are equal, we don't have to do anything */
 299         if (r == 0)
 300                 return odata;
 301
 302         y = x->fhe_p;
 303
 304         if (h->fh_keys && okey == key)
 305                 return odata;
 306
 307         if (y != NULL && fh_compare(h, x, y) <= 0) {
 308                 fh_cut(h, x, y);
 309                 fh_cascading_cut(h, y);
 310         }
 311
 312         /*
 313          * the = is so that the call from fh_delete will delete the proper
 314          * element.
 315          */
 316         if (fh_compare(h, x, h->fh_min) <= 0)
 317                 h->fh_min = x;
 318
 319         return odata;
 320 }
 321
 322 /*
 323  * Public void * Heap Functions
 324  */
 325 /*
 326  * this will return these values:
 327  *      NULL    failed for some reason
 328  *      ptr     token to use for manipulation of data
 329  */
 330 struct fibheap_el *
 331 fh_insert(struct fibheap *h, void *data)
 332 {
 333         struct fibheap_el *x;
 334
 335         if ((x = fhe_newelem()) == NULL)
 336                 return NULL;
 337
 338         /* just insert on root list, and make sure it's not the new min */
 339         x->fhe_data = data;
 340
 341         fh_insertel(h, x);
 342
 343         return x;
 344 }
 345
 346 void *
 347 fh_min(struct fibheap *h)
 348 {
 349         if (h->fh_min == NULL)
 350                 return NULL;
 351         return h->fh_min->fhe_data;
 352 }
 353
 354 void *
 355 fh_extractmin(struct fibheap *h)
 356 {
 357         struct fibheap_el *z;
 358         void *ret;
 359
 360         ret = NULL;
 361
 362         if (h->fh_min != NULL) {
 363                 z = fh_extractminel(h);
 364                 ret = z->fhe_data;
 365 #ifndef NO_FREE
 366                 fhe_destroy(z);
 367 #endif
 368
 369         }
 370
 371         return ret;
 372 }
 373
 374 void *
 375 fh_replacedata(struct fibheap *h, struct fibheap_el *x, void *data)
 376 {
 377         return fh_replacekeydata(h, x, x->fhe_key, data);
 378 }
 379
 380 void *
 381 fh_delete(struct fibheap *h, struct fibheap_el *x)
 382 {
 383         void *k;
 384
 385         k = x->fhe_data;
 386         if (!h->fh_keys)
 387                 fh_replacedata(h, x, h->fh_neginf);
 388         else
 389                 fh_replacekey(h, x, INT_MIN);
 390         fh_extractmin(h);
 391
 392         return k;
 393 }
 394
 395 /*
 396  * Statistics Functions
 397  */
 398 #ifdef FH_STATS
 399 int
 400 fh_maxn(struct fibheap *h)
 401 {
 402         return h->fh_maxn;
 403 }
 404
 405 int
 406 fh_ninserts(struct fibheap *h)
 407 {
 408         return h->fh_ninserts;
 409 }
 410
 411 int
 412 fh_nextracts(struct fibheap *h)
 413 {
 414         return h->fh_nextracts;
 415 }
 416 #endif
 417
 418 /*
 419  * begin of private element fuctions
 420  */
 421 static struct fibheap_el *
 422 fh_extractminel(struct fibheap *h)
 423 {
 424         struct fibheap_el *ret;
 425         struct fibheap_el *x, *y, *orig;
 426
 427         ret = h->fh_min;
 428
 429         orig = NULL;
 430         /* put all the children on the root list */
 431         /* for true consistancy, we should use fhe_remove */
 432         for(x = ret->fhe_child; x != orig && x != NULL;) {
 433                 if (orig == NULL)
 434                         orig = x;
 435                 y = x->fhe_right;
 436                 x->fhe_p = NULL;
 437                 fh_insertrootlist(h, x);
 438                 x = y;
 439         }
 440         /* remove minimum from root list */
 441         fh_removerootlist(h, ret);
 442         h->fh_n--;
 443
 444         /* if we aren't empty, consolidate the heap */
 445         if (h->fh_n == 0)
 446                 h->fh_min = NULL;
 447         else {
 448                 h->fh_min = ret->fhe_right;
 449                 fh_consolidate(h);
 450         }
 451
 452 #ifdef FH_STATS
 453         h->fh_nextracts++;
 454 #endif
 455
 456         return ret;
 457 }
 458
 459 static void
 460 fh_insertrootlist(struct fibheap *h, struct fibheap_el *x)
 461 {
 462         if (h->fh_root == NULL) {
 463                 h->fh_root = x;
 464                 x->fhe_left = x;
 465                 x->fhe_right = x;
 466                 return;
 467         }
 468
 469         fhe_insertafter(h->fh_root, x);
 470 }
 471
 472 static void
 473 fh_removerootlist(struct fibheap *h, struct fibheap_el *x)
 474 {
 475         if (x->fhe_left == x)
 476                 h->fh_root = NULL;
 477         else
 478                 h->fh_root = fhe_remove(x);
 479 }
 480
 481 static void
 482 fh_consolidate(struct fibheap *h)
 483 {
 484         struct fibheap_el **a;
 485         struct fibheap_el *w;
 486         struct fibheap_el *y;
 487         struct fibheap_el *x;
 488         int i;
 489         int d;
 490         int D;
 491
 492         fh_checkcons(h);
 493
 494         /* assign a the value of h->fh_cons so I don't have to rewrite code */
 495         D = h->fh_Dl + 1;
 496         a = h->fh_cons;
 497
 498         for (i = 0; i < D; i++)
 499                 a[i] = NULL;
 500
 501         while ((w = h->fh_root) != NULL) {
 502                 x = w;
 503                 fh_removerootlist(h, w);
 504                 d = x->fhe_degree;
 505                 /* XXX - assert that d < D */
 506                 while(a[d] != NULL) {
 507                         y = a[d];
 508                         if (fh_compare(h, x, y) > 0)
 509                                 swap(struct fibheap_el *, x, y);
 510                         fh_heaplink(h, y, x);
 511                         a[d] = NULL;
 512                         d++;
 513                 }
 514                 a[d] = x;
 515         }
 516         h->fh_min = NULL;
 517         for (i = 0; i < D; i++)
 518                 if (a[i] != NULL) {
 519                         fh_insertrootlist(h, a[i]);
 520                         if (h->fh_min == NULL || fh_compare(h, a[i],
 521                             h->fh_min) < 0)
 522                                 h->fh_min = a[i];
 523                 }
 524 }
 525
 526 static void
 527 fh_heaplink(struct fibheap *h, struct fibheap_el *y, struct fibheap_el *x)
 528 {
 529         /* make y a child of x */
 530         if (x->fhe_child == NULL)
 531                 x->fhe_child = y;
 532         else
 533                 fhe_insertbefore(x->fhe_child, y);
 534         y->fhe_p = x;
 535         x->fhe_degree++;
 536         y->fhe_mark = 0;
 537 }
 538
 539 static void
 540 fh_cut(struct fibheap *h, struct fibheap_el *x, struct fibheap_el *y)
 541 {
 542         fhe_remove(x);
 543         y->fhe_degree--;
 544         fh_insertrootlist(h, x);
 545         x->fhe_p = NULL;
 546         x->fhe_mark = 0;
 547 }
 548
 549 static void
 550 fh_cascading_cut(struct fibheap *h, struct fibheap_el *y)
 551 {
 552         struct fibheap_el *z;
 553
 554         while ((z = y->fhe_p) != NULL) {
 555                 if (y->fhe_mark == 0) {
 556                         y->fhe_mark = 1;
 557                         return;
 558                 } else {
 559                         fh_cut(h, y, z);
 560                         y = z;
 561                 }
 562         }
 563 }
 564
 565 /*
 566  * begining of handling elements of fibheap
 567  */
 568 static struct fibheap_el *
 569 fhe_newelem()
 570 {
 571         struct fibheap_el *e;
 572
 573         if ((e = malloc(sizeof *e)) == NULL)
 574                 return NULL;
 575
 576         fhe_initelem(e);
 577
 578         return e;
 579 }
 580
 581 static void
 582 fhe_initelem(struct fibheap_el *e)
 583 {
 584         e->fhe_degree = 0;
 585         e->fhe_mark = 0;
 586         e->fhe_p = NULL;
 587         e->fhe_child = NULL;
 588         e->fhe_left = e;
 589         e->fhe_right = e;
 590         e->fhe_data = NULL;
 591 }
 592
 593 static void
 594 fhe_insertafter(struct fibheap_el *a, struct fibheap_el *b)
 595 {
 596         if (a == a->fhe_right) {
 597                 a->fhe_right = b;
 598                 a->fhe_left = b;
 599                 b->fhe_right = a;
 600                 b->fhe_left = a;
 601         } else {
 602                 b->fhe_right = a->fhe_right;
 603                 a->fhe_right->fhe_left = b;
 604                 a->fhe_right = b;
 605                 b->fhe_left = a;
 606         }
 607 }
 608
 609 static inline void
 610 fhe_insertbefore(struct fibheap_el *a, struct fibheap_el *b)
 611 {
 612         fhe_insertafter(a->fhe_left, b);
 613 }
 614
 615 static struct fibheap_el *
 616 fhe_remove(struct fibheap_el *x)
 617 {
 618         struct fibheap_el *ret;
 619
 620         if (x == x->fhe_left)
 621                 ret = NULL;
 622         else
 623                 ret = x->fhe_left;
 624
 625         /* fix the parent pointer */
 626         if (x->fhe_p != NULL && x->fhe_p->fhe_child == x)
 627                 x->fhe_p->fhe_child = ret;
 628
 629         x->fhe_right->fhe_left = x->fhe_left;
 630         x->fhe_left->fhe_right = x->fhe_right;
 631
 632         /* clear out hanging pointers */
 633         x->fhe_p = NULL;
 634         x->fhe_left = x;
 635         x->fhe_right = x;
 636
 637         return ret;
 638 }
 639
 640 static void
 641 fh_checkcons(struct fibheap *h)
 642 {
 643         int oDl;
 644
 645         /* make sure we have enough memory allocated to "reorganize" */
 646         if (h->fh_Dl == -1 || h->fh_n > (1 << h->fh_Dl)) {
 647                 oDl = h->fh_Dl;
 648                 if ((h->fh_Dl = ceillog2(h->fh_n) + 1) < 8)
 649                         h->fh_Dl = 8;
 650                 if (oDl != h->fh_Dl)
 651                         h->fh_cons = (struct fibheap_el **)realloc(h->fh_cons,
 652                             sizeof *h->fh_cons * (h->fh_Dl + 1));
 653                 if (h->fh_cons == NULL)
 654                         abort();
 655         }
 656 }
 657
 658 static int
 659 fh_compare(struct fibheap *h, struct fibheap_el *a, struct fibheap_el *b)
 660 {
 661         if (h->fh_keys) {
 662                 if (a->fhe_key < b->fhe_key)
 663                         return -1;
 664                 if (a->fhe_key == b->fhe_key)
 665                         return 0;
 666                 return 1;
 667         } else
 668                 return h->fh_cmp_fnct(a->fhe_data, b->fhe_data);
 669 }
 670
 671 static int
 672 fh_comparedata(struct fibheap *h, int key, void *data, struct fibheap_el *b)
 673 {
 674         struct fibheap_el a;
 675
 676         a.fhe_key = key;
 677         a.fhe_data = data;
 678
 679         return fh_compare(h, &a, b);
 680 }
 681
 682 static void
 683 fh_insertel(struct fibheap *h, struct fibheap_el *x)
 684 {
 685         fh_insertrootlist(h, x);
 686
 687         if (h->fh_min == NULL || (h->fh_keys ? x->fhe_key < h->fh_min->fhe_key
 688             : h->fh_cmp_fnct(x->fhe_data, h->fh_min->fhe_data) < 0))
 689                 h->fh_min = x;
 690
 691         h->fh_n++;
 692
 693 #ifdef FH_STATS
 694         if (h->fh_n > h->fh_maxn)
 695                 h->fh_maxn = h->fh_n;
 696         h->fh_ninserts++;
 697 #endif
 698
 699 }