Significantly improve NPC steering (#17931)

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

@@ -292,13 +292,38 @@ 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(EntityUid uid, MapCoordinates mapCoordinates, Vector2 direction, Queue<PathPoly> nodes)
+    public void PrunePath(EntityUid uid, MapCoordinates mapCoordinates, Vector2 direction, List<PathPoly> nodes)
     {
         if (nodes.Count <= 1)
             return;
 
-        // Prune the first node as it's irrelevant (normally it is our node so we don't want to backtrack).
-        nodes.Dequeue();
+        // Work out if we're inside any nodes, then use the next one as the starting point.
+        var index = 0;
+        var found = false;
+
+        for (var i = 0; i < nodes.Count; i++)
+        {
+            var node = nodes[i];
+            var matrix = _transform.GetWorldMatrix(node.GraphUid);
+
+            // Always want to prune the poly itself so we point to the next poly and don't backtrack.
+            if (matrix.TransformBox(node.Box).Contains(mapCoordinates.Position))
+            {
+                index = i + 1;
+                found = true;
+                break;
+            }
+        }
+
+        if (found)
+        {
+            nodes.RemoveRange(0, index);
+            _pathfindingSystem.Simplify(nodes);
+            return;
+        }
+
+        // Otherwise, take the node after the nearest node.
+
         // TODO: Really need layer support
         CollisionGroup mask = 0;
 
@@ -310,11 +335,11 @@ public sealed partial class NPCSteeringSystem
         // 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))
+        for (var i = 0; i < nodes.Count; i++)
         {
+            var node = nodes[i];
+
             if (!node.Data.IsFreeSpace)
                 break;
 
@@ -322,16 +347,17 @@ 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 (canPrune &&
-                nodeMap.MapId == mapCoordinates.MapId &&
+            if (nodeMap.MapId == mapCoordinates.MapId &&
                 Vector2.Dot(direction, nodeMap.Position - mapCoordinates.Position) < 0f)
             {
-                nodes.Dequeue();
+                nodes.RemoveAt(i);
                 continue;
             }
 
             break;
         }
+
+        _pathfindingSystem.Simplify(nodes);
     }
 
     /// <summary>
@@ -382,7 +408,7 @@ public sealed partial class NPCSteeringSystem
         TransformComponent xform,
         float[] danger)
     {
-        var objectRadius = 0.10f;
+        var objectRadius = 0.15f;
         var detectionRadius = MathF.Max(0.35f, agentRadius + objectRadius);
 
         foreach (var ent in _lookup.GetEntitiesInRange(uid, detectionRadius, LookupFlags.Static))
@@ -430,7 +456,7 @@ public sealed partial class NPCSteeringSystem
             for (var i = 0; i < InterestDirections; i++)
             {
                 var dot = Vector2.Dot(norm, Directions[i]);
-                danger[i] = MathF.Max(dot * weight * 0.9f, danger[i]);
+                danger[i] = MathF.Max(dot * weight, danger[i]);
             }
         }
 

Content.Server/NPC/Systems/NPCSteeringSystem.cs

@@ -368,6 +368,12 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
 
         Separation(uid, offsetRot, worldPos, agentRadius, layer, mask, body, xform, danger);
 
+        // Prioritise whichever direction we went last tick if it's a tie-breaker.
+        if (steering.LastSteerIndex != -1)
+        {
+            interest[steering.LastSteerIndex] *= 1.1f;
+        }
+
         // Remove the danger map from the interest map.
         var desiredDirection = -1;
         var desiredValue = 0f;
@@ -392,6 +398,7 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
 
         steering.NextSteer = curTime + TimeSpan.FromSeconds(1f / NPCSteeringComponent.SteeringFrequency);
         steering.LastSteerDirection = resultDirection;
+        steering.LastSteerIndex = desiredDirection;
         DebugTools.Assert(!float.IsNaN(resultDirection.X));
         SetDirection(mover, steering, resultDirection, false);
     }
@@ -425,14 +432,6 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
             _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;
         }
@@ -468,7 +467,7 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
         var ourPos = xform.MapPosition;
 
         PrunePath(uid, ourPos, targetPos.Position - ourPos.Position, result.Path);
-        steering.CurrentPath = result.Path;
+        steering.CurrentPath = new Queue<PathPoly>(result.Path);
     }
 
     // TODO: Move these to movercontroller