Significantly improve NPC steering (space-wizards#17931)

Content.Server/NPC/Components/NPCSteeringComponent.cs

@@ -45,6 +45,9 @@ public sealed class NPCSteeringComponent : Component
     [DataField("nextSteer", customTypeSerializer:typeof(TimeOffsetSerializer))]
     public TimeSpan NextSteer = TimeSpan.Zero;
 
+    [DataField("lastSteerIndex")]
+    public int LastSteerIndex = -1;
+
     [DataField("lastSteerDirection")]
     public Vector2 LastSteerDirection = Vector2.Zero;
 

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

@@ -146,7 +146,7 @@ public override void Startup(NPCBlackboard blackboard)
                 _steering.PrunePath(uid, mapCoords, targetCoordinates.ToMapPos(_entManager, _transform) - mapCoords.Position, result.Path);
             }
 
-            comp.CurrentPath = result.Path;
+            comp.CurrentPath = new Queue<PathPoly>(result.Path);
         }
     }
 

Content.Server/NPC/Pathfinding/PathRequest.cs

@@ -17,7 +17,7 @@ public abstract class PathRequest
     public Task<PathResult> Task => Tcs.Task;
     public readonly TaskCompletionSource<PathResult> Tcs;
 
-    public Queue<PathPoly> Polys = new();
+    public List<PathPoly> Polys = new();
 
     public bool Started = false;
 
@@ -103,9 +103,9 @@ public BFSPathRequest(
 public sealed class PathResultEvent
 {
     public PathResult Result;
-    public readonly Queue<PathPoly> Path;
+    public readonly List<PathPoly> Path;
 
-    public PathResultEvent(PathResult result, Queue<PathPoly> path)
+    public PathResultEvent(PathResult result, List<PathPoly> path)
     {
         Result = result;
         Path = path;

Content.Server/NPC/Pathfinding/PathfindingSystem.Common.cs

@@ -24,7 +24,7 @@ public int Compare((float, PathPoly) x, (float, PathPoly) y)
 
     private static readonly PathComparer PathPolyComparer = new();
 
-    private Queue<PathPoly> ReconstructPath(Dictionary<PathPoly, PathPoly> path, PathPoly currentNodeRef)
+    private List<PathPoly> ReconstructPath(Dictionary<PathPoly, PathPoly> path, PathPoly currentNodeRef)
     {
         var running = new List<PathPoly> { currentNodeRef };
         while (path.ContainsKey(currentNodeRef))
@@ -35,10 +35,8 @@ private Queue<PathPoly> ReconstructPath(Dictionary<PathPoly, PathPoly> path, Pat
             running.Add(currentNodeRef);
         }
 
-        running = Simplify(running);
         running.Reverse();
-        var result = new Queue<PathPoly>(running);
-        return result;
+        return running;
     }
 
     private float GetTileCost(PathRequest request, PathPoly start, PathPoly end)

Content.Server/NPC/Pathfinding/PathfindingSystem.cs

@@ -238,7 +238,7 @@ public async Task<PathResultEvent> GetRandomPath(
             PathFlags flags = PathFlags.None)
         {
             if (!TryComp<TransformComponent>(entity, out var start))
-                return new PathResultEvent(PathResult.NoPath, new Queue<PathPoly>());
+                return new PathResultEvent(PathResult.NoPath, new List<PathPoly>());
 
             var layer = 0;
             var mask = 0;
@@ -252,7 +252,7 @@ public async Task<PathResultEvent> GetRandomPath(
             var path = await GetPath(request);
 
             if (path.Result != PathResult.Path)
-                return new PathResultEvent(PathResult.NoPath, new Queue<PathPoly>());
+                return new PathResultEvent(PathResult.NoPath, new List<PathPoly>());
 
             return new PathResultEvent(PathResult.Path, path.Path);
         }
@@ -280,14 +280,13 @@ public async Task<PathResultEvent> GetRandomPath(
                 return 0f;
 
             var distance = 0f;
-            var node = path.Path.Dequeue();
-            var lastNode = node;
+            var lastNode = path.Path[0];
 
-            do
+            for (var i = 1; i < path.Path.Count; i++)
             {
+                var node = path.Path[i];
                 distance += GetTileCost(request, lastNode, node);
-                lastNode = node;
-            } while (path.Path.TryDequeue(out node));
+            }
 
             return distance;
         }
@@ -301,7 +300,7 @@ public async Task<PathResultEvent> GetPath(
         {
             if (!TryComp<TransformComponent>(entity, out var xform) ||
                 !TryComp<TransformComponent>(target, out var targetXform))
-                return new PathResultEvent(PathResult.NoPath, new Queue<PathPoly>());
+                return new PathResultEvent(PathResult.NoPath, new List<PathPoly>());
 
             var request = GetRequest(entity, xform.Coordinates, targetXform.Coordinates, range, cancelToken, flags);
             return await GetPath(request);
@@ -471,7 +470,7 @@ private async Task<PathResultEvent> GetPath(
 
             if (!request.Task.IsCompletedSuccessfully)
             {
-                return new PathResultEvent(PathResult.NoPath, new Queue<PathPoly>());
+                return new PathResultEvent(PathResult.NoPath, new List<PathPoly>());
             }
 
             // Same context as do_after and not synchronously blocking soooo

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

@@ -292,13 +292,38 @@ private void CheckPath(EntityUid uid, NPCSteeringComponent steering, TransformCo
     /// <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,28 +335,29 @@ public void PrunePath(EntityUid uid, MapCoordinates mapCoordinates, Vector2 dire
         // 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;
 
             var nodeMap = node.Coordinates.ToMap(EntityManager, _transform);
 
             // 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 @@ private void CollisionAvoidance(
         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 @@ private void CollisionAvoidance(
             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 @@ private void Steer(
 
         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 @@ private void Steer(
 
         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 @@ private async void RequestPath(EntityUid uid, NPCSteeringComponent steering, Tra
             _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 @@ private async void RequestPath(EntityUid uid, NPCSteeringComponent steering, Tra
         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