Add API function to solve maze

2022-01-18 14:11:02 -05:00
parent 05d5cf3d79
commit 5169b4223a
3 changed files with 121 additions and 18 deletions
--- a/lib/path.c
+++ b/lib/path.c
@@ -0,0 +1,109 @@
+#include <mazemaker.h>
+#include <string.h>
+#include "pq.h"
+#include "set.h"
+
+DEFINE_PQ(maze_path_t, PathPQ, unsigned int);
+
+static void path_copy(maze_path_t const* src, maze_path_t* dst) {
+    dst->length = src->length;
+    dst->nodes = calloc(dst->length, sizeof(maze_point_t));
+    memcpy(dst->nodes, src->nodes, src->length * sizeof(maze_point_t));
+}
+
+static void path_copy_append(maze_path_t const* src, maze_path_t* dst, maze_point_t pt) {
+    dst->length = src->length + 1;
+    dst->nodes = calloc(dst->length, sizeof(maze_point_t));
+    memcpy(dst->nodes, src->nodes, src->length * sizeof(maze_point_t));
+    dst->nodes[dst->length - 1] = pt;
+}
+
+int mazemaker_solve(mazegrid_t const* maze, maze_path_t* path) {
+    int code = 0;
+    struct node_set* set = node_set_new(maze->width, maze->height);
+    NEW_PQ(PathPQ, pq);
+
+    // put starting point in the queue
+    maze_path_t start;
+    start.length = 1;
+    start.nodes = malloc(sizeof(maze_point_t));
+    start.nodes[0].x = 0;
+    start.nodes[0].y = 0;
+
+    PUSH_PQ(pq, PathPQ, start, 0);
+    while (!EMPTY_PQ(pq)) {
+        maze_path_t top;
+        unsigned int dist;
+        size_t x, y;
+        POP_PQ(pq, PathPQ, top, dist);
+
+        x = top.nodes[top.length - 1].x;
+        y = top.nodes[top.length - 1].y;
+        node_set_add(set, x, y);
+
+        // check to see if we've reached the end 
+        if ((x == maze->width - 1) && (y == maze->height - 1)) {
+            path_copy(&top, path);
+            code = 1;
+            break;
+        }
+
+        // otherwise check for unexplored paths in all directions...
+        
+        // ... up
+        if ((y < maze->height) && (mazegrid_get_edge(maze, x, y, EDGE_UP) != 0)) {
+            if (!node_set_contains(set, x, y + 1)) {
+                maze_path_t newpath;
+                maze_point_t pt = { .x = x, .y = y + 1 };
+                path_copy_append(&top, &newpath, pt);
+                PUSH_PQ(pq, PathPQ, newpath, dist + 1);
+            }
+        }
+        
+        // ... right
+        if ((x < maze->width) && (mazegrid_get_edge(maze, x, y, EDGE_RIGHT) != 0)) {
+            if (!node_set_contains(set, x + 1, y)) {
+                maze_path_t newpath;
+                maze_point_t pt = { .x = x + 1, .y = y };
+                path_copy_append(&top, &newpath, pt);
+                PUSH_PQ(pq, PathPQ, newpath, dist + 1);
+            }
+        }
+
+        // ... down
+        if ((y > 0) && (mazegrid_get_edge(maze, x, y, EDGE_DOWN) != 0)) {
+            if (!node_set_contains(set, x, y - 1)) {
+                maze_path_t newpath;
+                maze_point_t pt = { .x = x, .y = y - 1};
+                path_copy_append(&top, &newpath, pt);
+                PUSH_PQ(pq, PathPQ, newpath, dist + 1);
+            }
+        }
+
+        // ... left
+        if ((x > 0) && (mazegrid_get_edge(maze, x, y, EDGE_LEFT) != 0)) {
+            if (!node_set_contains(set, x - 1, y)) {
+                maze_path_t newpath;
+                maze_point_t pt = { .x = x - 1, .y = y };
+                path_copy_append(&top, &newpath, pt);
+                PUSH_PQ(pq, PathPQ, newpath, dist + 1);
+            }
+        }
+    }
+
+    // If no path is found, we will run out of new nodes to explore, the queue
+    // will go empty, and the loop will terminate. The default return code of 0
+    // indicates that no solution has been put in the path parameter.
+
+    if (!EMPTY_PQ(pq)) {
+        maze_path_t top;
+        unsigned int dist;
+        FOREACH_PQ(pq, top, dist) {
+            free(top.nodes);
+        }
+        FOREACH_PQ_END
+    }
+    node_set_free(set);
+    return code;
+}
+