基于 A* 的 Minecraft 机器人寻路系统改进：路径平滑、3D 邻域建模与安全执行#

1
/// 直线可达性：同平面线段 + 玩家碰撞盒（0.6 宽）逐点采样
2
public static bool CanTravelStraight(World world, Location from, Location to, double playerRadius = 0.3)
3
{
4
    if (Math.Abs(from.Y - to.Y) > 0.01)
5
        return false; // Smoothing is limited to same-plane segments for now
6
    double dx = to.X - from.X, dz = to.Z - from.Z;
7
    double distance = Math.Sqrt(dx * dx + dz * dz);
8
    if (distance < 1e-6) return true;
9
    int steps = Math.Max(1, (int)Math.Ceiling(distance / 0.25));
10
    for (int i = 0; i <= steps; i++)
11
    {
12
        double t = (double)i / steps;
13
        double px = from.X + dx * t, pz = from.Z + dz * t;
14
        int minX = (int)Math.Floor(px - playerRadius), maxX = (int)Math.Floor(px + playerRadius);
15
        int minZ = (int)Math.Floor(pz - playerRadius), maxZ = (int)Math.Floor(pz + playerRadius);
16
        for (int blockX = minX; blockX <= maxX; blockX++)
17
            for (int blockZ = minZ; blockZ <= maxZ; blockZ++)
18
                if (!IsWalkableColumn(world, new Location(blockX, from.Y, blockZ)))
19
                    return false;
20
    }
21
    return true;
22
}
23

24
/// 贪心视线裁剪：能直走就合并路点，切角撞方块则保留拐角
25
public static Queue<Location> SimplifyPath(World world, Queue<Location> path)
26
{
27
    if (path.Count <= 2) return path;
28
    List<Location> waypoints = path.ToList();
29
    List<Location> simplified = new() { waypoints[0] };
30
    int anchorIndex = 0;
31
    for (int candidateIndex = 1; candidateIndex < waypoints.Count; candidateIndex++)
32
    {
33
        if (!CanTravelStraight(world, waypoints[anchorIndex], waypoints[candidateIndex]))
34
        {
35
            int keptIndex = candidateIndex - 1;
36
            if (keptIndex > anchorIndex) { simplified.Add(waypoints[keptIndex]); anchorIndex = keptIndex; }
37
        }
38
    }
39
    if (simplified.Last() != waypoints[^1]) simplified.Add(waypoints[^1]);
40
    return new Queue<Location>(simplified);
41
}
42

43
/// 平滑采样列必须"3 格内有地面支撑"，否则会把路径压平跨过缺口（P6）
44
private static bool IsWalkableColumn(World world, Location feet)
45
{
46
    ChunkColumn? chunkColumn = world.GetChunkColumn(feet);
47
    if (chunkColumn is null || !chunkColumn.FullyLoaded) return false;
48
    if (world.GetBlock(feet).Type.IsSolid()
49
        || world.GetBlock(Move(feet, Direction.Up)).Type.IsSolid())
50
        return false;
51
    return world.GetBlock(Move(feet, Direction.Down)).Type.IsSolid()
52
        || world.GetBlock(Move(feet, Direction.Down, 2)).Type.IsSolid()
53
        || world.GetBlock(Move(feet, Direction.Down, 3)).Type.IsSolid()
54
        || IsClimbing(world, Move(feet, Direction.Down))
55
        || IsClimbing(world, Move(feet, Direction.Down, 2))
56
        || IsClimbing(world, Move(feet, Direction.Down, 3));
57
}

1
// 地面/游泳分支：原代码只遍历 8/10 方向做水平移动
2
foreach (Direction dir in Enum.GetValues(typeof(Direction)))
3
{
4
    Location dest = Move(location, dir);
5
    bool diagonalSafe = !IsDiagonal(dir)
6
        || (IsSafe(world, Move(location, DiagonalCardinalA(dir)))
7
            && IsSafe(world, Move(location, DiagonalCardinalB(dir))));
8
    if (CanMove(world, location, dir) && (allowUnsafe || (IsSafe(world, dest) && diagonalSafe)))
9
        availableMoves.Add(dest);
10
}
11

12
// 新增：斜上跨步（1 格高台阶）
13
foreach (Direction dir in new[] { Direction.East, Direction.West, Direction.North, Direction.South })
14
{
15
    Location stepUp = Move(Move(location, dir), Direction.Up);
16
    if (CanStepUp(world, location, dir) && (allowUnsafe || IsSafe(world, stepUp)))
17
        availableMoves.Add(stepUp);
18
}
19

20
/// 台阶判定：相邻格是 1 格高实心方块，且目标格（上方）身体放得下
21
private static bool CanStepUp(World world, Location location, Direction direction)
22
{
23
    if (!IsOnGround(world, location) && !IsSwimming(world, location)) return false;
24
    Location stepBlock = Move(location, direction);
25
    Location destination = Move(stepBlock, Direction.Up);
26
    if (!world.GetBlock(stepBlock).Type.IsSolid()) return false;
27
    if (world.GetBlock(destination).Type.IsSolid()
28
        || world.GetBlock(Move(destination, Direction.Up)).Type.IsSolid())
29
        return false;
30
    return true;
31
}

1
case Direction.Up:
2
    // 原实现：ground 情况下仅检查 y+2 非实心 → 允许"原地直跳进空气"
3
    // 修改后：同列上升仅限攀爬或游泳；台阶/出坑走斜上跨步
4
    if (IsClimbing(world, location))
5
        return IsClimbing(world, Move(location, Direction.Up))
6
            || (!world.GetBlock(Move(location, Direction.Up)).Type.IsSolid()
7
                && !world.GetBlock(Move(Move(location, Direction.Up), Direction.Up)).Type.IsSolid());
8
    return IsSwimming(world, location)
9
        && !world.GetBlock(Move(location, Direction.Up)).Type.IsSolid()
10
        && !world.GetBlock(Move(Move(location, Direction.Up), Direction.Up)).Type.IsSolid();

1
return
2
    !world.GetBlock(location).Type.IsSolid()          // 新增：目标脚方块可通行
3
    && !world.GetBlock(location).Type.CanHarmPlayers()
4
    && !world.GetBlock(Move(location, Direction.Up)).Type.CanHarmPlayers()
5
    && !world.GetBlock(Move(location, Direction.Down)).Type.CanHarmPlayers()
6
    // ...坠落高度、禁水检查不变

1
private static bool IsDiagonal(Direction direction)
2
{
3
    return direction == Direction.NorthEast || direction == Direction.SouthEast
4
        || direction == Direction.SouthWest || direction == Direction.NorthWest;
5
}
6

7
private static Direction DiagonalCardinalA(Direction direction) => direction switch
8
{
9
    Direction.NorthEast => Direction.North,
10
    Direction.SouthEast => Direction.South,
11
    Direction.SouthWest => Direction.South,
12
    Direction.NorthWest => Direction.North,
13
    _ => throw new ArgumentException("Not a diagonal direction", nameof(direction))
14
};
15

16
private static Direction DiagonalCardinalB(Direction direction) => direction switch
17
{
18
    Direction.NorthEast => Direction.East,
19
    Direction.SouthEast => Direction.East,
20
    Direction.SouthWest => Direction.West,
21
    Direction.NorthWest => Direction.West,
22
    _ => throw new ArgumentException("Not a diagonal direction", nameof(direction))
23
};

1
// 修改前：
2
// return dx * dx + dz * dz < 0.25; // within ~0.5 blocks horizontally
3

4
// 修改后：
5
private bool ReachedWaypoint(Location target)
6
{
7
    double dx = target.X - location.X;
8
    double dz = target.Z - location.Z;
9
    if (dx * dx + dz * dz >= 0.49)
10
        return false; // not within ~0.7 blocks horizontally
11

12
    // 路点在方块中心（Y+0.5），脚在方块顶（Y+1），用绝对容差 ±0.6
13
    double dy = target.Y - location.Y;
14
    if (Math.Abs(dy) >= WaypointVerticalTolerance)
15
        return false;
16

17
    // 必须真正落地/攀爬/游泳：跳跃中途不推进，先站稳台阶顶端
18
    return playerPhysics.OnGround
19
        || playerPhysics.InWater
20
        || playerPhysics.OnClimbable
21
        || playerPhysics.VerticalCollisionBelow;
22
}

1
private Location? movementGoal; // 原始目标，供卡住后重规划
2
private bool movementAllowUnsafe;
3
private int movementMaxOffset;
4
private int movementMinOffset;
5
private double lastWaypointDistanceSqr = double.MaxValue;
6
private double lastReplanGoalDistanceSqr = double.MaxValue;
7
private int pathStuckTicks;
8
private int replanCount;
9
private const int PathStuckThresholdTicks = 60;   // ~3 秒 @20TPS
10
private const int MaxReplansWithoutProgress = 5;
11
private const double PathProgressEpsilonSqr = 0.0025; // ~0.05 格
12
private const double WaypointVerticalTolerance = 0.6;

1
if (pathTarget is not null)
2
{
3
    double dx = pathTarget.Value.X - location.X;
4
    double dz = pathTarget.Value.Z - location.Z;
5
    double distSqr = dx * dx + dz * dz;
6
    if (distSqr < lastWaypointDistanceSqr - PathProgressEpsilonSqr)
7
    {
8
        lastWaypointDistanceSqr = distSqr;
9
        pathStuckTicks = 0;
10
    }
11
    else if (playerPhysics.OnGround && pathStuckTicks > PathStuckThresholdTicks)
12
    {
13
        ReplanMovement();
14
        return;
15
    }
16
    else if (playerPhysics.OnGround)
17
    {
18
        // 只在落地时计时：滞空是正常的跳跃/攀爬
19
        pathStuckTicks++;
20
    }
21
    SetInputToward(pathTarget.Value);
22
}

1
private void ReplanMovement()
2
{
3
    if (movementGoal is not Location goal) { path = null; pathTarget = null; return; }
4

5
    double goalDistanceSqr = goal.DistanceSquared(location);
6
    if (goalDistanceSqr < lastReplanGoalDistanceSqr - 1.0)
7
    {
8
        // 距目标净前进 >1 格：重规划有效，重置计数
9
        lastReplanGoalDistanceSqr = goalDistanceSqr;
10
        replanCount = 0;
11
    }
12
    if (++replanCount > MaxReplansWithoutProgress)
13
    {
14
        path = null; pathTarget = null; movementGoal = null; replanCount = 0;
15
        ConsoleIO.WriteLineFormatted("§c[MCC] Movement cancelled: cannot escape the current position after repeated retries. Run /move again.");
16
        return;
17
    }
18

19
    Queue<Location>? newPath = Movement.CalculatePath(
20
        world, location, goal, movementAllowUnsafe, movementMaxOffset, movementMinOffset,
21
        TimeSpan.FromSeconds(2));
22
    pathTarget = null;
23
    path = newPath;
24
    if (newPath is null || newPath.Count == 0)
25
    {
26
        path = null; movementGoal = null;
27
        ConsoleIO.WriteLineFormatted("§c[MCC] No recovery path found; movement cancelled. Run /move again.");
28
        return;
29
    }
30
    pathTarget = newPath.Dequeue();
31
    ResetPathProgress(pathTarget.Value);
32
}

1
// 修改前：if (dy > 0.5 && playerPhysics.OnGround) physicsInput.Jump = true;
2

3
// 修改后：原版风格自动跳跃
4
if (playerPhysics.OnGround && physicsInput.Forward)
5
{
6
    double yawRad = playerYaw * (Math.PI / 180.0);
7
    double forwardX = -Math.Sin(yawRad);
8
    double forwardZ = Math.Cos(yawRad);
9
    Location aheadBlock = new(
10
        Math.Floor(location.X + forwardX * 0.7),
11
        Math.Floor(location.Y),
12
        Math.Floor(location.Z + forwardZ * 0.7));
13
    if (world.GetBlock(aheadBlock).Type.IsSolid()
14
        && !world.GetBlock(Movement.Move(aheadBlock, Direction.Up)).Type.IsSolid()
15
        && !world.GetBlock(Movement.Move(aheadBlock, Direction.Up, 2)).Type.IsSolid())
16
    {
17
        physicsInput.Jump = true;
18
    }
19
}

1
if (playerPhysics.OnGround && physicsInput.Forward)
2
{
3
    double yawRad = playerYaw * (Math.PI / 180.0);
4
    double forwardX = -Math.Sin(yawRad);
5
    double forwardZ = Math.Cos(yawRad);
6
    Location currentCell = new(Math.Floor(location.X), Math.Floor(location.Y), Math.Floor(location.Z));
7
    Location aheadCell = new(
8
        Math.Floor(location.X + forwardX * 0.7),
9
        Math.Floor(location.Y),
10
        Math.Floor(location.Z + forwardZ * 0.7));
11
    if (aheadCell != currentCell
12
        && !world.GetBlock(aheadCell).Type.IsSolid()
13
        && !world.GetBlock(Movement.Move(aheadCell, Direction.Down)).Type.IsSolid()
14
        && !world.GetBlock(Movement.Move(aheadCell, Direction.Down, 2)).Type.IsSolid())
15
    {
16
        physicsInput.Forward = false; // 前方 2 格内无地面 → 悬崖，停步
17
    }
18
}

1
// 在计算朝向之前，叠加居中修正
2
double steerX = 0, steerZ = 0;
3
if (playerPhysics.OnGround)
4
{
5
    double centerX = Math.Floor(location.X) + 0.5;
6
    double centerZ = Math.Floor(location.Z) + 0.5;
7
    double offX = centerX - location.X;
8
    double offZ = centerZ - location.Z;
9
    if (Math.Abs(offX) > 0.1
10
        && IsCliffSide(world, location, Direction.East)
11
        && IsCliffSide(world, location, Direction.West))
12
        steerX = offX * 2.5;
13
    if (Math.Abs(offZ) > 0.1
14
        && IsCliffSide(world, location, Direction.North)
15
        && IsCliffSide(world, location, Direction.South))
16
        steerZ = offZ * 2.5;
17
}
18
float targetYaw = (float)(-Math.Atan2(dx + steerX, dz + steerZ) / Math.PI * 180.0);
19

20
/// 悬崖判定：相邻格非实心且 2 格内无地面
21
private bool IsCliffSide(World world, Location location, Direction direction)
22
{
23
    Location side = Movement.Move(location, direction);
24
    if (world.GetBlock(side).Type.IsSolid()) return false;
25
    return !world.GetBlock(Movement.Move(side, Direction.Down)).Type.IsSolid()
26
        && !world.GetBlock(Movement.Move(side, Direction.Down, 2)).Type.IsSolid();
27
}

1
private static int Heuristic(Location from, Location to)
2
{
3
    double dx = Math.Abs(to.X - from.X);
4
    double dy = Math.Abs(to.Y - from.Y);
5
    double dz = Math.Abs(to.Z - from.Z);
6
    return (int)Math.Round(MoveCostScale * Math.Sqrt(dx * dx + dy * dy + dz * dz));
7
}
8

9
private static Location? JumpRecursive(World world, Location next, Direction dir, Location goal)
10
{
11
    if (!PlayerFitsHere(world, next))
12
        return null;                       // 身体放不下：终止且不产出跳点
13
    if (next == goal)
14
        return next;
15
    if (!IsWalkableColumn(world, next))
16
        return next;                       // 悬空/未加载边缘：作为跳点交给下落过渡
17
    if (HasForcedNeighbor(world, next, dir))
18
        return next;                       // 障碍导致的强制邻居
19
    if (IsDiagonal(dir)
20
        && (JumpRecursive(world, Move(next, DiagonalCardinalA(dir)), DiagonalCardinalA(dir), goal) != null
21
            || JumpRecursive(world, Move(next, DiagonalCardinalB(dir)), DiagonalCardinalB(dir), goal) != null))
22
        return next;
23
    if (HasVerticalTransition(world, next))
24
        return next;                       // 可斜上/攀爬/下落的位置必须停下评估
25
    if (!CanWalkHorizontally(world, next, dir))
26
        return null;
27
    return JumpRecursive(world, Move(next, dir), dir, goal);
28
}

1
private static bool HasSupportWithin(World world, Location feet, int depth)
2
{
3
    if (!IsLoaded(world, feet))
4
        return true; // 未知区块：不是可证明的虚空
5
    for (int d = 1; d <= depth; d++)
6
    {
7
        Location below = Move(feet, Direction.Down, d);
8
        if (!IsLoaded(world, below))
9
            return true;
10
        if (world.GetBlock(below).Type.IsSolid() || IsClimbing(world, below))
11
            return true;
12
    }
13
    return false;
14
}

1
bool pathActive = pathTarget is not null || pathSearchTask is not null;
2
if (!pathActive)
3
{
4
    if (_yaw is not null) playerPhysics.Yaw = _yaw.Value;
5
    if (_pitch is not null) playerPhysics.Pitch = _pitch.Value;
6
}
7
// ...
8
playerYaw = !pathActive && _yaw is not null ? _yaw.Value : playerYaw;

1
// 1) CollisionDetector：可攀爬方块不产生碰撞盒
2
Block block = world.GetBlock(new Location(bx, by, bz));
3
if (block.Type.CanBeClimbedOn())
4
    continue; // ladders/vines are passable; climbing pushes against the wall behind
5

6
// 2) PlayerPhysics：在梯子上且有移动输入就产生爬升速度（不依赖水平碰撞）
7
bool wantsToMove = Xxa != 0.0f || Zza != 0.0f;
8
if (OnClimbable && (HorizontalCollision || Jumping || wantsToMove))
9
    vy = PhysicsConsts.ClimbWallBump;
10

11
// 3) HandleJumping：梯子上允许跳跃（原版行为，用于跳出梯子顶）
12
else if ((OnGround || OnClimbable) && noJumpDelay == 0)
13
{
14
    JumpFromGround();
15
    noJumpDelay = 10;
16
}

1
// a) 先走到梯子格中心（偏差 >0.15 格时朝中心走），再贴墙爬
2
double centerX = ladder.X + 0.5 - location.X;
3
double centerZ = ladder.Z + 0.5 - location.Z;
4
if (centerX * centerX + centerZ * centerZ > 0.0225) { /* 朝中心走并 return */ }
5
// b) 已居中：面向最近的实心邻格（梯子背后的墙），W 推墙触发爬升
6
// c) 梯子顶：feet 不再是梯子但脚下 1-2 格是梯子 → 朝路点方向 Forward + Jump 跳出
7
bool belowLadder = world.GetBlock(Move(feetCell, Direction.Down)).Type.CanBeClimbedOn()
8
    || world.GetBlock(Move(feetCell, Direction.Down, 2)).Type.CanBeClimbedOn();
9
if (belowLadder) { physicsInput.Forward = true; if (dy > 0.5) physicsInput.Jump = true; return; }

1
bool pathActive = pathTarget is not null || pathSearchTask is not null;
2
bool farFromPhysics = physicsInitialized
3
    && new Location(playerPhysics.Position.X, playerPhysics.Position.Y, playerPhysics.Position.Z)
4
        .DistanceSquared(location) >= 4.0;
5
if (!pathActive || !physicsInitialized || farFromPhysics)
6
    UpdateLocation(location, false);

1
private bool HasUnknownGround(World world)
2
{
3
    Location feet = new(Math.Floor(Position.X), Math.Floor(Position.Y), Math.Floor(Position.Z));
4
    for (int d = 1; d <= 3; d++)
5
    {
6
        Location below = Movement.Move(feet, Direction.Down, d);
7
        ChunkColumn? column = world.GetChunkColumn(below);
8
        if (column is null || !column.FullyLoaded)
9
            return true;
10
    }
11
    return false;
12
}

1
 MinecraftClient/Mapping/Movement.cs | 754 +++++++++++++++++++++++++--
2
 MinecraftClient/McClient.cs         | 667 ++++++++++++++++++-
3
 MinecraftClient/Physics/PlayerPhysics.cs |  45 ++++++-
4
 MinecraftClient/Physics/CollisionDetector.cs |   6 +
5
 MinecraftClient.Tests/MovementTests.cs | 217 ++++++++++
6
 MinecraftClient/Commands/Move.cs       |   7 +-
7
 6 files changed, 1633 insertions(+), 63 deletions(-)

1
diff --git a/MinecraftClient/Mapping/Movement.cs b/MinecraftClient/Mapping/Movement.cs
2
@@ -1,5 +1,6 @@
3
 using System;
4
 using System.Collections.Generic;
5
+using System.Linq;
6
 using System.Threading;
7
 using System.Threading.Tasks;
8

9
@@ -61,9 +62,23 @@ namespace MinecraftClient.Mapping
10
                 foreach (Direction dir in Enum.GetValues(typeof(Direction)))
11
                 {
12
                     Location dest = Move(location, dir);
13
-                    if (CanMove(world, location, dir) && (allowUnsafe || IsSafe(world, dest)))
14
+                    bool diagonalSafe = !IsDiagonal(dir)
15
+                        || (IsSafe(world, Move(location, DiagonalCardinalA(dir)))
16
+                            && IsSafe(world, Move(location, DiagonalCardinalB(dir))));
17
+                    if (CanMove(world, location, dir) && (allowUnsafe || (IsSafe(world, dest) && diagonalSafe)))
18
                         availableMoves.Add(dest);
19
                 }
20
+
21
+                // Step-up: jump onto a 1-high solid block in an adjacent cell.
22
+                foreach (Direction dir in new[] { Direction.East, Direction.West, Direction.North, Direction.South })
23
+                {
24
+                    Location stepUp = Move(Move(location, dir), Direction.Up);
25
+                    if (CanStepUp(world, location, dir) && (allowUnsafe || IsSafe(world, stepUp)))
26
+                        availableMoves.Add(stepUp);
27
+                }
28
             }
29
             else
30
             {
31
@@ -77,6 +92,36 @@ namespace MinecraftClient.Mapping
32
             return availableMoves;
33
         }
34

35
+        private static bool IsDiagonal(Direction direction) { ... }
36
+        private static Direction DiagonalCardinalA(Direction direction) { ... }
37
+        private static Direction DiagonalCardinalB(Direction direction) { ... }
38
+
39
         /// <summary>
40
         /// Decompose a single move from a block to another into several steps
41
         /// </summary>
42
@@ -219,7 +264,7 @@ namespace MinecraftClient.Mapping
43
                 if ((current.Location == goalLower && maxOffset <= 0) ||
44
                     (maxOffset > 0 && current.HScore >= minOffset && current.HScore <= maxOffset))
45
-                    return ReconstructPath(cameFrom, current.Location, start, goal);
46
+                    return SimplifyPath(world, ReconstructPath(cameFrom, current.Location, start, goal));
47

48
                 // Discover neighbored blocks
49
                 foreach (Location neighbor in GetAvailableMoves(world, current.Location, allowUnsafe))
50
@@ -249,11 +294,124 @@ namespace MinecraftClient.Mapping
51
             if (current is not null && openSet.MinHScoreNode is not null &&
52
                 (maxOffset == int.MaxValue || openSet.MinHScoreNode.HScore <= maxOffset))
53
-                return ReconstructPath(cameFrom, openSet.MinHScoreNode.Location, start, goal);
54
+                return SimplifyPath(world, ReconstructPath(cameFrom, openSet.MinHScoreNode.Location, start, goal));
55

56
             return null;
57
         }
58

59
+        public static bool CanTravelStraight(...) { ... }   // 全文见 3.1
60
+        public static Queue<Location> SimplifyPath(...) { ... }
61
+        private static bool IsWalkableColumn(...) { ... }
62
+
63
         /// <summary>
64
         /// Helper function for CalculatePath(). Backtrack from goal to start ...
65
         /// </summary>
66
@@ -558,8 +716,11 @@ namespace MinecraftClient.Mapping
67
         private static bool IsSafe(World world, Location location)
68
         {
69
             return
70
+                !world.GetBlock(location).Type.IsSolid()
71
+
72
                 //No block that can harm the player
73
-                !world.GetBlock(location).Type.CanHarmPlayers()
74
+                && !world.GetBlock(location).Type.CanHarmPlayers()
75
                 && !world.GetBlock(Move(location, Direction.Up)).Type.CanHarmPlayers()
76
                 && !world.GetBlock(Move(location, Direction.Down)).Type.CanHarmPlayers()
77
@@ -592,15 +753,18 @@ namespace MinecraftClient.Mapping
78
                 case Direction.Down:
79
                     return IsClimbing(world, Move(location, Direction.Down)) || !IsOnGround(world, location);
80
                 case Direction.Up:
81
-                    bool nextTwoBlocks =
82
-                        !world.GetBlock(Move(Move(location, Direction.Up), Direction.Up)).Type.IsSolid();
83
-
84
-                    if (IsClimbing(world, location))
85
-                        return IsClimbing(world, Move(location, Direction.Up)) || nextTwoBlocks;
86
-
87
-                    return (IsOnGround(world, location) || IsSwimming(world, location)) && nextTwoBlocks;
88
+                    if (IsClimbing(world, location))
89
+                        return IsClimbing(world, Move(location, Direction.Up))
90
+                            || (!world.GetBlock(Move(location, Direction.Up)).Type.IsSolid()
91
+                                && !world.GetBlock(Move(Move(location, Direction.Up), Direction.Up)).Type.IsSolid());
92
+                    return IsSwimming(world, location)
93
+                        && !world.GetBlock(Move(location, Direction.Up)).Type.IsSolid()
94
+                        && !world.GetBlock(Move(Move(location, Direction.Up), Direction.Up)).Type.IsSolid();
95
@@ -628,10 +792,35 @@ namespace MinecraftClient.Mapping
96
         }
97

98
+        private static bool CanStepUp(World world, Location location, Direction direction) { ... }
99
+
100
         /// <summary>
101
         /// Evaluates if a player fits in this location
102
         /// </summary>
103
@@ -645,9 +834,12 @@ namespace MinecraftClient.Mapping
104
             // Handle slabs
105
-            if (!isNotSolid && world.GetBlock(Move(location, Direction.Up))
106
-                    .IsTopSlab(McClient.Instance!.GetProtocolVersion()))
107
+            int? protocolVersion = McClient.Instance?.GetProtocolVersion();
108
+            if (!isNotSolid && protocolVersion is not null &&
109
+                world.GetBlock(Move(location, Direction.Up)).IsTopSlab(protocolVersion.Value))
110
+            {
111
                 isNotSolid = true;
112
+            }
113

114
             return canClimb || isNotSolid;
115
         }
116
@@ -713,4 +905,4 @@ namespace MinecraftClient.Mapping
117
             return true;
118
         }
119
     }
120
-}
121
\ No newline at end of file
122
+}
123
diff --git a/MinecraftClient/McClient.cs b/MinecraftClient/McClient.cs
124
@@ -83,6 +83,18 @@ namespace MinecraftClient
125
         private readonly MovementInput physicsInput = new();
126
         private bool physicsInitialized = false;
127
         private Location? pathTarget;
128
+        private Location? movementGoal;
129
+        private bool movementAllowUnsafe;
130
+        private int movementMaxOffset;
131
+        private int movementMinOffset;
132
+        private double lastWaypointDistanceSqr = double.MaxValue;
133
+        private double lastReplanGoalDistanceSqr = double.MaxValue;
134
+        private int pathStuckTicks;
135
+        private int replanCount;
136
+        private const int PathStuckThresholdTicks = 60;
137
+        private const int MaxReplansWithoutProgress = 5;
138
+        private const double PathProgressEpsilonSqr = 0.0025;
139
+        private const double WaypointVerticalTolerance = 0.6;
140
         public enum MovementType { Sneak, Walk, Sprint }
141
@@ -1783,8 +1795,21 @@ namespace MinecraftClient
142
                 else
143
                 {
144
+                    movementGoal = goal;
145
+                    movementAllowUnsafe = allowUnsafe;
146
+                    movementMaxOffset = maxOffset;
147
+                    movementMinOffset = minOffset;
148
+                    replanCount = 0;
149
+                    lastReplanGoalDistanceSqr = goal.DistanceSquared(location);
150
                     pathTarget = null;
151
                     path = Movement.CalculatePath(...);
152
+                    if (path is not null)
153
+                        ConsoleIO.WriteLineFormatted($"§e[MCC] Path ({path.Count}): ...");
154
+                    lastWaypointDistanceSqr = double.MaxValue;
155
+                    pathStuckTicks = 0;
156
                     return path is not null;
157
                 }
158
@@ -3558,6 +3583,7 @@ namespace MinecraftClient
159
                 if (path is not null && path.Count > 0)
160
                 {
161
                     pathTarget = path.Dequeue();
162
+                    ResetPathProgress(pathTarget.Value);
163
                     if (Config.Main.Advanced.MoveHeadWhileWalking)
164
@@ -3565,6 +3591,26 @@ namespace MinecraftClient
165
                 {
166
                     pathTarget = null;
167
                     path = null;
168
+                    movementGoal = null;
169
+                }
170
+            }
171
+            else if (pathTarget is not null && path is not null && path.Count > 0)
172
+            {
173
+                // Corner recovery: skip current waypoint when straight to next is clear
174
+                Location next = path.Peek();
175
+                if (curDx * curDx + curDz * curDz < 0.5625 &&
176
+                    Math.Abs(pathTarget.Value.Y - location.Y) < WaypointVerticalTolerance &&
177
+                    playerPhysics.OnGround &&
178
+                    Movement.CanTravelStraight(world, location, next))
179
+                {
180
+                    pathTarget = path.Dequeue();
181
+                    ResetPathProgress(pathTarget.Value);
182
                 }
183
             }
184
@@ -3572,16 +3618,107 @@ namespace MinecraftClient
185
             if (pathTarget is not null)
186
             {
187
+                // Stuck detection + replan（全文见 3.5）
188
                 SetInputToward(pathTarget.Value);
189
             }
190
         }
191

192
+        private void ResetPathProgress(Location target) { ... }
193
+        private void ReplanMovement() { ... }
194
+
195
         /// <summary>
196
         /// Check if the player has approximately reached a waypoint.
197
         /// </summary>
198
@@ -3589,7 +3726,23 @@ namespace MinecraftClient
199
         {
200
             double dx = target.X - location.X;
201
             double dz = target.Z - location.Z;
202
-            return dx * dx + dz * dz < 0.25;
203
+            if (dx * dx + dz * dz >= 0.49) return false;
204
+            double dy = target.Y - location.Y;
205
+            if (Math.Abs(dy) >= WaypointVerticalTolerance) return false;
206
+            return playerPhysics.OnGround
207
+                || playerPhysics.InWater
208
+                || playerPhysics.OnClimbable
209
+                || playerPhysics.VerticalCollisionBelow;
210
         }
211
@@ -3605,17 +3758,84 @@ namespace MinecraftClient
212
             if (distSqr < 0.01) return;
213

214
-            float targetYaw = (float)(-Math.Atan2(dx, dz) / Math.PI * 180.0);
215
+            // 居中校正（全文见 3.8）
216
+            float targetYaw = (float)(-Math.Atan2(dx + steerX, dz + steerZ) / Math.PI * 180.0);
217
             if (targetYaw < 0) targetYaw += 360;
218
             playerPhysics.Yaw = targetYaw;
219
             playerYaw = targetYaw;
220

221
             physicsInput.Forward = true;
222

223
-            if (dy > 0.5 && playerPhysics.OnGround)
224
-                physicsInput.Jump = true;
225
+            // 自动跳跃（全文见 3.6）
226
+            // 边缘探测（全文见 3.7）
227

228
             // Map MovementSpeed setting: 1=sneak, 2-4=walk, 5=sprint
229
@@ -3624,6 +3844,20 @@ namespace MinecraftClient
230
                 physicsInput.Sneak = true;
231
         }
232

233
+        private bool IsCliffSide(World world, Location location, Direction direction) { ... }
234
+
235
         /// <summary>
236
         /// Check if the client is currently processing a Movement.
237
         /// </summary>
238
@@ -3650,6 +3884,7 @@ namespace MinecraftClient
239
         {
240
             bool success = ClientIsMoving();
241
             path = null;
242
+            movementGoal = null;
243
             return success;
244
         }