7 * Implementation note: It might seem strange that we start indexing at 0
8 * but use 2*i and 2*i+1 to find the left and right sucessor of an index.
9 * The trick is that for index 0 the left successor is 0 again, and the
10 * right successor is 1 in this scheme. For the right successor 1 everything
11 * works like usual. We simply took care in the algorithms that they still
12 * work with the left child of 0 being 0 again. This was possible without
13 * any extra ifs or arithmetic.
14 * Thus we can save the wastage of 1 array position you can see in other
15 * implementations or the ugly (i+1)*2 - 1 and (i+1)*2 for calculating the
16 * left and right child. (At the expense that stuff easily breaks when you make
17 * changes and don't think that the left child of 0 is 0 :-/)
22 typedef struct _pqueue_el_t {
32 * Enforces the heap characteristics if the queue
33 * starting from element at position @p pos.
35 static void pqueue_heapify(pqueue *q, int pos) {
36 int len = ARR_LEN(q->elems);
38 while (pos * 2 < len) {
42 if (q->elems[exchange].key < q->elems[pos * 2].key) {
46 if ((pos * 2 + 1) < len && q->elems[exchange].key < q->elems[pos * 2 + 1].key) {
47 exchange = pos * 2 + 1;
54 q->elems[pos] = q->elems[exchange];
55 q->elems[exchange] = tmp;
62 * Sifts up a newly inserted element at position @p pos.
64 static void pqueue_sift_up(pqueue *q, int pos) {
65 while(q->elems[pos].key > q->elems[pos / 2].key) {
69 q->elems[pos] = q->elems[pos / 2];
70 q->elems[pos / 2] = tmp;
77 * Creates a new priority queue.
78 * @return A priority queue of initial length 0.
80 pqueue *new_pqueue(void) {
81 pqueue *res = xmalloc(sizeof(*res));
82 res->elems = NEW_ARR_F(pqueue_el_t, 0);
87 * Frees all memory allocated by the priority queue.
88 * @param q The priority queue to destroy.
90 void del_pqueue(pqueue *q) {
96 * Inserts a new element into a priority queue.
97 * @param q The priority queue the element should be inserted to.
98 * @param data The actual data which should be stored in the queue.
99 * @param key The priority for the data.
101 void pqueue_put(pqueue *q, void *data, double key) {
107 ARR_APP1(pqueue_el_t, q->elems, el);
109 pqueue_sift_up(q, ARR_LEN(q->elems) - 1);
113 * Returns and removes the first element, ie. that one with the highest priority, from the queue.
114 * @param q The priority queue.
115 * @return The first element of the queue. Asserts if queue is empty.
117 void *pqueue_get(pqueue *q) {
118 switch(ARR_LEN(q->elems)) {
120 assert(0 && "Attempt to retrieve element from empty priority queue.");
124 ARR_SHRINKLEN(q->elems, 0);
125 return q->elems[0].data;
128 void *data = q->elems[0].data;
129 int len = ARR_LEN(q->elems) - 1;
131 q->elems[0] = q->elems[len];
132 ARR_SHRINKLEN(q->elems, len);
133 pqueue_heapify(q, 0);
141 * Get the length of the priority queue.
142 * @param q The priority queue.
143 * @return The length of the queue.
145 int pqueue_length(pqueue *q) {
146 return ARR_LEN(q->elems);
150 * Returns true if queue is empty.
151 * @param q The priority queue.
152 * @return 1 if the queue is empty, 0 otherwise.
154 int pqueue_empty(pqueue *q) {
155 return ARR_LEN(q->elems) == 0;