1 /**
2 * @file
3 * @brief Implementation of a priority queue by using a binary heap.
4 * @note Manage a priority queue as a heap - the heap is implemented as an array.
5 */
6
7 /*
8 Originally written by Justin-Heyes Jones
9 Modified by Shlomi Fish, 2000
10 Modified by Florian Festi, 2007
11
12 This file is in the public domain (it's uncopyrighted).
13
14 Bases on Justin-Heyes Jones' A* tutorial
15 */
16
17 #include "common.h"
18 #include "pqueue.h"
19
20 /* given an index to any element in a binary tree stored in a linear array with the root at 1 and
21 * a "sentinel" value at 0 these macros are useful in making the code clearer */
22
23 /* the parent is always given by index/2 */
24 #define PQ_PARENT_INDEX(i) ((i)>>1)
25 #define PQ_FIRST_ENTRY (1)
26
27 /* left and right children are index * 2 and (index * 2) +1 respectively */
28 #define PQ_LEFT_CHILD_INDEX(i) ((i)<<1)
29 #define PQ_RIGHT_CHILD_INDEX(i) (((i)<<1)+1)
30 #define PGetRating(elem) ((elem).rating)
31
32
33 /**
34 * @brief initialise the priority queue with a maximum size of maxelements.
35 */
PQueueInitialise(priorityQueue_t * pq,uint32_t maxElements)36 void PQueueInitialise (priorityQueue_t* pq, uint32_t maxElements)
37 {
38 pq->maxSize = maxElements;
39 pq->currentSize = 0;
40
41 pq->elements = Mem_AllocTypeN(priorityQueueElement_t, maxElements + 1);
42
43 if (pq->elements == nullptr)
44 Sys_Error("PQueueInitialise: Memory alloc failed!");
45 }
46
PQueuePush(priorityQueue_t * pq,const pos4_t item,priorityQueueRating_t r)47 void PQueuePush (priorityQueue_t* pq, const pos4_t item, priorityQueueRating_t r)
48 {
49 uint32_t i, j;
50 uint32_t currentSize = pq->currentSize;
51
52 if (currentSize == pq->maxSize) {
53 const int new_size = pq->maxSize * 2;
54 pq->elements = (priorityQueueElement_t*)Mem_ReAlloc(pq->elements, sizeof(*pq->elements) * (new_size + 1));
55 pq->maxSize = new_size;
56 }
57
58 /* set i to the first unused element and increment CurrentSize */
59 i = (++currentSize);
60
61 /* while the parent of the space we're putting the new node into is worse than
62 * our new node, swap the space with the worse node. We keep doing that until we
63 * get to a worse node or until we get to the top
64 * note that we also can sort so that the minimum elements bubble up so we need to loops
65 * with the comparison operator flipped... */
66
67 while (i > PQ_FIRST_ENTRY && pq->elements[PQ_PARENT_INDEX(i)].rating > r) {
68 pq->elements[i] = pq->elements[PQ_PARENT_INDEX(i)];
69 i = PQ_PARENT_INDEX(i);
70 }
71
72 /* then add the element at the space we created. */
73 for (j = 0; j < 4; j++)
74 pq->elements[i].item[j] = item[j];
75
76 pq->elements[i].rating = r;
77
78 pq->currentSize = currentSize;
79 }
80
81 /**
82 * @brief free up memory for pqueue
83 */
PQueueFree(priorityQueue_t * pq)84 void PQueueFree (priorityQueue_t* pq)
85 {
86 Mem_Free(pq->elements);
87 }
88
89 /**
90 * @brief remove the first node from the pqueue and provide a pointer to it
91 */
PQueuePop(priorityQueue_t * pq,pos4_t item)92 void PQueuePop (priorityQueue_t* pq, pos4_t item)
93 {
94 uint32_t i, j;
95 uint32_t child;
96 priorityQueueElement_t* elements = pq->elements;
97 uint32_t currentSize = pq->currentSize;
98 priorityQueueElement_t pMaxElement;
99 priorityQueueElement_t pLastElement;
100
101 if (PQueueIsEmpty(pq))
102 return;
103
104 pMaxElement = elements[PQ_FIRST_ENTRY];
105
106 /* get pointer to last element in tree */
107 pLastElement = elements[currentSize--];
108
109 for (j = 0; j < 4; j++)
110 item[j] = pMaxElement.item[j];
111
112 for (i = PQ_FIRST_ENTRY; (child = PQ_LEFT_CHILD_INDEX(i)) <= currentSize; i = child) {
113 /* set child to the smaller of the two children... */
114
115 if ((child != currentSize) && (elements[child + 1].rating < elements[child].rating))
116 child ++;
117
118 if (pLastElement.rating > elements[child].rating)
119 elements[i] = elements[child];
120 else
121 break;
122 }
123
124 elements[i] = pLastElement;
125 pq->currentSize = currentSize;
126 }
127