Implement A*, finally

5 files changed, 105 insertions, 14 deletions

diff --git a/Makefile b/Makefile
index 218e81f..360214f 100644
--- a/Makefile
+++ b/Makefile
@@ -2,10 +2,10 @@ CC=gcc
 EXECUTABLE=astar
 
 astar: *.c *.h
-	$(CC) -Wall -Wpedantic -Wextra -Werror -O3 -o $(EXECUTABLE) priority_queue.c map.c stack.c path.c bmp.c main.c -lncurses
+	$(CC) -Wall -Wpedantic -Wextra -Werror -O3 -o $(EXECUTABLE) priority_queue.c map.c stack.c path.c bmp.c main.c -lncurses -lm
 
 debug: *.c *.h
-	$(CC) -Wall -g -o $(EXECUTABLE) priority_queue.c map.c stack.c path.c bmp.c main.c -lncurses
+	$(CC) -Wall -g -o $(EXECUTABLE) priority_queue.c map.c stack.c path.c bmp.c main.c -lncurses -lm
 
 clean: 
 	rm ./$(EXECUTABLE)
diff --git a/main.c b/main.c
index 317a237..c35afdd 100644
--- a/main.c
+++ b/main.c
@@ -56,7 +56,7 @@ int main(int argc, char **argv) {
     Position start_pos, end_pos;
     size_t width, height;
     Map map = NULL;
-    Path (*path_func)(int, Map, size_t, size_t, Position, Position, char **, char) = &dijkstra_path;
+    Path (*path_func)(int, Map, size_t, size_t, Position, Position, char **, char) = &astar_path;
 
     size_t mwidth = 20;  /* Maze width  */
     size_t mheight = 10; /* Maze height */
@@ -72,6 +72,7 @@ int main(int argc, char **argv) {
     /* Handle args.
      * Currently supported:
      *  -a to animate the pathfinding (get input after each step)
+     *  -d for Dijkstra
      *  -m {width}x{height} to give a maze of given size
      *  -f {filename} to load a map from the filename
      *  -b {filename} to just generate a bitmap, no interface
@@ -80,9 +81,10 @@ int main(int argc, char **argv) {
     /* TODO: Argument to choose the algorithm */
     /* TODO: Argument to set seed */
     int opt;
-    while ((opt = getopt(argc, argv, ":am:f:b:48")) != -1) {
+    while ((opt = getopt(argc, argv, ":adm:f:b:48")) != -1) {
         switch (opt) {
             case 'a': anim = 1; break;
+            case 'd': path_func = &dijkstra_path; break;
             case 'm':
                       if (sscanf(optarg, "%zux%zu", &mwidth, &mheight) != 2) error("Wrong maze size string in argument\n");
                       is_maze = 1;
diff --git a/path.c b/path.c
index 76f7b83..ea2b4e0 100644
--- a/path.c
+++ b/path.c
@@ -2,6 +2,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <curses.h>
+#include <math.h>
 
 #include "path.h"
 #include "map.h"
@@ -17,10 +18,10 @@ int anim(Map map, size_t width, size_t height, Position start, Position end, Pos
         draw_map(map, width, height, offset_x, offset_y, start, end, cur, NULL, visited, frontier);
         mvprintw(height+2 + offset_y, offset_x, "cur: %zu %zu", cur->x, cur->y);
         switch (getch()) {
-            case 'h': offset_x -= 2; break; 
-            case 'l': offset_x += 2; break; 
-            case 'j': offset_y += 1; break; 
-            case 'k': offset_y -= 1; break; 
+            case 'h': offset_x -= 2; break;
+            case 'l': offset_x += 2; break;
+            case 'j': offset_y += 1; break;
+            case 'k': offset_y -= 1; break;
             case 'q': return -1;
             default:  return 0;
         }
@@ -36,7 +37,7 @@ Path breadth_first_search_path(int dirs, Map map, size_t width, size_t height, P
         case 8: neighbours = &neighbours_8dir; break;
         default: error("Tried to call breadth_first_search_path with wrong direction amount\n");
     }
-    
+
     Path path = malloc(sizeof(PathNode)*height);
     if (path == NULL) return NULL;
 
@@ -73,7 +74,7 @@ Path breadth_first_search_path(int dirs, Map map, size_t width, size_t height, P
                 path[na[i].y][na[i].x].parent = cur;
             }
         }
-        
+
         if (should_anim) {
             if (anim(map, width, height, start, end, &cur, visited, frontier) == -1) {
                 clear();
@@ -94,13 +95,13 @@ Path breadth_first_search_path(int dirs, Map map, size_t width, size_t height, P
 Path dijkstra_path(int dirs, Map map, size_t width, size_t height, Position start, Position end, char **visited, char should_anim) {
     /* The function to use to find neighbours */
     unsigned int (*neighbours)(Position[], size_t[], Position, size_t, size_t, char**) = NULL;
-   
+
     switch (dirs) {
         case 4: neighbours = &neighbours_4dir; break;
         case 8: neighbours = &neighbours_8dir; break;
         default: error("Tried to call dijkstra_path with wrong direction amount\n");
     }
-    
+
     Path path = malloc(sizeof(PathNode)*height);
     if (path == NULL) return NULL;
 
@@ -141,7 +142,75 @@ Path dijkstra_path(int dirs, Map map, size_t width, size_t height, Position star
                 ppq_insert(&frontier, na[i], new_cost);
             }
         }
-        
+
+        if (should_anim) {
+            if (anim(map, width, height, start, end, &cur, visited, frontier) == -1) {
+                clear();
+                path_free(path, height);
+                return NULL;
+            }
+        }
+    }
+
+    path_free(path, height);
+    return NULL;
+}
+
+Path astar_path(int dirs, Map map, size_t width, size_t height, Position start, Position end, char **visited, char should_anim) {
+    /* The function to use to find neighbours */
+    unsigned int (*neighbours)(Position[], size_t[], Position, size_t, size_t, char**) = NULL;
+    /* The heuristic function */
+    size_t (*heuristic)(Position, Position) = NULL;
+
+    /* TODO: maybe a sort of a demo with different heuristics would be cool idk */
+    switch (dirs) {
+        case 4: neighbours = &neighbours_4dir; heuristic = &manhattan_distance; break;
+        case 8: neighbours = &neighbours_8dir; heuristic = &diagonal_distance; break;
+        default: error("Tried to call dijkstra_path with wrong direction amount\n");
+    }
+
+    Path path = malloc(sizeof(PathNode)*height);
+    if (path == NULL) return NULL;
+
+    for (size_t row = 0; row < height; row++) {
+        path[row] = malloc(sizeof(PathNode) * width);
+        if (path[row] == NULL) return NULL;
+        memset(path[row], 0, width * sizeof(PathNode));
+    }
+
+    PositionPQ *frontier = ppq_new(start, 0);
+
+    visited_clear(visited, width, height);
+    size_t cost_so_far[height][width];
+    memset(cost_so_far, 0xFF, sizeof(size_t) * height * width);
+    cost_so_far[start.y][start.x] = 0;
+
+    while (frontier != NULL) {
+        Position cur = ppq_pop(&frontier);
+        visited[cur.y][cur.x] = 1;
+
+        if (cur.x == end.x && cur.y == end.y) {
+            ppq_free(frontier);
+            return path; /* Found path */
+        }
+
+        Position na[dirs];
+        size_t costs[dirs];
+        unsigned int nc = neighbours(na, costs, cur, width, height, visited);
+
+        for (unsigned int i = 0; i < nc; i++) {
+            /* The Russian constitution doesn't allow walking on walls */
+            if (map[na[i].y][na[i].x] == WALL) continue;
+
+            size_t new_cost = cost_so_far[cur.y][cur.x] + costs[i];
+            if (new_cost < cost_so_far[na[i].y][na[i].x]) {
+                cost_so_far[na[i].y][na[i].x] = new_cost;
+                path[na[i].y][na[i].x].parent = cur;
+                size_t priority = new_cost + heuristic(na[i], end);
+                ppq_insert(&frontier, na[i], priority);
+            }
+        }
+
         if (should_anim) {
             if (anim(map, width, height, start, end, &cur, visited, frontier) == -1) {
                 clear();
@@ -172,6 +241,23 @@ size_t manhattan_distance(Position a, Position b) {
     return d;
 }
 
+/* FIXME: something faster, maybe? or at least better written. */
+size_t diagonal_distance(Position a, Position b) {
+    size_t one = 0;
+    size_t two = 0;
+    if (a.x > b.x) {
+        one = (a.x - b.x) * COST_ORTHOGONAL;
+    } else {
+        one = (b.x - a.x) * COST_ORTHOGONAL;
+    }
+    if (a.y > b.y) {
+        two = (a.y - b.y) * COST_ORTHOGONAL;
+    } else {
+        two = (b.y - a.y) * COST_ORTHOGONAL;
+    }
+    return (size_t) hypot(one, two);
+}
+
 void path_free(Path path, size_t height) {
     if (path == NULL) return;
     for (size_t i = 0; i < height; i++) {
diff --git a/path.h b/path.h
index f9c9c75..37c410a 100644
--- a/path.h
+++ b/path.h
@@ -7,8 +7,10 @@
 /* dirs can be 4 or 8 to disallow or allow diagonal movement */
 Path breadth_first_search_path(int dirs, Map map, size_t width, size_t height, Position start, Position end, char **visited, char should_anim);
 Path dijkstra_path(int dirs, Map map, size_t width, size_t height, Position start, Position end, char **visited, char should_anim);
+Path astar_path(int dirs, Map map, size_t width, size_t height, Position start, Position end, char **visited, char should_anim);
 
 size_t manhattan_distance(Position a, Position b);
+size_t diagonal_distance(Position a, Position b);
 
 void path_free(Path path, size_t height);
 
diff --git a/priority_queue.c b/priority_queue.c
index d899103..c90f661 100644
--- a/priority_queue.c
+++ b/priority_queue.c
@@ -35,7 +35,8 @@ int ppq_insert(PositionPQ **ppq, Position pos, size_t priority) {
 
     PositionPQ *temp = *ppq;
 
-    while(temp->next != NULL && temp->next->priority <= priority) {
+    /* The '=' is commented out because we want to insert before, for speed. To insert after, uncomment it*/
+    while(temp->next != NULL && temp->next->priority </*=*/ priority) {
         if (temp->pos.x == pos.x && temp->pos.y == pos.y && temp->priority <= priority) {
             free(n);
             return PPQ_INSERT_ALREADY;