feat: add Phase 1 core pathfinding architecture

Implements the new Baritone-inspired A* pathfinding system:
- Core types: PathNode, PathResult, MoveResult, MoveType, ActionCosts
- BinaryHeapOpenSet min-heap for A* open set
- AStarPathFinder with timeout, cancellation, partial path support
- CalculationContext for thread-safe world state snapshots
- MoveHelper for block passability checks
- IGoal interface + GoalBlock, GoalXZ, GoalNear, GoalComposite
- IMove interface + MoveTraverse, MoveDiagonal, MoveAscend, MoveDescend, MoveClimb
- /pathfind command for testing the new pathfinder

Made-with: Cursor

MinecraftClient/Pathing/Core/AStarPathFinder.cs

@@ -0,0 +1,189 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Threading;
+using MinecraftClient.Pathing.Goals;
+using MinecraftClient.Pathing.Moves;
+using MinecraftClient.Pathing.Moves.Impl;
+
+namespace MinecraftClient.Pathing.Core
+{
+    public sealed class AStarPathFinder
+    {
+        private readonly IMove[] _allMoves;
+        private readonly int _maxChunkBorderFetch;
+
+        public Action<string>? DebugLog { get; set; }
+
+        public AStarPathFinder(IMove[]? moves = null, int maxChunkBorderFetch = 64)
+        {
+            _allMoves = moves ?? BuildDefaultMoves();
+            _maxChunkBorderFetch = maxChunkBorderFetch;
+        }
+
+        public static IMove[] BuildDefaultMoves()
+        {
+            var moves = new List<IMove>();
+
+            int[] offsets = [1, -1];
+            foreach (int dx in offsets)
+            {
+                moves.Add(new MoveTraverse(dx, 0));
+                moves.Add(new MoveAscend(dx, 0));
+                moves.Add(new MoveDescend(dx, 0));
+            }
+            foreach (int dz in offsets)
+            {
+                moves.Add(new MoveTraverse(0, dz));
+                moves.Add(new MoveAscend(0, dz));
+                moves.Add(new MoveDescend(0, dz));
+            }
+
+            moves.Add(new MoveDiagonal(1, 1));
+            moves.Add(new MoveDiagonal(1, -1));
+            moves.Add(new MoveDiagonal(-1, 1));
+            moves.Add(new MoveDiagonal(-1, -1));
+
+            moves.Add(new MoveClimb(true));
+            moves.Add(new MoveClimb(false));
+
+            return [.. moves];
+        }
+
+        public PathResult Calculate(
+            CalculationContext ctx,
+            int startX, int startY, int startZ,
+            IGoal goal,
+            CancellationToken ct,
+            long timeoutMs = 5000)
+        {
+            var sw = Stopwatch.StartNew();
+            var openSet = new BinaryHeapOpenSet(4096);
+            var nodeMap = new Dictionary<long, PathNode>(4096);
+
+            var startNode = new PathNode(startX, startY, startZ)
+            {
+                GCost = 0,
+                HCost = goal.Heuristic(startX, startY, startZ),
+                IsOpen = true
+            };
+            openSet.Insert(startNode);
+            nodeMap[startNode.PackedPosition] = startNode;
+
+            int nodesExplored = 0;
+            int unloadedChunkHits = 0;
+            PathNode? bestPartialNode = startNode;
+            double bestPartialScore = startNode.HCost + startNode.GCost * 0.5;
+            MoveResult moveResult = default;
+
+            DebugLog?.Invoke($"[A*] Start ({startX},{startY},{startZ}), goal={goal}");
+
+            while (openSet.Count > 0)
+            {
+                if (ct.IsCancellationRequested)
+                {
+                    DebugLog?.Invoke($"[A*] Cancelled after {nodesExplored} nodes, {sw.ElapsedMilliseconds}ms");
+                    break;
+                }
+
+                if (sw.ElapsedMilliseconds > timeoutMs)
+                {
+                    DebugLog?.Invoke($"[A*] Timeout ({timeoutMs}ms) after {nodesExplored} nodes");
+                    break;
+                }
+
+                var current = openSet.RemoveMin();
+                current.IsClosed = true;
+                nodesExplored++;
+
+                if (goal.IsInGoal(current.X, current.Y, current.Z))
+                {
+                    DebugLog?.Invoke($"[A*] Goal reached! {nodesExplored} nodes, {sw.ElapsedMilliseconds}ms");
+                    var path = ReconstructPath(current);
+                    return new PathResult(PathStatus.Success, path, nodesExplored, sw.ElapsedMilliseconds);
+                }
+
+                foreach (var move in _allMoves)
+                {
+                    moveResult.Cost = 0;
+                    move.Calculate(ctx, current.X, current.Y, current.Z, ref moveResult);
+
+                    if (moveResult.IsImpossible)
+                        continue;
+
+                    int nx = moveResult.DestX;
+                    int ny = moveResult.DestY;
+                    int nz = moveResult.DestZ;
+
+                    if (!ctx.IsChunkLoaded(nx, nz))
+                    {
+                        unloadedChunkHits++;
+                        if (unloadedChunkHits > _maxChunkBorderFetch)
+                            continue;
+                    }
+
+                    double tentativeG = current.GCost + moveResult.Cost;
+                    long packed = PathNode.Pack(nx, ny, nz);
+
+                    if (nodeMap.TryGetValue(packed, out var neighbor))
+                    {
+                        if (neighbor.IsClosed)
+                            continue;
+                        if (tentativeG >= neighbor.GCost)
+                            continue;
+
+                        neighbor.GCost = tentativeG;
+                        neighbor.Parent = current;
+                        neighbor.MoveUsed = move.Type;
+                        if (neighbor.IsOpen)
+                            openSet.Update(neighbor);
+                    }
+                    else
+                    {
+                        neighbor = new PathNode(nx, ny, nz)
+                        {
+                            GCost = tentativeG,
+                            HCost = goal.Heuristic(nx, ny, nz),
+                            Parent = current,
+                            MoveUsed = move.Type,
+                            IsOpen = true
+                        };
+                        nodeMap[packed] = neighbor;
+                        openSet.Insert(neighbor);
+                    }
+
+                    double partialScore = neighbor.HCost + neighbor.GCost * 0.5;
+                    if (partialScore < bestPartialScore)
+                    {
+                        bestPartialScore = partialScore;
+                        bestPartialNode = neighbor;
+                    }
+                }
+            }
+
+            if (bestPartialNode is not null && bestPartialNode != startNode)
+            {
+                DebugLog?.Invoke($"[A*] Partial path to ({bestPartialNode.X},{bestPartialNode.Y},{bestPartialNode.Z}), " +
+                    $"{nodesExplored} nodes, {sw.ElapsedMilliseconds}ms");
+                var path = ReconstructPath(bestPartialNode);
+                return new PathResult(PathStatus.Partial, path, nodesExplored, sw.ElapsedMilliseconds);
+            }
+
+            DebugLog?.Invoke($"[A*] Failed, {nodesExplored} nodes, {sw.ElapsedMilliseconds}ms");
+            return PathResult.Fail(nodesExplored, sw.ElapsedMilliseconds);
+        }
+
+        private static List<PathNode> ReconstructPath(PathNode end)
+        {
+            var path = new List<PathNode>();
+            var current = end;
+            while (current is not null)
+            {
+                path.Add(current);
+                current = current.Parent;
+            }
+            path.Reverse();
+            return path;
+        }
+    }
+}

MinecraftClient/Pathing/Core/ActionCosts.cs

@@ -0,0 +1,63 @@
+namespace MinecraftClient.Pathing.Core
+{
+    /// <summary>
+    /// All pathfinding movement costs in ticks, derived from vanilla walking/sprinting speeds.
+    /// Mirrors Baritone's ActionCosts design.
+    /// </summary>
+    public static class ActionCosts
+    {
+        public const double WalkOneBlock = 20.0 / 4.317;
+        public const double SprintOneBlock = 20.0 / 5.612;
+        public const double SneakOneBlock = 20.0 / 1.3;
+        public const double LadderUpOne = 20.0 / 2.35;
+        public const double LadderDownOne = 20.0 / 3.0;
+        public const double WalkOffBlock = WalkOneBlock * 0.8;
+        public const double SprintMultiplier = SprintOneBlock / WalkOneBlock;
+        public const double DiagonalMultiplier = 1.4142135623730951;
+        public const double CostInf = 1_000_000;
+
+        public const double JumpPenalty = 2.0;
+
+        public static readonly double[] FallNBlocksCost = BuildFallTable(257);
+
+        private static double[] BuildFallTable(int maxBlocks)
+        {
+            var table = new double[maxBlocks];
+            table[0] = 0;
+
+            double velocity = 0;
+            double distance = 0;
+            int ticks = 0;
+            int blockIndex = 1;
+
+            while (blockIndex < maxBlocks)
+            {
+                velocity += 0.08;
+                velocity *= 0.98;
+                distance += velocity;
+                ticks++;
+
+                while (blockIndex < maxBlocks && distance >= blockIndex)
+                {
+                    table[blockIndex] = ticks;
+                    blockIndex++;
+                }
+
+                if (ticks > 10000)
+                    break;
+            }
+
+            for (int i = blockIndex; i < maxBlocks; i++)
+                table[i] = CostInf;
+
+            return table;
+        }
+
+        public static double FallCost(int blocks)
+        {
+            if (blocks < 0 || blocks >= FallNBlocksCost.Length)
+                return CostInf;
+            return FallNBlocksCost[blocks];
+        }
+    }
+}

MinecraftClient/Pathing/Core/BinaryHeapOpenSet.cs

@@ -0,0 +1,96 @@
+using System;
+
+namespace MinecraftClient.Pathing.Core
+{
+    /// <summary>
+    /// Min-heap of PathNodes ordered by FCost, used as the A* open set.
+    /// </summary>
+    public sealed class BinaryHeapOpenSet
+    {
+        private PathNode[] _heap;
+        private int _size;
+
+        public int Count => _size;
+
+        public BinaryHeapOpenSet(int initialCapacity = 1024)
+        {
+            _heap = new PathNode[initialCapacity];
+            _size = 0;
+        }
+
+        public void Insert(PathNode node)
+        {
+            if (_size == _heap.Length)
+                Array.Resize(ref _heap, _heap.Length * 2);
+
+            node.HeapIndex = _size;
+            _heap[_size] = node;
+            _size++;
+            SiftUp(_size - 1);
+        }
+
+        public PathNode RemoveMin()
+        {
+            var min = _heap[0];
+            _size--;
+            if (_size > 0)
+            {
+                _heap[0] = _heap[_size];
+                _heap[0].HeapIndex = 0;
+                SiftDown(0);
+            }
+            _heap[_size] = null!;
+            min.IsOpen = false;
+            return min;
+        }
+
+        public void Update(PathNode node)
+        {
+            SiftUp(node.HeapIndex);
+        }
+
+        private void SiftUp(int i)
+        {
+            var node = _heap[i];
+            while (i > 0)
+            {
+                int parent = (i - 1) >> 1;
+                if (Compare(node, _heap[parent]) >= 0)
+                    break;
+                _heap[i] = _heap[parent];
+                _heap[i].HeapIndex = i;
+                i = parent;
+            }
+            _heap[i] = node;
+            node.HeapIndex = i;
+        }
+
+        private void SiftDown(int i)
+        {
+            var node = _heap[i];
+            int half = _size >> 1;
+            while (i < half)
+            {
+                int left = (i << 1) + 1;
+                int right = left + 1;
+                int best = left;
+                if (right < _size && Compare(_heap[right], _heap[left]) < 0)
+                    best = right;
+                if (Compare(node, _heap[best]) <= 0)
+                    break;
+                _heap[i] = _heap[best];
+                _heap[i].HeapIndex = i;
+                i = best;
+            }
+            _heap[i] = node;
+            node.HeapIndex = i;
+        }
+
+        private static int Compare(PathNode a, PathNode b)
+        {
+            int cmp = a.FCost.CompareTo(b.FCost);
+            if (cmp != 0) return cmp;
+            return a.HCost.CompareTo(b.HCost);
+        }
+    }
+}

MinecraftClient/Pathing/Core/CalculationContext.cs

@@ -0,0 +1,67 @@
+using MinecraftClient.Mapping;
+using MinecraftClient.Pathing.Moves;
+
+namespace MinecraftClient.Pathing.Core
+{
+    /// <summary>
+    /// Thread-safe snapshot of world state and player capabilities for path planning.
+    /// Created once at the start of a search; all move calculations read from this.
+    /// </summary>
+    public sealed class CalculationContext
+    {
+        public World World { get; }
+        public bool CanSprint { get; }
+        public bool AllowParkour { get; }
+        public bool AllowParkourAscend { get; }
+        public bool AllowDiagonalDescend { get; }
+        public int MaxFallHeight { get; }
+        public double JumpPenalty { get; }
+        public double WalkCost { get; }
+        public double SprintCost { get; }
+        public double SneakCost { get; }
+
+        public CalculationContext(
+            World world,
+            bool canSprint = true,
+            bool allowParkour = false,
+            bool allowParkourAscend = false,
+            bool allowDiagonalDescend = true,
+            int maxFallHeight = 3,
+            double jumpPenalty = ActionCosts.JumpPenalty)
+        {
+            World = world;
+            CanSprint = canSprint;
+            AllowParkour = allowParkour;
+            AllowParkourAscend = allowParkourAscend;
+            AllowDiagonalDescend = allowDiagonalDescend;
+            MaxFallHeight = maxFallHeight;
+            JumpPenalty = jumpPenalty;
+            WalkCost = ActionCosts.WalkOneBlock;
+            SprintCost = CanSprint ? ActionCosts.SprintOneBlock : ActionCosts.WalkOneBlock;
+            SneakCost = ActionCosts.SneakOneBlock;
+        }
+
+        public Block GetBlock(int x, int y, int z)
+            => World.GetBlock(new Location(x, y, z));
+
+        public Material GetMaterial(int x, int y, int z)
+            => GetBlock(x, y, z).Type;
+
+        public bool CanWalkThrough(int x, int y, int z)
+            => MoveHelper.CanWalkThrough(this, x, y, z);
+
+        public bool CanWalkOn(int x, int y, int z)
+            => MoveHelper.CanWalkOn(this, x, y, z);
+
+        public bool IsFullyPassable(int x, int y, int z)
+            => MoveHelper.IsFullyPassable(this, x, y, z);
+
+        public bool IsChunkLoaded(int x, int z)
+        {
+            int cx = x >> 4;
+            int cz = z >> 4;
+            var col = World[cx, cz];
+            return col is not null && col.FullyLoaded;
+        }
+    }
+}

MinecraftClient/Pathing/Core/MoveResult.cs

@@ -0,0 +1,28 @@
+namespace MinecraftClient.Pathing.Core
+{
+    /// <summary>
+    /// Result of an IMove.Calculate() call. Mutable struct passed by ref for zero-alloc hot path.
+    /// </summary>
+    public struct MoveResult
+    {
+        public int DestX;
+        public int DestY;
+        public int DestZ;
+        public double Cost;
+
+        public void Set(int x, int y, int z, double cost)
+        {
+            DestX = x;
+            DestY = y;
+            DestZ = z;
+            Cost = cost;
+        }
+
+        public void SetImpossible()
+        {
+            Cost = ActionCosts.CostInf;
+        }
+
+        public readonly bool IsImpossible => Cost >= ActionCosts.CostInf;
+    }
+}

MinecraftClient/Pathing/Core/MoveType.cs

@@ -0,0 +1,13 @@
+namespace MinecraftClient.Pathing.Core
+{
+    public enum MoveType
+    {
+        Traverse,
+        Diagonal,
+        Ascend,
+        Descend,
+        Fall,
+        Climb,
+        Parkour
+    }
+}

MinecraftClient/Pathing/Core/PathNode.cs

@@ -0,0 +1,39 @@
+namespace MinecraftClient.Pathing.Core
+{
+    /// <summary>
+    /// A* search node. Stored in the open/closed sets during pathfinding.
+    /// </summary>
+    public sealed class PathNode
+    {
+        public readonly int X;
+        public readonly int Y;
+        public readonly int Z;
+
+        public double GCost;
+        public double HCost;
+        public double FCost => GCost + HCost;
+
+        public PathNode? Parent;
+        public MoveType MoveUsed;
+
+        public int HeapIndex;
+        public bool IsOpen;
+        public bool IsClosed;
+
+        public PathNode(int x, int y, int z)
+        {
+            X = x;
+            Y = y;
+            Z = z;
+        }
+
+        public long PackedPosition => Pack(X, Y, Z);
+
+        public static long Pack(int x, int y, int z)
+        {
+            return ((long)(x + 30_000_000) << 36)
+                 | ((long)(z + 30_000_000) << 12)
+                 | (long)((y + 64) & 0xFFF);
+        }
+    }
+}

MinecraftClient/Pathing/Core/PathResult.cs

@@ -0,0 +1,30 @@
+using System.Collections.Generic;
+
+namespace MinecraftClient.Pathing.Core
+{
+    public enum PathStatus
+    {
+        Success,
+        Partial,
+        Failed
+    }
+
+    public sealed class PathResult
+    {
+        public PathStatus Status { get; }
+        public IReadOnlyList<PathNode> Path { get; }
+        public int NodesExplored { get; }
+        public long ElapsedMs { get; }
+
+        public PathResult(PathStatus status, IReadOnlyList<PathNode> path, int nodesExplored, long elapsedMs)
+        {
+            Status = status;
+            Path = path;
+            NodesExplored = nodesExplored;
+            ElapsedMs = elapsedMs;
+        }
+
+        public static PathResult Fail(int nodesExplored, long elapsedMs)
+            => new(PathStatus.Failed, [], nodesExplored, elapsedMs);
+    }
+}