Add a path_length() function

2 files changed, 18 insertions, 0 deletions

diff --git a/path.c b/path.c
index ea2b4e0..3ea350c 100644
--- a/path.c
+++ b/path.c
@@ -258,6 +258,22 @@ size_t diagonal_distance(Position a, Position b) {
     return (size_t) hypot(one, two);
 }
 
+size_t path_length(Path path, Position start, Position goal) {
+    size_t length = 0;
+    if (path != NULL) {
+        Position cur = goal;
+        while (cur.x != start.x || cur.y != start.y) {
+            if (cur.x - path[cur.y][cur.x].parent.x == 0 || cur.y - path[cur.y][cur.x].parent.y == 0) {
+                length += COST_ORTHOGONAL;
+            } else {
+                length += COST_DIAGONAL;
+            }
+            cur = path[cur.y][cur.x].parent;
+        }
+    }
+    return length;
+}
+
 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 37c410a..12cfa80 100644
--- a/path.h
+++ b/path.h
@@ -14,6 +14,8 @@ size_t diagonal_distance(Position a, Position b);
 
 void path_free(Path path, size_t height);
 
+size_t path_length(Path path, Position start, Position goal);
+
 /* Helper funcs for the visited array */
 char **visited_new(size_t width, size_t height);
 void visited_clear(char **visited, size_t width, size_t height);