1 files changed, 16 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++) {