pathing: snap yaw for sprint jump approach

MinecraftClient.Tests/Pathing/Execution/SprintJumpTemplateScenarioTests.cs

@@ -35,6 +35,111 @@ public sealed class SprintJumpTemplateScenarioTests
         Assert.True(TemplateFootingHelper.IsFootprintInsideTargetBlock(finalPos, segment.End));
     }
 
+    [Fact]
+    public void SprintJumpTemplate_TwoBlockGap_FinalStop_CompletesFromOppositeYaw()
+    {
+        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 0, max: 16);
+        FlatWorldTestBuilder.ClearBox(world, 0, 79, 0, 4, 82, 1);
+        FlatWorldTestBuilder.SetSolid(world, 0, 79, 0);
+        FlatWorldTestBuilder.SetSolid(world, 2, 79, 0);
+
+        var segment = new PathSegment
+        {
+            Start = new Location(0.5, 80, 0.5),
+            End = new Location(2.5, 80, 0.5),
+            MoveType = MoveType.Parkour,
+            ExitTransition = PathTransitionType.FinalStop
+        };
+
+        var template = new SprintJumpTemplate(segment, null);
+        var physics = TemplateSimulationRunner.CreateGroundedPhysics(segment.Start, yaw: 90f);
+
+        TemplateState state = TemplateSimulationRunner.Run(template, physics, world, maxTicks: 140, out Location finalPos);
+
+        Assert.True(state == TemplateState.Complete, $"state={state} finalPos={finalPos} vel={physics.DeltaMovement}");
+        Assert.True(TemplateFootingHelper.IsFootprintInsideTargetBlock(finalPos, segment.End));
+    }
+
+    [Fact]
+    public void SprintJumpTemplate_Approach_SnapsYawImmediatelyFromOppositeYaw()
+    {
+        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 0, max: 16);
+        FlatWorldTestBuilder.ClearBox(world, 0, 79, 0, 4, 82, 1);
+        FlatWorldTestBuilder.SetSolid(world, 0, 79, 0);
+        FlatWorldTestBuilder.SetSolid(world, 2, 79, 0);
+
+        var segment = new PathSegment
+        {
+            Start = new Location(0.5, 80, 0.5),
+            End = new Location(2.5, 80, 0.5),
+            MoveType = MoveType.Parkour,
+            ExitTransition = PathTransitionType.FinalStop
+        };
+
+        var template = new SprintJumpTemplate(segment, null);
+        var physics = TemplateSimulationRunner.CreateGroundedPhysics(segment.Start, yaw: 90f);
+        var input = new MovementInput();
+
+        TemplateState state = template.Tick(segment.Start, physics, input, world);
+
+        Assert.Equal(TemplateState.InProgress, state);
+        Assert.InRange(physics.Yaw, 269.9f, 270.1f);
+        Assert.True(input.Forward);
+        Assert.True(input.Sprint);
+        Assert.True(input.Jump);
+    }
+
+    [Fact]
+    public void SprintJumpTemplate_TwoBlockGap_FinalStop_CompletesFromOppositeYawWithinTwentyTicks()
+    {
+        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 0, max: 16);
+        FlatWorldTestBuilder.ClearBox(world, 0, 79, 0, 4, 82, 1);
+        FlatWorldTestBuilder.SetSolid(world, 0, 79, 0);
+        FlatWorldTestBuilder.SetSolid(world, 2, 79, 0);
+
+        var segment = new PathSegment
+        {
+            Start = new Location(0.5, 80, 0.5),
+            End = new Location(2.5, 80, 0.5),
+            MoveType = MoveType.Parkour,
+            ExitTransition = PathTransitionType.FinalStop
+        };
+
+        var template = new SprintJumpTemplate(segment, null);
+        var physics = TemplateSimulationRunner.CreateGroundedPhysics(segment.Start, yaw: 90f);
+        var input = new MovementInput();
+        TemplateState state = TemplateState.InProgress;
+        int elapsedTicks = 0;
+        Location finalPos = segment.Start;
+        var trace = new List<string>();
+        for (; elapsedTicks < 80; elapsedTicks++)
+        {
+            input.Reset();
+            Location pos = new(physics.Position.X, physics.Position.Y, physics.Position.Z);
+            state = template.Tick(pos, physics, input, world);
+            if (elapsedTicks < 20 || state != TemplateState.InProgress)
+            {
+                trace.Add(
+                    $"tick={elapsedTicks} state={state} pos={pos} yaw={physics.Yaw:F1} vel={physics.DeltaMovement} " +
+                    $"onGround={physics.OnGround} input(F={input.Forward},J={input.Jump},S={input.Sprint})");
+            }
+
+            if (state != TemplateState.InProgress)
+            {
+                finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
+                break;
+            }
+
+            physics.ApplyInput(input);
+            physics.Tick(world);
+            finalPos = new Location(physics.Position.X, physics.Position.Y, physics.Position.Z);
+        }
+
+        Assert.Equal(TemplateState.Complete, state);
+        Assert.True(elapsedTicks <= 20, $"elapsedTicks={elapsedTicks} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
+        Assert.True(TemplateFootingHelper.IsFootprintInsideTargetBlock(finalPos, segment.End), $"elapsedTicks={elapsedTicks} finalPos={finalPos} vel={physics.DeltaMovement}\n{string.Join('\n', trace)}");
+    }
+
     [Fact]
     public void SprintJumpTemplate_ThreeBlockGap_FinalStop_Completes()
     {
@@ -136,6 +241,129 @@ public sealed class SprintJumpTemplateScenarioTests
         Assert.InRange(horizontalSpeed, 0.0, 0.04);
     }
 
+    [Fact]
+    public void SprintJumpTemplate_PrepareJumpIntoSecondParkour_CompletesWithoutSettling()
+    {
+        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 0, max: 16);
+        FlatWorldTestBuilder.ClearBox(world, 0, 79, 0, 6, 82, 1);
+        FlatWorldTestBuilder.SetSolid(world, 0, 79, 0);
+        FlatWorldTestBuilder.SetSolid(world, 2, 79, 0);
+        FlatWorldTestBuilder.SetSolid(world, 4, 79, 0);
+
+        var segment = new PathSegment
+        {
+            Start = new Location(0.5, 80, 0.5),
+            End = new Location(2.5, 80, 0.5),
+            MoveType = MoveType.Parkour,
+            ExitTransition = PathTransitionType.PrepareJump,
+            ExitHints = new PathTransitionHints(1, 0, 0.10, double.PositiveInfinity, false, true, true, false, 10),
+            PreserveSprint = true
+        };
+        var next = new PathSegment
+        {
+            Start = new Location(2.5, 80, 0.5),
+            End = new Location(4.5, 80, 0.5),
+            MoveType = MoveType.Parkour,
+            ExitTransition = PathTransitionType.FinalStop
+        };
+
+        var template = new SprintJumpTemplate(segment, next);
+        var physics = TemplateSimulationRunner.CreateGroundedPhysics(segment.Start, yaw: 270f);
+
+        TemplateState state = TemplateSimulationRunner.Run(template, physics, world, maxTicks: 140, out Location finalPos);
+        double horizontalSpeed = Math.Sqrt(physics.DeltaMovement.X * physics.DeltaMovement.X + physics.DeltaMovement.Z * physics.DeltaMovement.Z);
+
+        Assert.Equal(TemplateState.Complete, state);
+        Assert.True(TemplateFootingHelper.IsCenterInsideTargetBlock(finalPos, segment.End), $"finalPos={finalPos} vel={physics.DeltaMovement}");
+        Assert.True(horizontalSpeed > 0.02, $"finalPos={finalPos} vel={physics.DeltaMovement}");
+    }
+
+    [Fact]
+    public void SprintJumpTemplate_FinalStop_CompletesAfterPrepareJumpCarry()
+    {
+        World world = FlatWorldTestBuilder.CreateStoneFloor(min: 0, max: 16);
+        FlatWorldTestBuilder.ClearBox(world, 0, 79, 0, 6, 82, 1);
+        FlatWorldTestBuilder.SetSolid(world, 0, 79, 0);
+        FlatWorldTestBuilder.SetSolid(world, 2, 79, 0);
+        FlatWorldTestBuilder.SetSolid(world, 4, 79, 0);
+
+        var first = new PathSegment
+        {
+            Start = new Location(0.5, 80, 0.5),
+            End = new Location(2.5, 80, 0.5),
+            MoveType = MoveType.Parkour,
+            ExitTransition = PathTransitionType.PrepareJump,
+            ExitHints = new PathTransitionHints(1, 0, 0.10, double.PositiveInfinity, false, true, true, false, 10),
+            PreserveSprint = true
+        };
+        var second = new PathSegment
+        {
+            Start = new Location(2.5, 80, 0.5),
+            End = new Location(4.5, 80, 0.5),
+            MoveType = MoveType.Parkour,
+            ExitTransition = PathTransitionType.FinalStop
+        };
+
+        var physics = TemplateSimulationRunner.CreateGroundedPhysics(first.Start, yaw: 270f);
+
+        TemplateState firstState = TemplateSimulationRunner.Run(new SprintJumpTemplate(first, second), physics, world, maxTicks: 140, out Location handoffPos);
+        TemplateState secondState = TemplateSimulationRunner.Run(new SprintJumpTemplate(second, null), physics, world, maxTicks: 140, out Location finalPos);
+
+        Assert.Equal(TemplateState.Complete, firstState);
+        Assert.True(
+            secondState == TemplateState.Complete,
+            $"secondState={secondState} handoffPos={handoffPos} finalPos={finalPos} vel={physics.DeltaMovement}");
+        Assert.True(TemplateFootingHelper.IsFootprintInsideTargetBlock(finalPos, second.End), $"handoffPos={handoffPos} finalPos={finalPos} vel={physics.DeltaMovement}");
+    }
+
+    [Fact]
+    public void SprintJumpTemplate_DiagonalLandingRecovery_HandsOffToTurnTraverse()
+    {
+        World world = FlatWorldTestBuilder.CreateStoneFloor(min: -1, max: 8);
+        FlatWorldTestBuilder.ClearBox(world, -1, 79, -1, 8, 82, 4);
+        FlatWorldTestBuilder.SetSolid(world, 0, 79, 0);
+        FlatWorldTestBuilder.SetSolid(world, 2, 79, 1);
+        FlatWorldTestBuilder.SetSolid(world, 3, 79, 1);
+        FlatWorldTestBuilder.SetSolid(world, 4, 79, 2);
+
+        var parkour = new PathSegment
+        {
+            Start = new Location(0.5, 80, 0.5),
+            End = new Location(2.5, 80, 1.5),
+            MoveType = MoveType.Parkour,
+            ExitTransition = PathTransitionType.LandingRecovery,
+            ExitHints = new PathTransitionHints(1, 0, 0.0, 0.05, true, true, false, true, 12)
+        };
+        var traverse = new PathSegment
+        {
+            Start = new Location(2.5, 80, 1.5),
+            End = new Location(3.5, 80, 1.5),
+            MoveType = MoveType.Traverse,
+            ExitTransition = PathTransitionType.Turn,
+            ExitHints = new PathTransitionHints(1, 1, 0.0, 0.05, true, true, false, true, 12)
+        };
+        var next = new PathSegment
+        {
+            Start = new Location(3.5, 80, 1.5),
+            End = new Location(4.5, 80, 2.5),
+            MoveType = MoveType.Diagonal,
+            ExitTransition = PathTransitionType.FinalStop
+        };
+
+        var physics = TemplateSimulationRunner.CreateGroundedPhysics(parkour.Start, yaw: 315f);
+
+        TemplateState parkourState = TemplateSimulationRunner.Run(new SprintJumpTemplate(parkour, traverse), physics, world, maxTicks: 160, out Location handoffPos);
+        TemplateState traverseState = TemplateSimulationRunner.Run(new WalkTemplate(traverse, next), physics, world, maxTicks: 160, out Location finalPos);
+
+        Assert.Equal(TemplateState.Complete, parkourState);
+        Assert.True(
+            traverseState == TemplateState.Complete,
+            $"traverseState={traverseState} handoffPos={handoffPos} finalPos={finalPos} vel={physics.DeltaMovement}");
+        Assert.True(
+            TemplateFootingHelper.IsCenterInsideTargetBlock(finalPos, traverse.End),
+            $"handoffPos={handoffPos} finalPos={finalPos} vel={physics.DeltaMovement}");
+    }
+
     [Fact]
     public void SprintJumpTemplate_ThreeBlockGap_WithIsolatedTakeoffBlock_JumpsImmediately()
     {

MinecraftClient/Pathing/Execution/Templates/SprintJumpTemplate.cs

@@ -30,6 +30,8 @@ namespace MinecraftClient.Pathing.Execution.Templates
         private int _tickCount;
         private Phase _phase = Phase.Approach;
         private bool _leftGround;
+        private bool _carriedGroundEntry;
+        private bool _airBrakeLatched;
 
         private const float YawToleranceDeg = 5f;
 
@@ -55,19 +57,25 @@ namespace MinecraftClient.Pathing.Execution.Templates
 
             float targetYaw = TemplateHelper.CalculateYaw(dx, dz);
             float targetPitch = TemplateHelper.CalculatePitch(dx, dy, dz);
-            physics.Yaw = TemplateHelper.SmoothYaw(physics.Yaw, targetYaw);
+            YawAlignmentMode yawMode = _phase == Phase.Approach
+                ? YawAlignmentMode.Snap
+                : YawAlignmentMode.Smooth;
+            physics.Yaw = TemplateHelper.AlignYaw(physics.Yaw, targetYaw, yawMode);
             physics.Pitch = TemplateHelper.SmoothPitch(physics.Pitch, targetPitch);
 
             switch (_phase)
             {
                 case Phase.Approach:
-                    input.Forward = true;
-                    input.Sprint = true;
-
                     if (physics.OnGround)
                     {
+                        if (_tickCount == 1 && TemplateHelper.GetHorizontalSpeed(physics) > 0.02)
+                            _carriedGroundEntry = true;
+
                         double fromStartSq = TemplateHelper.HorizontalDistanceSq(pos, ExpectedStart);
                         float yawDelta = YawDifference(physics.Yaw, targetYaw);
+                        bool turnInPlace = yawDelta > 35f;
+                        input.Forward = !turnInPlace;
+                        input.Sprint = !turnInPlace;
 
                         // Build momentum before jumping. Sprint speed is ~5.6 m/s
                         // (0.28 blocks/tick). More run-up = more airtime distance.
@@ -84,6 +92,14 @@ namespace MinecraftClient.Pathing.Execution.Templates
                         else
                             minApproachSq = 0.0;
 
+                        if (_carriedGroundEntry
+                            && _segment.ExitTransition == PathTransitionType.FinalStop
+                            && _horizDist <= 2.5
+                            && GetLateralOffsetFromSegmentLine(pos) > 0.20)
+                        {
+                            input.Sprint = false;
+                        }
+
                         bool yawAligned = yawDelta < YawToleranceDeg;
                         bool posReady = fromStartSq >= minApproachSq;
 
@@ -93,6 +109,13 @@ namespace MinecraftClient.Pathing.Execution.Templates
                             _phase = Phase.Airborne;
                         }
                     }
+                    else
+                    {
+                        float yawDelta = YawDifference(physics.Yaw, targetYaw);
+                        bool turnInPlace = yawDelta > 35f;
+                        input.Forward = !turnInPlace;
+                        input.Sprint = !turnInPlace;
+                    }
                     if (_tickCount > 40)
                         return TemplateState.Failed;
                     break;
@@ -116,7 +139,14 @@ namespace MinecraftClient.Pathing.Execution.Templates
                         && lookaheadAirBrake
                         && !releaseInAir;
 
-                    if (releaseInAir || pastTarget)
+                    if (_segment.ExitTransition == PathTransitionType.FinalStop
+                        && _horizDist <= 2.5
+                        && (releaseInAir || pastTarget))
+                    {
+                        _airBrakeLatched = true;
+                    }
+
+                    if (_airBrakeLatched || releaseInAir || pastTarget)
                     {
                         input.Forward = false;
                         input.Sprint = false;
@@ -148,6 +178,13 @@ namespace MinecraftClient.Pathing.Execution.Templates
                     else if (TemplateHelper.ShouldBiasTowardExitHeading(pos, _segment))
                         TemplateHelper.FaceExitHeading(physics, _segment);
 
+                    if (_segment.ExitTransition == PathTransitionType.PrepareJump
+                        && physics.OnGround
+                        && GroundedSegmentController.ShouldComplete(_segment, pos, physics))
+                    {
+                        return TemplateState.Complete;
+                    }
+
                     double horizToleranceLinear = _horizDist >= 3.5 ? 1.5 : 1.0;
                     double horizToleranceSq = horizToleranceLinear * horizToleranceLinear;
                     double vertTolerance = Math.Abs(ExpectedEnd.Y - ExpectedStart.Y) > 0.5 ? 1.5 : 1.0;
@@ -189,6 +226,22 @@ namespace MinecraftClient.Pathing.Execution.Templates
             return dot > 0.0;
         }
 
+        private double GetLateralOffsetFromSegmentLine(Location pos)
+        {
+            double dirX = ExpectedEnd.X - ExpectedStart.X;
+            double dirZ = ExpectedEnd.Z - ExpectedStart.Z;
+            double len = Math.Sqrt(dirX * dirX + dirZ * dirZ);
+            if (len < 0.001)
+                return 0.0;
+
+            dirX /= len;
+            dirZ /= len;
+
+            double relX = pos.X - ExpectedStart.X;
+            double relZ = pos.Z - ExpectedStart.Z;
+            return Math.Abs((-dirZ * relX) + (dirX * relZ));
+        }
+
         private bool ShouldReleaseInAir(Location pos, PlayerPhysics physics, World world)
         {
             if (_segment.ExitTransition == PathTransitionType.ContinueStraight || physics.OnGround)

MinecraftClient/Pathing/Execution/Templates/TemplateHelper.cs

@@ -4,6 +4,12 @@ using MinecraftClient.Physics;
 
 namespace MinecraftClient.Pathing.Execution.Templates
 {
+    internal enum YawAlignmentMode
+    {
+        Smooth,
+        Snap
+    }
+
     internal static class TemplateHelper
     {
         private const double EyeHeight = 1.62;
@@ -50,6 +56,14 @@ namespace MinecraftClient.Pathing.Execution.Templates
             return result;
         }
 
+        internal static float AlignYaw(float current, float target, YawAlignmentMode mode, float maxStep = MaxYawStepPerTick)
+        {
+            target = NormalizeYaw(target);
+            return mode == YawAlignmentMode.Snap
+                ? target
+                : SmoothYaw(current, target, maxStep);
+        }
+
         /// <summary>
         /// Smoothly interpolate pitch toward a target.
         /// </summary>
@@ -77,16 +91,18 @@ namespace MinecraftClient.Pathing.Execution.Templates
             return dx * dx + dz * dz < horizThresholdSq && Math.Abs(dy) < vertThreshold;
         }
 
-        internal static void FaceSegmentHeading(PlayerPhysics physics, PathSegment segment)
+        internal static void FaceSegmentHeading(PlayerPhysics physics, PathSegment segment,
+            YawAlignmentMode mode = YawAlignmentMode.Smooth)
         {
             float headingYaw = CalculateYaw(segment.HeadingX, segment.HeadingZ);
-            physics.Yaw = SmoothYaw(physics.Yaw, headingYaw);
+            physics.Yaw = AlignYaw(physics.Yaw, headingYaw, mode);
         }
 
-        internal static void FaceExitHeading(PlayerPhysics physics, PathSegment segment)
+        internal static void FaceExitHeading(PlayerPhysics physics, PathSegment segment,
+            YawAlignmentMode mode = YawAlignmentMode.Smooth)
         {
             float headingYaw = GetExitHeadingYaw(segment);
-            physics.Yaw = SmoothYaw(physics.Yaw, headingYaw);
+            physics.Yaw = AlignYaw(physics.Yaw, headingYaw, mode);
         }
 
         internal static void ApplyDecision(MovementInput input, TransitionBrakingDecision decision)
@@ -187,6 +203,11 @@ namespace MinecraftClient.Pathing.Execution.Templates
             return HeadingPenaltyDegrees(yaw, headingX, headingZ);
         }
 
+        internal static bool ShouldTurnInPlaceBeforeAdvancing(float yaw, PathSegment segment, double maxAdvanceHeadingPenalty = 35.0)
+        {
+            return HeadingPenaltyDegrees(yaw, segment) > maxAdvanceHeadingPenalty;
+        }
+
         internal static double HeadingPenaltyDegrees(float yaw, int headingX, int headingZ)
         {
             if (headingX == 0 && headingZ == 0)
@@ -217,6 +238,13 @@ namespace MinecraftClient.Pathing.Execution.Templates
             }
         }
 
+        private static float NormalizeYaw(float yaw)
+        {
+            while (yaw < 0f) yaw += 360f;
+            while (yaw >= 360f) yaw -= 360f;
+            return yaw;
+        }
+
         internal static PlayerPhysics ClonePhysicsForPlanning(PlayerPhysics physics)
         {
             return new PlayerPhysics