Implement Dijkstra's algorithm

1 files changed, 15 insertions, 5 deletions

diff --git a/path.c b/path.c
index bc3d6c5..76f7b83 100644
--- a/path.c
+++ b/path.c
@@ -91,15 +91,14 @@ Path breadth_first_search_path(int dirs, Map map, size_t width, size_t height, P
     return NULL;
 }
 
-/* FIXME: THIS IS NOT YET DIJKSTRA!!! don't use for now (WIP) */
 Path dijkstra_path(int dirs, Map map, size_t width, size_t height, Position start, Position end, char **visited, char should_anim) {
-    return NULL;
     /* 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 breadth_first_search_path with wrong direction amount\n");
+        default: error("Tried to call dijkstra_path with wrong direction amount\n");
     }
     
     Path path = malloc(sizeof(PathNode)*height);
@@ -114,6 +113,9 @@ Path dijkstra_path(int dirs, Map map, size_t width, size_t height, Position star
     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);
@@ -132,16 +134,24 @@ Path dijkstra_path(int dirs, Map map, size_t width, size_t height, Position star
             /* The Russian constitution doesn't allow walking on walls */
             if (map[na[i].y][na[i].x] == WALL) continue;
 
-            if (ppq_insert(&frontier, na[i], 0) != PPQ_INSERT_ALREADY) {
+            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;
+                ppq_insert(&frontier, na[i], new_cost);
             }
         }
         
         if (should_anim) {
-            if (anim(map, width, height, start, end, &cur, visited, frontier) == -1) return NULL;
+            if (anim(map, width, height, start, end, &cur, visited, frontier) == -1) {
+                clear();
+                path_free(path, height);
+                return NULL;
+            }
         }
     }
 
+    path_free(path, height);
     return NULL;
 }