76 lines
1.8 KiB
C
76 lines
1.8 KiB
C
#include <stdint.h>
|
|
|
|
#include "memory.h"
|
|
#include "cartesian.h"
|
|
#include "bfs.h"
|
|
|
|
struct queue_descriptor {
|
|
int start;
|
|
int end;
|
|
};
|
|
|
|
struct position {
|
|
int16_t x;
|
|
int16_t y;
|
|
};
|
|
|
|
static inline void queue_append(struct position * queue,
|
|
struct queue_descriptor * qd,
|
|
int x,
|
|
int y)
|
|
{
|
|
queue[qd->end].x = x;
|
|
queue[qd->end].y = y;
|
|
qd->end += 1;
|
|
}
|
|
|
|
static inline struct position * queue_pop(struct position * queue,
|
|
struct queue_descriptor * qd)
|
|
{
|
|
struct position * position = &queue[qd->start];
|
|
qd->start += 1;
|
|
return position;
|
|
}
|
|
|
|
void bfs(const char * graph,
|
|
int stride,
|
|
int width, int height,
|
|
int start_x, int start_y,
|
|
char wall,
|
|
struct cartesian_link * path)
|
|
{
|
|
struct position queue[width * height];
|
|
struct queue_descriptor qd;
|
|
qd.start = 0;
|
|
qd.end = 0;
|
|
|
|
memory_set_int((int *)path, -1, width * height);
|
|
path[start_y * width + start_x].parent_x = start_x;
|
|
path[start_y * width + start_x].parent_y = start_y;
|
|
|
|
queue_append(&queue, &qd, start_x, start_y);
|
|
|
|
while (qd.start != qd.end) {
|
|
struct position * p = queue_pop(&queue, &qd);
|
|
|
|
for (int i = 0; i < 4; i++) {
|
|
int nx = p->x + cartesian_neighbor[i].x;
|
|
int ny = p->y + cartesian_neighbor[i].y;
|
|
// ignore all edges that are outside the graph
|
|
if (!cartesian_inside(width, height, nx, ny))
|
|
continue;
|
|
// ignore all edges that are walls
|
|
if (graph[ny * stride + nx] == wall)
|
|
continue;
|
|
// ignore all edges that were previously visited
|
|
if (path[ny * width + nx].parent_x != -1)
|
|
continue;
|
|
|
|
path[ny * width + nx].parent_x = p->x;
|
|
path[ny * width + nx].parent_y = p->y;
|
|
|
|
queue_append(&queue, &qd, nx, ny);
|
|
}
|
|
}
|
|
}
|