Implement Dijkstra's algorithm

2 files changed, 19 insertions, 8 deletions

diff --git a/main.c b/main.c
index eded452..317a237 100644
--- a/main.c
+++ b/main.c
@@ -56,6 +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;
 
     size_t mwidth = 20;  /* Maze width  */
     size_t mheight = 10; /* Maze height */
@@ -134,7 +135,7 @@ int main(int argc, char **argv) {
 
     char **visited = visited_new(width, height);
     Path path = NULL;
-    path = breadth_first_search_path(dirs, map, width, height, start_pos, end_pos, visited, anim);
+    path = path_func(dirs, map, width, height, start_pos, end_pos, visited, anim);
 
     if (bmp_only) {
         map_to_bmp(map, width, height, start_pos, end_pos, path, visited, bmp_filename);
@@ -174,7 +175,7 @@ int main(int argc, char **argv) {
 
                           map = rbt_maze_map(mwidth, mheight, rand());
                           visited = visited_new(width, height);
-                          path = breadth_first_search_path(dirs, map, width, height, start_pos, end_pos, visited, anim);
+                          path = path_func(dirs, map, width, height, start_pos, end_pos, visited, anim);
                       }
                       break;
             case 's':
@@ -188,7 +189,7 @@ int main(int argc, char **argv) {
                           map_free(map, height);
                           map = rbt_maze_map(mwidth, mheight, rand());
                           path_free(path, height);
-                          path = breadth_first_search_path(dirs, map, width, height, start_pos, end_pos, visited, anim);
+                          path = path_func(dirs, map, width, height, start_pos, end_pos, visited, anim);
                       }
                 break;
             case 'q': map_free(map, height); path_free(path, height); endwin(); return 0;
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;
 }