lots done

src/Roboto.Navigation/PathFinder.cs

@@ -0,0 +1,153 @@
+using System.Numerics;
+using Roboto.Core;
+
+namespace Roboto.Navigation;
+
+public static class PathFinder
+{
+    private static readonly int[] Dx = [-1, 0, 1, 0, -1, -1, 1, 1];
+    private static readonly int[] Dy = [0, -1, 0, 1, -1, 1, -1, 1];
+    private static readonly float[] Cost = [1f, 1f, 1f, 1f, 1.414f, 1.414f, 1.414f, 1.414f];
+
+    /// <summary>
+    /// A* pathfinding on WalkabilitySnapshot. Returns world-coord waypoints or null if no path.
+    /// </summary>
+    public static List<Vector2>? FindPath(WalkabilitySnapshot terrain, Vector2 start, Vector2 goal, float worldToGrid)
+    {
+        var w = terrain.Width;
+        var h = terrain.Height;
+        var gridToWorld = 1f / worldToGrid;
+
+        var startGx = Math.Clamp((int)(start.X * worldToGrid), 0, w - 1);
+        var startGy = Math.Clamp((int)(start.Y * worldToGrid), 0, h - 1);
+        var goalGx = Math.Clamp((int)(goal.X * worldToGrid), 0, w - 1);
+        var goalGy = Math.Clamp((int)(goal.Y * worldToGrid), 0, h - 1);
+
+        // Snap to nearest walkable if start/goal are in walls
+        (startGx, startGy) = SnapToWalkable(terrain, startGx, startGy, w, h);
+        (goalGx, goalGy) = SnapToWalkable(terrain, goalGx, goalGy, w, h);
+
+        var startNode = (startGx, startGy);
+        var goalNode = (goalGx, goalGy);
+
+        if (startNode == goalNode)
+            return [new Vector2(goalGx * gridToWorld, goalGy * gridToWorld)];
+
+        var openSet = new PriorityQueue<(int x, int y), float>();
+        var cameFrom = new Dictionary<(int, int), (int, int)>();
+        var gScore = new Dictionary<(int, int), float>();
+
+        gScore[startNode] = 0;
+        openSet.Enqueue(startNode, Heuristic(startNode, goalNode));
+
+        var iterations = 0;
+        const int maxIterations = 50_000;
+
+        while (openSet.Count > 0 && iterations++ < maxIterations)
+        {
+            var current = openSet.Dequeue();
+
+            if (current == goalNode)
+                return ReconstructAndSimplify(cameFrom, current, gridToWorld);
+
+            var currentG = gScore.GetValueOrDefault(current, float.MaxValue);
+
+            for (var i = 0; i < 8; i++)
+            {
+                var nx = current.x + Dx[i];
+                var ny = current.y + Dy[i];
+
+                if (nx < 0 || nx >= w || ny < 0 || ny >= h) continue;
+                if (!terrain.IsWalkable(nx, ny)) continue;
+
+                // Diagonal corner-cut check
+                if (i >= 4)
+                {
+                    if (!terrain.IsWalkable(current.x + Dx[i], current.y) ||
+                        !terrain.IsWalkable(current.x, current.y + Dy[i]))
+                        continue;
+                }
+
+                var neighbor = (nx, ny);
+                var tentativeG = currentG + Cost[i];
+
+                if (tentativeG < gScore.GetValueOrDefault(neighbor, float.MaxValue))
+                {
+                    cameFrom[neighbor] = current;
+                    gScore[neighbor] = tentativeG;
+                    openSet.Enqueue(neighbor, tentativeG + Heuristic(neighbor, goalNode));
+                }
+            }
+        }
+
+        return null;
+    }
+
+    private static float Heuristic((int x, int y) a, (int x, int y) b)
+    {
+        var dx = Math.Abs(a.x - b.x);
+        var dy = Math.Abs(a.y - b.y);
+        return Math.Max(dx, dy) + 0.414f * Math.Min(dx, dy);
+    }
+
+    private static (int, int) SnapToWalkable(WalkabilitySnapshot terrain, int gx, int gy, int w, int h)
+    {
+        if (terrain.IsWalkable(gx, gy)) return (gx, gy);
+
+        // BFS outward to find nearest walkable cell
+        for (var r = 1; r < 20; r++)
+        {
+            for (var dx = -r; dx <= r; dx++)
+            {
+                for (var dy = -r; dy <= r; dy++)
+                {
+                    if (Math.Abs(dx) != r && Math.Abs(dy) != r) continue;
+                    var nx = gx + dx;
+                    var ny = gy + dy;
+                    if (nx >= 0 && nx < w && ny >= 0 && ny < h && terrain.IsWalkable(nx, ny))
+                        return (nx, ny);
+                }
+            }
+        }
+        return (gx, gy);
+    }
+
+    private static List<Vector2> ReconstructAndSimplify(
+        Dictionary<(int, int), (int, int)> cameFrom, (int x, int y) current, float gridToWorld)
+    {
+        var path = new List<(int x, int y)>();
+        var node = current;
+        while (cameFrom.ContainsKey(node))
+        {
+            path.Add(node);
+            node = cameFrom[node];
+        }
+        path.Reverse();
+
+        if (path.Count <= 2)
+            return path.Select(n => new Vector2(n.x * gridToWorld, n.y * gridToWorld)).ToList();
+
+        // Simplify: skip collinear waypoints
+        var simplified = new List<Vector2> { ToWorld(path[0], gridToWorld) };
+        for (var i = 2; i < path.Count; i++)
+        {
+            var prev = path[i - 2];
+            var mid = path[i - 1];
+            var curr = path[i];
+
+            var d1x = mid.x - prev.x;
+            var d1y = mid.y - prev.y;
+            var d2x = curr.x - mid.x;
+            var d2y = curr.y - mid.y;
+
+            // Direction changed
+            if (d1x != d2x || d1y != d2y)
+                simplified.Add(ToWorld(mid, gridToWorld));
+        }
+        simplified.Add(ToWorld(path[^1], gridToWorld));
+        return simplified;
+    }
+
+    private static Vector2 ToWorld((int x, int y) node, float gridToWorld)
+        => new(node.x * gridToWorld, node.y * gridToWorld);
+}