Avoid NPCs getting stuck around tables (#14807)

Content.Server/NPC/Components/NPCSteeringComponent.cs

@@ -57,7 +57,7 @@ public sealed class NPCSteeringComponent : Component
     [DataField("lastStuckTime", customTypeSerializer:typeof(TimeOffsetSerializer))]
     public TimeSpan LastStuckTime;
 
-    public const float StuckDistance = 0.5f;
+    public const float StuckDistance = 1f;
 
     /// <summary>
     /// Have we currently requested a path.

Content.Server/NPC/HTN/PrimitiveTasks/Operators/MoveToOperator.cs

@@ -17,6 +17,7 @@ public sealed class MoveToOperator : HTNOperator
     [Dependency] private readonly IMapManager _mapManager = default!;
     private NPCSteeringSystem _steering = default!;
     private PathfindingSystem _pathfind = default!;
+    private SharedTransformSystem _transform = default!;
 
     /// <summary>
     /// Should we assume the MovementTarget is reachable during planning or should we pathfind to it?
@@ -55,6 +56,7 @@ public sealed class MoveToOperator : HTNOperator
         base.Initialize(sysManager);
         _pathfind = sysManager.GetEntitySystem<PathfindingSystem>();
         _steering = sysManager.GetEntitySystem<NPCSteeringSystem>();
+        _transform = sysManager.GetEntitySystem<SharedTransformSystem>();
     }
 
     public override async Task<(bool Valid, Dictionary<string, object>? Effects)> Plan(NPCBlackboard blackboard,
@@ -126,9 +128,10 @@ public sealed class MoveToOperator : HTNOperator
         // Need to remove the planning value for execution.
         blackboard.Remove<EntityCoordinates>(NPCBlackboard.OwnerCoordinates);
         var targetCoordinates = blackboard.GetValue<EntityCoordinates>(TargetKey);
+        var uid = blackboard.GetValue<EntityUid>(NPCBlackboard.Owner);
 
         // Re-use the path we may have if applicable.
-        var comp = _steering.Register(blackboard.GetValue<EntityUid>(NPCBlackboard.Owner), targetCoordinates);
+        var comp = _steering.Register(uid, targetCoordinates);
 
         if (blackboard.TryGetValue<float>(RangeKey, out var range, _entManager))
         {
@@ -139,8 +142,8 @@ public sealed class MoveToOperator : HTNOperator
         {
             if (blackboard.TryGetValue<EntityCoordinates>(NPCBlackboard.OwnerCoordinates, out var coordinates, _entManager))
             {
-                var mapCoords = coordinates.ToMap(_entManager);
-                _steering.PrunePath(mapCoords, targetCoordinates.ToMapPos(_entManager) - mapCoords.Position, result.Path);
+                var mapCoords = coordinates.ToMap(_entManager, _transform);
+                _steering.PrunePath(uid, mapCoords, targetCoordinates.ToMapPos(_entManager, _transform) - mapCoords.Position, result.Path);
             }
 
             comp.CurrentPath = result.Path;

Content.Server/NPC/Systems/NPCSteeringSystem.Context.cs

@@ -1,9 +1,11 @@
 using System.Linq;
+using Content.Server.Examine;
 using Content.Server.NPC.Components;
 using Content.Server.NPC.Pathfinding;
 using Content.Shared.Interaction;
 using Content.Shared.Movement.Components;
 using Content.Shared.NPC;
+using Content.Shared.Physics;
 using Robust.Shared.Map;
 using Robust.Shared.Physics.Components;
 
@@ -177,7 +179,8 @@ public sealed partial class NPCSteeringSystem
         }
         // Stuck detection
         // Check if we have moved further than the movespeed * stuck time.
-        else if (ourCoordinates.TryDistance(EntityManager, steering.LastStuckCoordinates, out var stuckDistance) &&
+        else if (AntiStuck &&
+                 ourCoordinates.TryDistance(EntityManager, steering.LastStuckCoordinates, out var stuckDistance) &&
                  stuckDistance < NPCSteeringComponent.StuckDistance)
         {
             var stuckTime = _timing.CurTime - steering.LastStuckTime;
@@ -281,13 +284,26 @@ public sealed partial class NPCSteeringSystem
     /// <summary>
     /// We may be pathfinding and moving at the same time in which case early nodes may be out of date.
     /// </summary>
-    public void PrunePath(MapCoordinates mapCoordinates, Vector2 direction, Queue<PathPoly> nodes)
+    public void PrunePath(EntityUid uid, MapCoordinates mapCoordinates, Vector2 direction, Queue<PathPoly> nodes)
     {
-        if (nodes.Count == 0)
+        if (nodes.Count <= 1)
             return;
 
-        // Prune the first node as it's irrelevant.
+        // Prune the first node as it's irrelevant (normally it is our node so we don't want to backtrack).
         nodes.Dequeue();
+        // TODO: Really need layer support
+        CollisionGroup mask = 0;
+
+        if (TryComp<PhysicsComponent>(uid, out var physics))
+        {
+            mask = (CollisionGroup) physics.CollisionMask;
+        }
+
+        // If we have to backtrack (for example, we're behind a table and the target is on the other side)
+        // Then don't consider pruning.
+        var goal = nodes.Last().Coordinates.ToMap(EntityManager, _transform);
+        var canPrune =
+            _interaction.InRangeUnobstructed(mapCoordinates, goal, (goal.Position - mapCoordinates.Position).Length + 0.1f, mask);
 
         while (nodes.TryPeek(out var node))
         {
@@ -298,7 +314,8 @@ public sealed partial class NPCSteeringSystem
 
             // If any nodes are 'behind us' relative to the target we'll prune them.
             // This isn't perfect but should fix most cases of stutter stepping.
-            if (nodeMap.MapId == mapCoordinates.MapId &&
+            if (canPrune &&
+                nodeMap.MapId == mapCoordinates.MapId &&
                 Vector2.Dot(direction, nodeMap.Position - mapCoordinates.Position) < 0f)
             {
                 nodes.Dequeue();

Content.Server/NPC/Systems/NPCSteeringSystem.cs

@@ -12,6 +12,7 @@ using Content.Shared.Movement.Components;
 using Content.Shared.Movement.Systems;
 using Content.Shared.NPC;
 using Content.Shared.NPC.Events;
+using Content.Shared.Physics;
 using Content.Shared.Weapons.Melee;
 using Robust.Server.Player;
 using Robust.Shared.Configuration;
@@ -55,6 +56,11 @@ namespace Content.Server.NPC.Systems
         [Dependency] private readonly SharedPhysicsSystem _physics = default!;
         [Dependency] private readonly SharedTransformSystem _transform = default!;
 
+        /// <summary>
+        /// Enabled antistuck detection so if an NPC is in the same spot for a while it will re-path.
+        /// </summary>
+        public bool AntiStuck = true;
+
         private bool _enabled;
 
         private bool _pathfinding = true;
@@ -402,11 +408,21 @@ namespace Content.Server.NPC.Systems
             var targetPoly = _pathfindingSystem.GetPoly(steering.Coordinates);
 
             // If this still causes issues future sloth adjust the collision mask.
+            // Thanks past sloth I already realised.
             if (targetPoly != null &&
                 steering.Coordinates.Position.Equals(Vector2.Zero) &&
-                _interaction.InRangeUnobstructed(uid, steering.Coordinates.EntityId, range: 30f))
+                TryComp<PhysicsComponent>(uid, out var physics) &&
+                _interaction.InRangeUnobstructed(uid, steering.Coordinates.EntityId, range: 30f, (CollisionGroup) physics.CollisionMask))
             {
                 steering.CurrentPath.Clear();
+                // Enqueue our poly as it will be pruned later.
+                var ourPoly = _pathfindingSystem.GetPoly(xform.Coordinates);
+
+                if (ourPoly != null)
+                {
+                    steering.CurrentPath.Enqueue(ourPoly);
+                }
+
                 steering.CurrentPath.Enqueue(targetPoly);
                 return;
             }
@@ -441,7 +457,7 @@ namespace Content.Server.NPC.Systems
             var targetPos = steering.Coordinates.ToMap(EntityManager, _transform);
             var ourPos = xform.MapPosition;
 
-            PrunePath(ourPos, targetPos.Position - ourPos.Position, result.Path);
+            PrunePath(uid, ourPos, targetPos.Position - ourPos.Position, result.Path);
             steering.CurrentPath = result.Path;
         }