Improve diagonal_distance(), dramatically enhancing performance

1 files changed, 12 insertions, 14 deletions

diff --git a/path.c b/path.c
index 7fcc73f..415d0af 100644
--- a/path.c
+++ b/path.c
@@ -295,21 +295,19 @@ size_t manhattan_distance(Position a, Position b) {
     return d;
 }
 
-/* FIXME: something faster, maybe? or at least better written. */
 size_t diagonal_distance(Position a, Position b) {
-    size_t one = 0;
-    size_t two = 0;
-    if (a.x > b.x) {
-        one = (a.x - b.x) * COST_ORTHOGONAL;
-    } else {
-        one = (b.x - a.x) * COST_ORTHOGONAL;
-    }
-    if (a.y > b.y) {
-        two = (a.y - b.y) * COST_ORTHOGONAL;
-    } else {
-        two = (b.y - a.y) * COST_ORTHOGONAL;
-    }
-    return (size_t) hypot(one, two);
+    size_t dx = 0, dy = 0, min = 0;
+
+    if (a.x > b.x) dx = a.x - b.x;
+    else dx = b.x - a.x;
+
+    if (a.y > b.y) dy = a.y - b.y;
+    else dy = b.y - a.y;
+
+    if (dx < dy) min = dx;
+    else min = dy;
+
+    return COST_ORTHOGONAL * (dx + dy) + (COST_DIAGONAL - 2 * COST_ORTHOGONAL) * min;
 }
 
 size_t path_length(Path path, Position start, Position goal) {