Implement A*, finally

1 files changed, 95 insertions, 9 deletions

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++) {