111 lines
2.6 KiB
C
111 lines
2.6 KiB
C
#include <stdlib.h>
|
|
#include "pq.h"
|
|
|
|
typedef struct {
|
|
unsigned int priority;
|
|
size_t x, y;
|
|
mazeedge_dir_t dir;
|
|
} _point_t;
|
|
|
|
struct pq {
|
|
size_t heap_size, next;
|
|
_point_t* heap;
|
|
};
|
|
|
|
struct pq* pq_new(size_t heap_size) {
|
|
struct pq* result = malloc(sizeof(struct pq));
|
|
result->heap_size = heap_size;
|
|
result->next = 0;
|
|
result->heap = calloc(heap_size, sizeof(_point_t));
|
|
return result;
|
|
}
|
|
|
|
static void _swap(struct pq* q, size_t idx1, size_t idx2) {
|
|
unsigned int priority = q->heap[idx1].priority;
|
|
size_t x = q->heap[idx1].x;
|
|
size_t y = q->heap[idx1].y;
|
|
mazeedge_dir_t dir = q->heap[idx1].dir;
|
|
|
|
q->heap[idx1].priority = q->heap[idx2].priority;
|
|
q->heap[idx1].x = q->heap[idx2].x;
|
|
q->heap[idx1].y = q->heap[idx2].y;
|
|
q->heap[idx1].dir = q->heap[idx2].dir;
|
|
|
|
q->heap[idx2].priority = priority;
|
|
q->heap[idx2].x = x;
|
|
q->heap[idx2].y = y;
|
|
q->heap[idx2].dir = dir;
|
|
}
|
|
|
|
static void _heapify_up(struct pq* q) {
|
|
size_t i = q->next - 1;
|
|
while (i > 0) {
|
|
size_t parent = i / 2;
|
|
if (q->heap[parent].priority > q->heap[i].priority) {
|
|
_swap(q, parent, i);
|
|
i = parent;
|
|
} else {
|
|
// done
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
void pq_add(struct pq* q, unsigned int priority, size_t x, size_t y, mazeedge_dir_t dir) {
|
|
q->heap[q->next].x = x;
|
|
q->heap[q->next].y = y;
|
|
q->heap[q->next].priority = priority;
|
|
q->heap[q->next].dir = dir;
|
|
q->next++;
|
|
_heapify_up(q);
|
|
}
|
|
|
|
bool pq_empty(struct pq const* q) {
|
|
return q->next == 0;
|
|
}
|
|
|
|
static void _heapify_down(struct pq* q) {
|
|
size_t i = 0;
|
|
while (i < q->next) {
|
|
size_t ch1 = i * 2, ch2 = i * 2 + 1;
|
|
if (ch1 >= q->next) return;
|
|
if (ch2 >= q->next) {
|
|
if (q->heap[ch1].priority < q->heap[i].priority) {
|
|
_swap(q, i, ch1);
|
|
}
|
|
return;
|
|
}
|
|
|
|
// three-way compare
|
|
if ((q->heap[i].priority < q->heap[ch1].priority) &&
|
|
(q->heap[i].priority < q->heap[ch2].priority)) {
|
|
return; // all done - partial order ensured
|
|
}
|
|
if (q->heap[ch1].priority < q->heap[ch2].priority) {
|
|
_swap(q, i, ch1);
|
|
i = ch1;
|
|
} else {
|
|
_swap(q, i, ch2);
|
|
i = ch2;
|
|
}
|
|
}
|
|
}
|
|
|
|
int pq_pop(struct pq* q, size_t* x, size_t* y, mazeedge_dir_t* dir) {
|
|
if (pq_empty(q)) return 0;
|
|
*x = q->heap[0].x;
|
|
*y = q->heap[0].y;
|
|
*dir = q->heap[0].dir;
|
|
_swap(q, 0, q->next-1);
|
|
q->next--;
|
|
_heapify_down(q);
|
|
return 1;
|
|
}
|
|
|
|
void pq_free(struct pq* q) {
|
|
free(q->heap); q->heap = NULL;
|
|
q->heap_size = 0;
|
|
q->next = 0;
|
|
free(q);
|
|
}
|