65 lines
2.1 KiB
C
65 lines
2.1 KiB
C
#include "cartesian.h"
|
|
#include "dijkstra.h"
|
|
#include "heap.h"
|
|
#include "memory.h"
|
|
#include "printf.h"
|
|
|
|
//#include <stdio.h>
|
|
//#include <assert.h>
|
|
|
|
void dijkstra_cartesian(const char * graph,
|
|
int stride,
|
|
int width, int height,
|
|
int start_x, int start_y,
|
|
int end_x, int end_y,
|
|
char wall,
|
|
int turn_cost,
|
|
struct dijkstra_xy16 * prev,
|
|
int * dist)
|
|
{
|
|
struct heap_entry queue[width * height / 2 + 1];
|
|
int queue_length = 0;
|
|
|
|
// initialize all distances to the maximum distance
|
|
memory_set_int(dist, 0x7fffffff, width * height);
|
|
// initialize the distance from the start to the start to zero
|
|
dist[start_y * width + start_x] = 0;
|
|
|
|
// initialize the all node paths to "unknown"
|
|
memory_set_int((int *)prev, -1, width * height);
|
|
prev[start_y * width + start_x] =
|
|
(struct dijkstra_xy16){start_x - 1, start_y}; // west
|
|
|
|
// initialize the queue with "start"
|
|
heap_append_xy(queue, &queue_length, 0, start_x, start_y);
|
|
|
|
while (queue_length > 0) {
|
|
struct heap_entry min = heap_extract_max(queue, &queue_length);
|
|
//if (min.x == end_x && min.y == end_y)
|
|
//return;
|
|
for (int i = 0; i < 4; i++) {
|
|
int nx = min.x + cartesian_neighbor[i].x;
|
|
int ny = min.y + cartesian_neighbor[i].y;
|
|
if (!cartesian_inside(width, height, nx, ny))
|
|
continue;
|
|
if (graph[ny * stride + nx] == wall)
|
|
continue;
|
|
//if (prev[ny * width + nx].x != -1)
|
|
//continue;
|
|
int turn = 0;
|
|
struct dijkstra_xy16 * xy = &prev[min.y * width + min.x];
|
|
int dx = min.x - xy->x;
|
|
int dy = min.y - xy->y;
|
|
if (cartesian_neighbor[i].x != dx || cartesian_neighbor[i].y != dy)
|
|
turn = turn_cost;
|
|
|
|
int alt = dist[min.y * width + min.x] + 1 + turn;
|
|
if (alt < dist[ny * width + nx]) {
|
|
prev[ny * width + nx] = (struct dijkstra_xy16){min.x, min.y};
|
|
dist[ny * width + nx] = alt;
|
|
heap_append_xy(queue, &queue_length, -alt, nx, ny);
|
|
}
|
|
}
|
|
}
|
|
}
|