navit/fib-1.1/fib.c

/*-
 * Copyright 1997-2003 John-Mark Gurney.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 *      $Id: fib.c,v 1.2 2007-07-04 22:44:39 martin-s Exp $
 *
 */

#include <fib.h>
#include <fibpriv.h>

#include <limits.h>
#include <stdlib.h>

#define swap(type, a, b)                \
                do {                    \
                        type c;         \
                        c = a;          \
                        a = b;          \
                        b = c;          \
                } while (0)             \

#define INT_BITS        (sizeof(int) * 8)
static int
ceillog2(unsigned int a)
{
        int oa;
        int i;
        int b;

        oa = a;
        b = INT_BITS / 2;
        i = 0;
        while (b) {
                i = (i << 1);
                if (a >= (1 << b)) {
                        a /= (1 << b);
                        i = i | 1;
                } else
                        a &= (1 << b) - 1;
                b /= 2;
        }
        if ((1 << i) == oa)
                return i;
        else
                return i + 1;
}

/*
 * Private Heap Functions
 */
static void
fh_deleteel(struct fibheap *h, struct fibheap_el *x)
{
        void *data;
        int key;

        data = x->fhe_data;
        key = x->fhe_key;

        if (!h->fh_keys)
                fh_replacedata(h, x, h->fh_neginf);
        else
                fh_replacekey(h, x, INT_MIN);
        if (fh_extractminel(h) != x) {
                /*
                 * XXX - This should never happen as fh_replace should set it
                 * to min.
                 */
                abort();
        }

        x->fhe_data = data;
        x->fhe_key = key;
}

static void
fh_initheap(struct fibheap *new)
{
        new->fh_cmp_fnct = NULL;
        new->fh_neginf = NULL;
        new->fh_n = 0;
        new->fh_Dl = -1;
        new->fh_cons = NULL;
        new->fh_min = NULL;
        new->fh_root = NULL;
        new->fh_keys = 0;
#ifdef FH_STATS
        new->fh_maxn = 0;
        new->fh_ninserts = 0;
        new->fh_nextracts = 0;
#endif
}

static void
fh_destroyheap(struct fibheap *h)
{
        h->fh_cmp_fnct = NULL;
        h->fh_neginf = NULL;
        if (h->fh_cons != NULL)
                free(h->fh_cons);
        h->fh_cons = NULL;
        free(h);
}

/*
 * Public Heap Functions
 */
struct fibheap *
fh_makekeyheap()
{
        struct fibheap *n;

        if ((n = malloc(sizeof *n)) == NULL)
                return NULL;

        fh_initheap(n);
        n->fh_keys = 1;

        return n;
}

struct fibheap *
fh_makeheap()
{
        struct fibheap *n;

        if ((n = malloc(sizeof *n)) == NULL)
                return NULL;

        fh_initheap(n);

        return n;
}

voidcmp
fh_setcmp(struct fibheap *h, voidcmp fnct)
{
        voidcmp oldfnct;
        
        oldfnct = h->fh_cmp_fnct;
        h->fh_cmp_fnct = fnct;

        return oldfnct;
}

void *
fh_setneginf(struct fibheap *h, void *data)
{
        void *old;

        old = h->fh_neginf;
        h->fh_neginf = data;

        return old;
}

struct fibheap *
fh_union(struct fibheap *ha, struct fibheap *hb)
{
        struct fibheap_el *x;

        if (ha->fh_root == NULL || hb->fh_root == NULL) {
                /* either one or both are empty */
                if (ha->fh_root == NULL) {
                        fh_destroyheap(ha);
                        return hb;
                } else {
                        fh_destroyheap(hb);
                        return ha;
                }
        }
        ha->fh_root->fhe_left->fhe_right = hb->fh_root;
        hb->fh_root->fhe_left->fhe_right = ha->fh_root;
        x = ha->fh_root->fhe_left;
        ha->fh_root->fhe_left = hb->fh_root->fhe_left;
        hb->fh_root->fhe_left = x;
        ha->fh_n += hb->fh_n;
        /*
         * we probably should also keep stats on number of unions
         */

        /* set fh_min if necessary */
        if (fh_compare(ha, hb->fh_min, ha->fh_min) < 0)
                ha->fh_min = hb->fh_min;

        fh_destroyheap(hb);
        return ha;
}

void
fh_deleteheap(struct fibheap *h)
{
        /*
         * We could do this even faster by walking each binomial tree, but
         * this is simpler to code.
         */
        while (h->fh_min != NULL)
                fhe_destroy(fh_extractminel(h));

        fh_destroyheap(h);
}

/*
 * Public Key Heap Functions
 */
struct fibheap_el *
fh_insertkey(struct fibheap *h, int key, void *data)
{
        struct fibheap_el *x;

        if ((x = fhe_newelem()) == NULL)
                return NULL;

        /* just insert on root list, and make sure it's not the new min */
        x->fhe_data = data;
        x->fhe_key = key;

        fh_insertel(h, x);

        return x;
}

int
fh_minkey(struct fibheap *h)
{
        if (h->fh_min == NULL)
                return INT_MIN;
        return h->fh_min->fhe_key;
}

int
fh_replacekey(struct fibheap *h, struct fibheap_el *x, int key)
{
        int ret;

        ret = x->fhe_key;
        (void)fh_replacekeydata(h, x, key, x->fhe_data);

        return ret;
}

#include <stdio.h>

void *
fh_replacekeydata(struct fibheap *h, struct fibheap_el *x, int key, void *data)
{
        void *odata;
        int okey;
        struct fibheap_el *y;
        int r;

        odata = x->fhe_data;
        okey = x->fhe_key;

        /*
         * we can increase a key by deleting and reinserting, that
         * requires O(lgn) time.
         */
        if ((r = fh_comparedata(h, key, data, x)) > 0) {
                printf("fh_comparedata r=%d key=%d data=%p\n", r, key, data);
                /* XXX - bad code! */
                abort();
                fh_deleteel(h, x);

                x->fhe_data = data;
                x->fhe_key = key;

                fh_insertel(h, x);

                return odata;
        }

        x->fhe_data = data;
        x->fhe_key = key;

        /* because they are equal, we don't have to do anything */
        if (r == 0)
                return odata;

        y = x->fhe_p;

        if (h->fh_keys && okey == key)
                return odata;

        if (y != NULL && fh_compare(h, x, y) <= 0) {
                fh_cut(h, x, y);
                fh_cascading_cut(h, y);
        }

        /*
         * the = is so that the call from fh_delete will delete the proper
         * element.
         */
        if (fh_compare(h, x, h->fh_min) <= 0)
                h->fh_min = x;

        return odata;
}

/*
 * Public void * Heap Functions
 */
/*
 * this will return these values:
 *      NULL    failed for some reason
 *      ptr     token to use for manipulation of data
 */
struct fibheap_el *
fh_insert(struct fibheap *h, void *data)
{
        struct fibheap_el *x;

        if ((x = fhe_newelem()) == NULL)
                return NULL;

        /* just insert on root list, and make sure it's not the new min */
        x->fhe_data = data;

        fh_insertel(h, x);

        return x;
}

void *
fh_min(struct fibheap *h)
{
        if (h->fh_min == NULL)
                return NULL;
        return h->fh_min->fhe_data;
}

void *
fh_extractmin(struct fibheap *h)
{
        struct fibheap_el *z;
        void *ret;

        ret = NULL;

        if (h->fh_min != NULL) {
                z = fh_extractminel(h);
                ret = z->fhe_data;
#ifndef NO_FREE
                fhe_destroy(z);
#endif

        }

        return ret;
}

void *
fh_replacedata(struct fibheap *h, struct fibheap_el *x, void *data)
{
        return fh_replacekeydata(h, x, x->fhe_key, data);
}

void *
fh_delete(struct fibheap *h, struct fibheap_el *x)
{
        void *k;

        k = x->fhe_data;
        if (!h->fh_keys)
                fh_replacedata(h, x, h->fh_neginf);
        else
                fh_replacekey(h, x, INT_MIN);
        fh_extractmin(h);

        return k;
}

/*
 * Statistics Functions
 */
#ifdef FH_STATS
int
fh_maxn(struct fibheap *h)
{
        return h->fh_maxn;
}

int
fh_ninserts(struct fibheap *h)
{
        return h->fh_ninserts;
}

int
fh_nextracts(struct fibheap *h)
{
        return h->fh_nextracts;
}
#endif

/*
 * begin of private element fuctions
 */
static struct fibheap_el *
fh_extractminel(struct fibheap *h)
{
        struct fibheap_el *ret;
        struct fibheap_el *x, *y, *orig;

        ret = h->fh_min;

        orig = NULL;
        /* put all the children on the root list */
        /* for true consistancy, we should use fhe_remove */
        for(x = ret->fhe_child; x != orig && x != NULL;) {
                if (orig == NULL)
                        orig = x;
                y = x->fhe_right;
                x->fhe_p = NULL;
                fh_insertrootlist(h, x);
                x = y;
        }
        /* remove minimum from root list */
        fh_removerootlist(h, ret);
        h->fh_n--;

        /* if we aren't empty, consolidate the heap */
        if (h->fh_n == 0)
                h->fh_min = NULL;
        else {
                h->fh_min = ret->fhe_right;
                fh_consolidate(h);
        }

#ifdef FH_STATS
        h->fh_nextracts++;
#endif

        return ret;
}

static void
fh_insertrootlist(struct fibheap *h, struct fibheap_el *x)
{
        if (h->fh_root == NULL) {
                h->fh_root = x;
                x->fhe_left = x;
                x->fhe_right = x;
                return;
        }

        fhe_insertafter(h->fh_root, x);
}

static void
fh_removerootlist(struct fibheap *h, struct fibheap_el *x)
{
        if (x->fhe_left == x)
                h->fh_root = NULL;
        else
                h->fh_root = fhe_remove(x);
}

static void
fh_consolidate(struct fibheap *h)
{
        struct fibheap_el **a;
        struct fibheap_el *w;
        struct fibheap_el *y;
        struct fibheap_el *x;
        int i;
        int d;
        int D;

        fh_checkcons(h);

        /* assign a the value of h->fh_cons so I don't have to rewrite code */
        D = h->fh_Dl + 1;
        a = h->fh_cons;

        for (i = 0; i < D; i++)
                a[i] = NULL;

        while ((w = h->fh_root) != NULL) {
                x = w;
                fh_removerootlist(h, w);
                d = x->fhe_degree;
                /* XXX - assert that d < D */
                while(a[d] != NULL) {
                        y = a[d];
                        if (fh_compare(h, x, y) > 0)
                                swap(struct fibheap_el *, x, y);
                        fh_heaplink(h, y, x);
                        a[d] = NULL;
                        d++;
                }
                a[d] = x;
        }
        h->fh_min = NULL;
        for (i = 0; i < D; i++)
                if (a[i] != NULL) {
                        fh_insertrootlist(h, a[i]);
                        if (h->fh_min == NULL || fh_compare(h, a[i],
                            h->fh_min) < 0)
                                h->fh_min = a[i];
                }
}

static void
fh_heaplink(struct fibheap *h, struct fibheap_el *y, struct fibheap_el *x)
{
        /* make y a child of x */
        if (x->fhe_child == NULL)
                x->fhe_child = y;
        else
                fhe_insertbefore(x->fhe_child, y);
        y->fhe_p = x;
        x->fhe_degree++;
        y->fhe_mark = 0;
}

static void
fh_cut(struct fibheap *h, struct fibheap_el *x, struct fibheap_el *y)
{
        fhe_remove(x);
        y->fhe_degree--;
        fh_insertrootlist(h, x);
        x->fhe_p = NULL;
        x->fhe_mark = 0;
}

static void
fh_cascading_cut(struct fibheap *h, struct fibheap_el *y)
{
        struct fibheap_el *z;

        while ((z = y->fhe_p) != NULL) {
                if (y->fhe_mark == 0) {
                        y->fhe_mark = 1;
                        return;
                } else {
                        fh_cut(h, y, z);
                        y = z;
                }
        }
}

/*
 * begining of handling elements of fibheap
 */
static struct fibheap_el *
fhe_newelem()
{
        struct fibheap_el *e;

        if ((e = malloc(sizeof *e)) == NULL)
                return NULL;

        fhe_initelem(e);

        return e;
}

static void
fhe_initelem(struct fibheap_el *e)
{
        e->fhe_degree = 0;
        e->fhe_mark = 0;
        e->fhe_p = NULL;
        e->fhe_child = NULL;
        e->fhe_left = e;
        e->fhe_right = e;
        e->fhe_data = NULL;
}

static void
fhe_insertafter(struct fibheap_el *a, struct fibheap_el *b)
{
        if (a == a->fhe_right) {
                a->fhe_right = b;
                a->fhe_left = b;
                b->fhe_right = a;
                b->fhe_left = a;
        } else {
                b->fhe_right = a->fhe_right;
                a->fhe_right->fhe_left = b;
                a->fhe_right = b;
                b->fhe_left = a;
        }
}

static void
fhe_insertbefore(struct fibheap_el *a, struct fibheap_el *b)
{
        fhe_insertafter(a->fhe_left, b);
}

static struct fibheap_el *
fhe_remove(struct fibheap_el *x)
{
        struct fibheap_el *ret;

        if (x == x->fhe_left)
                ret = NULL;
        else
                ret = x->fhe_left;

        /* fix the parent pointer */
        if (x->fhe_p != NULL && x->fhe_p->fhe_child == x)
                x->fhe_p->fhe_child = ret;

        x->fhe_right->fhe_left = x->fhe_left;
        x->fhe_left->fhe_right = x->fhe_right;

        /* clear out hanging pointers */
        x->fhe_p = NULL;
        x->fhe_left = x;
        x->fhe_right = x;

        return ret;
}

static void
fh_checkcons(struct fibheap *h)
{
        int oDl;

        /* make sure we have enough memory allocated to "reorganize" */
        if (h->fh_Dl == -1 || h->fh_n > (1 << h->fh_Dl)) {
                oDl = h->fh_Dl;
                if ((h->fh_Dl = ceillog2(h->fh_n) + 1) < 8)
                        h->fh_Dl = 8;
                if (oDl != h->fh_Dl)
                        h->fh_cons = (struct fibheap_el **)realloc(h->fh_cons,
                            sizeof *h->fh_cons * (h->fh_Dl + 1));
                if (h->fh_cons == NULL)
                        abort();
        }
}

static int
fh_compare(struct fibheap *h, struct fibheap_el *a, struct fibheap_el *b)
{
        if (h->fh_keys) {
                if (a->fhe_key < b->fhe_key)
                        return -1;
                if (a->fhe_key == b->fhe_key)
                        return 0;
                return 1;
        } else
                return h->fh_cmp_fnct(a->fhe_data, b->fhe_data);
}

static int
fh_comparedata(struct fibheap *h, int key, void *data, struct fibheap_el *b)
{
        struct fibheap_el a;

        a.fhe_key = key;
        a.fhe_data = data;

        return fh_compare(h, &a, b);
}

static void
fh_insertel(struct fibheap *h, struct fibheap_el *x)
{
        fh_insertrootlist(h, x);

        if (h->fh_min == NULL || (h->fh_keys ? x->fhe_key < h->fh_min->fhe_key
            : h->fh_cmp_fnct(x->fhe_data, h->fh_min->fhe_data) < 0))
                h->fh_min = x;

        h->fh_n++;

#ifdef FH_STATS
        if (h->fh_n > h->fh_maxn)
                h->fh_maxn = h->fh_n;
        h->fh_ninserts++;
#endif

}
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 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 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	}