Created
January 13, 2020 06:14
-
-
Save Tankenstein/f91ee2528f415a1796638eb863593935 to your computer and use it in GitHub Desktop.
Simple suboptimal maze solver for fun in c.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
101 | |
101 | |
E XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX | |
X X X X X X X X X X X X X X X X X X X X X X X | |
X XXX X X XXXXX XXX X XXXXX XXX XXX X XXX XXXXX X X X X XXX X X X X X XXX X X XXX XXX XXXXX XXX X X X | |
X X X X X X X X X X X X X X X X X X X X X X X X | |
X X X XXXXX XXX XXXXX X XXXXXXXXX XXX X XXXXXXXXXXXXXXXXX X XXXXX X X XXXXXXXXX XXXXX X X XXXXXXXXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X | |
X XXX X X XXXXXXX X XXXXXXX XXXXXXX XXXXX X X XXX X XXXXX X X XXXXX XXX X X XXXXX XXXXXXXXX X X XXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXXXX X X XXX X XXX X XXX X XXX XXX XXXXXXX X X X X XXXXX XXXXX X X XXXXXXXXX XXXXX XXX X XXX XXXXX X | |
X X X X X X X X X X X X X X X X X X X X X | |
XXX XXXXXXX XXX X XXXXX X X XXX X X XXXXX XXXXXXX XXX XXX X X XXXXX X XXX XXX X XXXXX X X XXXXXXX X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXXXX X X XXXXX XXX XXX XXX XXX XXX X X XXXXX X XXXXX XXXXXXXXX XXXXXXX X XXX XXX X X X X X X X X X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X XXX XXX X X X X X X X XXX XXX X X X X XXX XXX XXXXXXX XXXXX X XXXXX X XXX XXX X XXXXX X X XXXXXXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X XXX X X XXX XXX XXX X X X X X XXX XXXXX XXXXXXX X XXX XXX XXX XXX X XXX X XXX XXX X X XXX XXX X X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X XXXXXXX XXX XXXXX X X X X X XXXXXXXXXXX X XXXXXXX X XXX X X XXXXX X X X XXX X XXXXX XXXXX X XXXXXXX | |
X X X X X X X X X X X X X X X X X X X X X X X | |
X X X XXX X XXX XXXXXXX X XXX XXX X XXX XXXXXXXXX XXXXX XXXXX XXXXX XXXXX X X X XXX XXX XXXXX X XXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X | |
X XXX XXX X XXXXX X XXXXXXXXX X X XXX X X XXX X XXXXXXX X XXXXXXXXX XXX X X XXXXXXXXXXXXX XXX XXX X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X XXXXXXX XXX XXX X XXXXXXXXX XXXXXXX XXXXXXXXX XXXXX X XXX XXX X XXX XXX X X XXXXX XXXXX XXXXX X X X | |
X X X X X X X X X X X X X X X X X X X X X | |
X X X XXX X XXX XXXXXXXXXXX XXX X XXX X XXXXXXXXXXXXX XXXXXXX X X XXXXXXXXXXXXXXXXXXX X XXXXX X XXXXX | |
X X X X X X X X X X X X X X X X X X X X X | |
X XXX XXXXXXX XXX XXX XXXXX XXX X XXX XXX XXXXX XXX X X X XXX XXX X XXX X XXX X X X XXXXXXXXX XXX XXX | |
X X X X X X X X X X X X X X X X X X X X X X X | |
XXXXXXXXX X XXX XXXXXXX X XXXXX XXX XXX X XXXXXXX XXX XXX XXX XXX XXXXX X X XXX XXXXXXX X XXX X X X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXX X XXX X X XXXXXXX X XXXXXXXXXXXXX XXXXXXX XXXXX XXX XXXXXXX XXXXXXXXXXX XXXXX X X XXX XXX XXX XXX | |
X X X X X X X X X X X X X X X X X X X X X X X | |
X XXXXX X XXXXXXXXXXXXX XXX X XXX XXXXX XXXXXXX X X X X XXXXXXXXX X XXXXXXX XXXXX XXX X XXXXX XXXXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXX XXXXX X X X X X X XXX XXX XXX X X XXXXX XXXXX XXXXXXX X XXXXX X X X XXX X X XXX XXX X XXXXX XXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X XXX X XXX XXXXX X XXXXXXX X X X X XXX X XXXXX XXX X XXX XXX XXXXXXXXX X XXX XXX XXX X X XXXXX XXXXX | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXX X X X XXX X X X X XXX XXXXXXX XXXXXXXXXXX X X X X X XXX XXXXX X X X X X XXXXX X X XXX XXXXXXX X X | |
X X X X X X X X X X X X X X X X X X X X X X X | |
XXX XXX XXX XXXXXXX X X X X X XXX X X X XXXXXXX XXXXXXX XXXXX X X XXX XXXXXXXXX XXX X XXX XXXXXXXXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXX XXXXX XXX X X X XXXXXXXXXXXXX XXXXX XXXXXXXXX X X X X XXXXXXXXXXXXX XXX XXX XXXXX X X XXXXXXXXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X X XXX XXX X X XXX XXX XXX X XXXXX XXXXX XXXXX X XXXXXXXXX X XXXXX X X X XXX X X XXXXX XXX X X X X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X X X XXXXX XXX X X X XXXXX XXX X X XXX X X X XXXXX X X X XXX X X XXXXXXXXX X XXXXXXX X XXX XXXXX XXX | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X X XXX X X X XXX XXX XXX XXXXXXX X XXXXXXXXX X X X X XXX XXXXX XXXXX X X XXX XXX XXXXX XXX X XXX X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXX XXXXXXXXXXX XXXXX XXX X XXX XXX XXX X XXX XXXXX X X XXXXX XXXXX XXX XXXXX X XXX X XXX XXXXX X X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X X XXXXX X X X X XXXXX XXX X XXX XXXXX XXXXX XXXXX X XXXXX X XXX XXX X X X X XXXXX XXXXXXX X XXXXXXX | |
X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXXXX X X X X XXX XXXXXXX X X X XXX XXX XXX XXX X XXXXX XXX X X XXX XXXXXXX XXX XXXXXXX XXXXXXX XXXXX | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXXXX XXX XXX XXX X XXXXXXXXX XXX X XXX X X XXX X X XXXXX X X XXX X X XXXXX X XXX X X XXXXX X X X X X | |
X X X X X X X X X X X X X X X X X X X X X | |
XXX X X X XXXXX XXXXX XXXXXXX XXXXX X XXXXX X X X XXXXXXX X X XXXXX XXXXXXX XXXXXXXXX X X XXXXXXXXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X XXXXX X X XXX X XXX X X XXXXXXXXX XXXXXXXXXXX X X XXX X X XXX X XXX XXX XXXXX XXX XXXXXXXXX X XXXXX | |
X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X X X XXXXXXXXXXX XXXXX X X XXXXX X X XXX X X XXXXX X X XXXXXXXXXXX X XXX XXXXX XXX XXXXXXX XXX XXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X | |
X X XXX XXXXXXXXXXX XXX XXXXXXX XXXXXXX X X X X X X X XXXXXXX X XXXXX X X X XXX XXXXXXXXX X X X XXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X XXXXXXXXX XXXXX XXX X X XXXXX XXXXX XXXXXXX XXX XXX XXX X X XXXXX XXXXX X X XXXXX XXX X X XXX XXXXX | |
X X X X X X X X X X X X X X X X X X X X X X X X | |
X X XXX XXX XXX XXX XXX XXXXX XXXXX XXXXX X X X X X XXXXXXX XXX X XXXXXXX X XXX XXXXXXX XXX X X X XXX | |
X X X X X X X X X X X X X X X X X X X X X X X | |
X XXXXXXX XXX XXX XXX XXX XXX X X XXXXXXX X X XXX XXXXX XXXXX XXX X X X X XXXXXXX XXXXXXX XXX XXXXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X | |
XXXXX X XXX XXX XXX X X XXX X XXXXXXXXXXX XXXXXXX X X X XXXXXXXXX XXX XXX XXXXX X XXX X XXXXX XXXXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X X X X XXXXXXXXXXXXXXXXX XXX XXX XXX X X XXXXXXX XXX X X XXXXX X XXXXXXXXXXX XXXXX XXX X XXX XXX X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXXXXXX X X XXXXX X X XXX X XXX X X XXXXX X XXX XXXXX XXXXX X XXXXXXX X XXXXX X X XXX XXXXXXX X X XXX | |
X X X X X X X X X X X X X X X X X X X X X X X | |
XXX XXXXX X X X X XXXXXXX XXXXX X XXXXX XXX X XXXXXXX X XXX X XXX X XXXXX XXXXXXX XXXXXXX XXX X XXXXX | |
X X X X X X X X X X X X X X X X X X X X X | |
X XXXXXXX XXX X XXXXX X X XXX X X XXXXX XXXXXXX X XXXXX XXXXXXXXX XXXXX XXXXX X X XXX X X XXXXX X XXX | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXX XXX X XXXXXXX X X X X X XXXXXXX X XXXXXXXXXXX XXX XXX XXXXXXX X XXX XXXXX X XXXXXXXXX XXX X X X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X X XXXXX XXX X X XXX XXXXX XXXXXXX XXXXXXXXXXX X X XXX X XXXXXXXXX X X XXX X X X X X XXX X XXX XXXXX | |
X X X X X X X X X X X X X X X X X X X X X X | |
X X X XXX X X XXXXX XXX XXXXX XXXXX X XXXXX X XXXXX X XXX X X X X XXX XXXXX X XXXXXXX XXXXXXX XXX X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X X XXXXX XXXXX X X X X X X XXX XXX X X XXX X X X XXX XXX X XXX XXXXX XXX X X XXXXXXXXX XXX X X X X X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
XXX XXXXXXX X XXXXXXX X XXX XXX XXXXX X XXXXXXX XXX X XXX X X XXX XXX X XXXXX XXXXXXX XXX XXXXX XXX X | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X X X X XXX X X X X X X X XXX XXXXXXXXX XXX XXXXX X X XXX X XXX X X X X XXXXXXXXXXX X XXX XXX X XXX X | |
X X X X X X X X X X X X X X X X X X X | |
X XXXXXXX X XXX X XXX XXX XXX XXXXX XXXXXXXXXXX XXX X XXXXX X XXX XXX XXXXXXXXX XXXXX XXX X X XXXXXXX | |
X X X X X X X X X X X X X X X X X X X X X X X X X X X X X X | |
X XXX XXXXXXX XXXXX XXX XXX XXX X X XXX XXXXX XXX X X XXXXX X XXX XXX X XXX X X X X XXXXX XXX X X X X | |
X X X X X X X X X X X X X X X X X X | |
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX S |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <stdio.h> | |
#include <stdlib.h> | |
#include <unistd.h> | |
#include <limits.h> | |
#include <math.h> | |
#include <time.h> | |
#define MAZE_FILE "maze-simple-large" | |
/* #define DEBUG_TIME 1 */ | |
// We start by defining various structures and operations on them | |
// Location struct /////////////////////////////////////////////////////////////////////// | |
typedef struct Location { | |
int x; | |
int y; | |
} Location; | |
Location *Location_new(int x, int y) { | |
Location *location = (Location *) malloc(sizeof(Location)); | |
location->x = x; | |
location->y = y; | |
return location; | |
} | |
Location *Location_copy(Location *location) { | |
return Location_new(location->x, location->y); | |
} | |
void Location_destroy(Location *location) { | |
free(location); | |
} | |
int Location_squareDistance(Location *first, Location *second) { | |
return pow(first->x - second->x, 2) + pow(first->y - second->y, 2); | |
} | |
int Location_equals(Location *first, Location *second) { | |
return first->x == second->x && first->y == second->y; | |
} | |
// Maze struct /////////////////////////////////////////////////////////////////////// | |
typedef enum CellType { | |
EMPTY, | |
BLOCKED, | |
START, | |
END | |
} CellType; | |
CellType parseCellType(char character) { | |
switch (character) { | |
case ' ': | |
return EMPTY; | |
case 'X': | |
return BLOCKED; | |
case 'S': | |
return START; | |
case 'E': | |
return END; | |
default: | |
printf("Failed to parse maze symbol %c\n", character); | |
exit(EXIT_FAILURE); | |
} | |
} | |
typedef struct Maze { | |
size_t width; | |
size_t height; | |
CellType *cells; | |
} Maze; | |
Maze *Maze_new(size_t width, size_t height) { | |
Maze *maze = (Maze *) malloc(sizeof(Maze)); | |
maze->width = width; | |
maze->height = height; | |
maze->cells = (CellType *) malloc(sizeof(CellType) * width * height); | |
return maze; | |
} | |
CellType Maze_getCell(Maze *maze, size_t x, size_t y) { | |
return maze->cells[maze->height * y + x]; | |
} | |
void Maze_findCell(Maze *maze, CellType cell, Location *location) { | |
for (size_t y = 0; y < maze->height; y++) { | |
for (size_t x = 0; x < maze->width; x++) { | |
if (Maze_getCell(maze, x, y) == cell) { | |
location->x = x; | |
location->y = y; | |
return; | |
} | |
} | |
} | |
printf("Failed to find cell %d\n", cell); | |
} | |
int Maze_inBounds(Maze *maze, Location *location) { | |
return ( | |
location->x >= 0 && | |
location->x < maze->width && | |
location->y >= 0 && | |
location->y < maze->height | |
); | |
} | |
void Maze_setCell(Maze *maze, size_t x, size_t y, CellType value) { | |
maze->cells[maze->height * y + x] = value; | |
} | |
void Maze_destroy(Maze *maze) { | |
free(maze->cells); | |
free(maze); | |
} | |
// Parsing /////////////////////////////////////////////////////////////////////// | |
Maze *parseMazeFile(const char *fileName) { | |
FILE *file = fopen(fileName, "r"); | |
if (file == NULL) { | |
printf("Failed to open maze file\n"); | |
exit(EXIT_FAILURE); | |
} | |
size_t index = 0; | |
char *line = NULL; | |
size_t length = 0; | |
ssize_t readSize; | |
size_t mazeWidth; | |
// todo: add start/end to the maze | |
Maze *maze; | |
while ((readSize = getline(&line, &length, file)) != -1) { | |
if (index == 0) { | |
sscanf(line, "%zu", &mazeWidth); | |
} else if (index == 1) { | |
size_t mazeHeigth; | |
sscanf(line, "%zu", &mazeHeigth); | |
maze = Maze_new(mazeWidth, mazeHeigth); | |
} else { | |
for (size_t charIndex = 0; charIndex < mazeWidth; charIndex++) { | |
/* printf("Checking cell %zu %zu\n", charIndex, index - 2); */ | |
CellType cell = parseCellType(line[charIndex]); | |
Maze_setCell(maze, charIndex, index - 2, cell); | |
} | |
} | |
index++; | |
} | |
fclose(file); | |
if (line) { | |
free(line); | |
} | |
return maze; | |
} | |
// Location priority queue structure, along with supporting crap //////////////////////////////////////////// | |
typedef struct LocationNode { | |
Location *location; | |
int priority; | |
struct LocationNode *next; | |
} LocationNode; | |
LocationNode *LocationNode_new(Location *location, int priority) { | |
LocationNode *node = (LocationNode *) malloc(sizeof(LocationNode)); | |
node->location = location; | |
node->priority = priority; | |
node->next = NULL; | |
return node; | |
} | |
void LocationNode_destroy(LocationNode *node) { | |
if (node->next != NULL) { | |
LocationNode_destroy(node->next); | |
} | |
Location_destroy(node->location); | |
free(node); | |
} | |
// TODO: turn it into a priority queue by making it a heap | |
typedef struct LocationStack { | |
LocationNode *root; | |
} LocationStack; | |
LocationStack *LocationStack_new() { | |
LocationStack *stack = (LocationStack *) malloc(sizeof(LocationStack)); | |
stack->root = NULL; | |
return stack; | |
} | |
void LocationStack_destroyAll(LocationStack *stack) { | |
if (stack->root != NULL) { | |
LocationNode_destroy(stack->root); | |
} | |
free(stack); | |
} | |
Location *LocationStack_pop(LocationStack *stack) { | |
LocationNode *rootNode = stack->root; | |
Location *location = rootNode->location; | |
stack->root = rootNode->next; | |
free(rootNode); | |
return location; | |
} | |
char LocationStack_isEmpty(LocationStack *stack) { | |
return stack->root == NULL; | |
} | |
void LocationStack_push(LocationStack *stack, Location *location, int priority) { | |
LocationNode *node = LocationNode_new(location, priority); | |
if (stack->root != NULL) { | |
LocationNode *prev = NULL; | |
LocationNode *current = stack->root; | |
while (1) { | |
if (current == NULL) { | |
prev->next = node; | |
break; | |
} | |
if (current->priority > node->priority) { | |
// found right spot | |
if (prev != NULL) { | |
prev->next = node; | |
node->next = current; | |
} else { | |
stack->root = node; | |
node->next = current; | |
} | |
break; | |
} | |
prev = current; | |
current = current->next; | |
} | |
} else { | |
stack->root = node; | |
} | |
} | |
// TODO: super suboptimal | |
char LocationStack_contains(LocationStack *stack, Location *location, int priority) { | |
LocationNode *current = stack->root; | |
while (current != NULL && current->priority <= priority) { // priority is a tiny optimization, we know it is sorted so will not be in otherwise | |
if (Location_equals(current->location, location)) { | |
return 1; | |
} | |
current = current->next; | |
} | |
return 0; | |
} | |
void LocationNode_print(LocationNode *node) { | |
printf("LocationNode(x: %d, y: %d, priority: %d)\n", node->location->x, node->location->y, node->priority); | |
if (node->next != NULL) { | |
LocationNode_print(node->next); | |
} | |
} | |
void LocationStack_print(LocationStack *stack) { | |
if (stack->root != NULL) { | |
LocationNode_print(stack->root); | |
} | |
} | |
int addWithInfinity(int one, int two) { | |
if (one == INT_MAX || two == INT_MAX) { | |
return INT_MAX; | |
} | |
return one + two; | |
} | |
// Path struct /////////////////////////////////////////////////////////////////////// | |
typedef struct PathNode { | |
Location *location; | |
struct PathNode *next; | |
} PathNode; | |
void PathNode_destroyAll(PathNode *node) { | |
if (node->next != NULL) { | |
PathNode_destroyAll(node->next); | |
} | |
Location_destroy(node->location); | |
free(node); | |
} | |
PathNode *PathNode_new(Location *location) { | |
PathNode *node = (PathNode *) malloc(sizeof(PathNode)); | |
node->location = location; | |
node->next = NULL; | |
return node; | |
} | |
typedef struct Path { | |
PathNode *root; | |
} Path; | |
Path *Path_new() { | |
Path *path = (Path *) malloc(sizeof(Path)); | |
path->root = NULL; | |
return path; | |
} | |
void Path_destroyAll(Path *path) { | |
if (path->root != NULL) { | |
PathNode_destroyAll(path->root); | |
} | |
free(path); | |
} | |
void Path_print(Path *path) { | |
printf("Path("); | |
PathNode *currentNode = path->root; | |
while (currentNode != NULL) { | |
printf("(%d, %d)", currentNode->location->x, currentNode->location->y); | |
currentNode = currentNode->next; | |
} | |
printf(")\n"); | |
} | |
void Path_draw(Maze *maze, Path *path) { | |
char pathMatrix[maze->width * maze->height]; // This can only contain pointers to the stack! | |
for (size_t i = 0; i < maze->width * maze->height; i++) { | |
pathMatrix[i] = 0; | |
} | |
PathNode *current = path->root; | |
while (current != NULL) { | |
pathMatrix[maze->width * current->location->y + current->location->x] = 1; | |
current = current->next; | |
} | |
for (size_t y = 0; y < maze->height; y++) { | |
for (size_t x = 0; x < maze->width; x++) { | |
if (pathMatrix[maze->width * y + x]) { | |
printf("\033[0;32mP\033[0m"); | |
} else { | |
switch(Maze_getCell(maze, x, y)) { | |
case BLOCKED: | |
printf("\033[0;31mX\033[0m"); | |
break; | |
case START: | |
printf("S"); | |
break; | |
case END: | |
printf("E"); | |
break; | |
default: | |
printf(" "); | |
} | |
} | |
} | |
printf("\n"); | |
} | |
} | |
void Path_prepend(Path *path, Location *location) { | |
PathNode *node = PathNode_new(location); | |
node->next = path->root; | |
path->root = node; | |
} | |
// Core a* implementation /////////////////////////////////////////////////////////////////////// | |
Path *reconstructPath(Maze *maze, Location **cameFrom, Location *end) { | |
Path *path = Path_new(); | |
Location *current = Location_copy(end); | |
Path_prepend(path, current); | |
while (Maze_getCell(maze, current->x, current->y) != START) { | |
Location *location = cameFrom[(maze->width * current->y) + current->x]; | |
if (location != NULL) { | |
current = Location_copy(location); | |
Path_prepend(path, current); | |
} else { | |
current = NULL; | |
} | |
} | |
return path; | |
} | |
Path *solve(Maze *maze) { | |
Location *start = Location_new(0, 0); // will be put on location stack so has to be memory-managed | |
Location end; | |
Maze_findCell(maze, START, start); | |
Maze_findCell(maze, END, &end); | |
Location *cameFrom[maze->width * maze->height]; | |
int gScore[maze->width * maze->height]; | |
int fScore[maze->width * maze->height]; | |
for (size_t i = 0; i < maze->width * maze->height; i++) { | |
cameFrom[i] = NULL; | |
gScore[i] = INT_MAX; | |
fScore[i] = INT_MAX; | |
} | |
LocationStack *stack = LocationStack_new(); | |
int startDistance = Location_squareDistance(start, &end); | |
LocationStack_push(stack, start, startDistance); | |
gScore[start->y * maze->height + start->x] = 0; | |
fScore[start->y * maze->height + start->x] = startDistance; | |
Path *path = NULL; // final result | |
while (!LocationStack_isEmpty(stack)) { | |
Location *location = LocationStack_pop(stack); // once it is popped, we HAVE to destroy it manually | |
size_t locationIndex = location->y * maze->width + location->x; | |
if (Maze_getCell(maze, location->x, location->y) == END) { | |
path = reconstructPath(maze, cameFrom, location); | |
Location_destroy(location); | |
break; | |
} | |
size_t x = location->x; | |
size_t y = location->y; | |
// TODO: have these be indeces into a static location pointers array that we initialize once | |
Location *neighbors[4] = { | |
Location_new(x - 1, y), | |
Location_new(x + 1, y), | |
Location_new(x, y - 1), | |
Location_new(x, y + 1), | |
}; | |
for (size_t i = 0; i < 4; i++) { | |
char didPushNeighbor = 0; | |
Location *neighbor = neighbors[i]; | |
size_t neighborIndex = neighbor->y * maze->width + neighbor->x; | |
if (Maze_inBounds(maze, neighbor) && Maze_getCell(maze, neighbor->x, neighbor->y) != BLOCKED) { | |
int tentativeGScore = addWithInfinity(gScore[locationIndex], Location_squareDistance(location, neighbor)); | |
int neighborGScore = gScore[neighborIndex]; | |
if (tentativeGScore < neighborGScore) { | |
// we manage camefrom's elements' lifetime separately. At the end we delete everything non-null, meanwhile we reuse. | |
if (cameFrom[neighborIndex] == NULL) { | |
Location *locationCopy = Location_copy(location); | |
cameFrom[neighborIndex] = locationCopy; | |
} else { | |
Location *last = cameFrom[neighborIndex]; | |
last->x = location->x; | |
last->y = location->y; | |
} | |
gScore[neighborIndex] = tentativeGScore; | |
int distance = Location_squareDistance(neighbor, &end); | |
fScore[neighborIndex] = addWithInfinity(tentativeGScore, distance); | |
if (!LocationStack_contains(stack, neighbor, distance)) { | |
LocationStack_push(stack, neighbor, distance); | |
didPushNeighbor = 1; | |
} | |
} | |
} | |
if (!didPushNeighbor) { | |
Location_destroy(neighbor); | |
} | |
} | |
Location_destroy(location); | |
} | |
for (size_t i = 0; i < maze->width * maze->height; i++) { | |
if (cameFrom[i] != NULL) { | |
Location_destroy(cameFrom[i]); | |
} | |
} | |
LocationStack_destroyAll(stack); | |
return path; | |
} | |
int doubleCompare(const void *a, const void *b) { | |
const double *da = (const double *)a; | |
const double *db = (const double *)b; | |
return *da - *db; | |
} | |
int main(int argc, char **argv) { | |
if (argc < 2) { | |
printf("Please provide a file with a maze to solve\n"); | |
return 1; | |
} | |
Maze *maze = parseMazeFile(argv[1]); | |
#ifdef DEBUG_TIME | |
double solveTimes[1000]; | |
double solveAvg = 0; | |
printf("Doing 1000 solves\n"); | |
for (size_t i = 0; i < 1000; i++) { | |
clock_t start = clock(); | |
Path *path = solve(maze); | |
clock_t end = clock(); | |
Path_destroyAll(path); | |
solveTimes[i] = ((double) (end - start)) / CLOCKS_PER_SEC; | |
solveAvg += solveTimes[i]; | |
} | |
solveAvg = solveAvg / 1000.0; | |
qsort(solveTimes, 1000, sizeof(double), doubleCompare); | |
printf("Average solve took %lf ms\n", solveAvg * 1000); | |
printf("Mean solve took %lf ms\n", solveTimes[500] * 1000); | |
#endif | |
Path *path = solve(maze); | |
Path_draw(maze, path); | |
Path_destroyAll(path); | |
Maze_destroy(maze); | |
return EXIT_SUCCESS; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment