NPC steering blending (#25666)

* NPC steering blending

Significantly more stable than using LastSteerDirection and also AntiStuck never got tripped locally when I was running around. I also left future notes for me to cleanup the pathfinder in future.

* Remove index

Content.Server/NPC/Components/NPCSteeringComponent.cs

@@ -28,11 +28,11 @@ public sealed partial class NPCSteeringComponent : Component
     [ViewVariables(VVAccess.ReadWrite)]
     public float Radius = 0.35f;
 
-    [ViewVariables]
+    [ViewVariables, DataField]
-    public readonly float[] Interest = new float[SharedNPCSteeringSystem.InterestDirections];
+    public float[] Interest = new float[SharedNPCSteeringSystem.InterestDirections];
 
-    [ViewVariables]
+    [ViewVariables, DataField]
-    public readonly float[] Danger = new float[SharedNPCSteeringSystem.InterestDirections];
+    public float[] Danger = new float[SharedNPCSteeringSystem.InterestDirections];
 
     // TODO: Update radius, also danger points debug only
     public readonly List<Vector2> DangerPoints = new();
@@ -45,21 +45,9 @@ public sealed partial class NPCSteeringComponent : Component
     [DataField("forceMove")]
     public bool ForceMove = false;
 
-    /// <summary>
-    /// Next time we can change our steering direction.
-    /// </summary>
-    [DataField("nextSteer", customTypeSerializer:typeof(TimeOffsetSerializer))]
-    [AutoPausedField]
-    public TimeSpan NextSteer = TimeSpan.Zero;
-
-    [DataField("lastSteerIndex")]
-    public int LastSteerIndex = -1;
-
     [DataField("lastSteerDirection")]
     public Vector2 LastSteerDirection = Vector2.Zero;
 
-    public const int SteeringFrequency = 5;
-
     /// <summary>
     /// Last position we considered for being stuck.
     /// </summary>

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

@@ -9,6 +9,7 @@ using Content.Shared.Movement.Components;
 using Content.Shared.NPC;
 using Content.Shared.Physics;
 using Robust.Shared.Map;
+using Robust.Shared.Physics;
 using Robust.Shared.Physics.Components;
 using ClimbingComponent = Content.Shared.Climbing.Components.ClimbingComponent;
 
@@ -16,7 +17,7 @@ namespace Content.Server.NPC.Systems;
 
 public sealed partial class NPCSteeringSystem
 {
-    private void ApplySeek(float[] interest, Vector2 direction, float weight)
+    private void ApplySeek(Span<float> interest, Vector2 direction, float weight)
     {
         if (weight == 0f || direction == Vector2.Zero)
             return;
@@ -25,13 +26,10 @@ public sealed partial class NPCSteeringSystem
 
         for (var i = 0; i < InterestDirections; i++)
         {
-            if (interest[i].Equals(-1f))
-                continue;
-
             var angle = i * InterestRadians;
             var dot = MathF.Cos(directionAngle - angle);
-            dot = (dot + 1) * 0.5f;
+            dot = (dot + 1f) * 0.5f;
-            interest[i] += dot * weight;
+            interest[i] = Math.Clamp(interest[i] + dot * weight, 0f, 1f);
         }
     }
 
@@ -72,7 +70,7 @@ public sealed partial class NPCSteeringSystem
         TransformComponent xform,
         Angle offsetRot,
         float moveSpeed,
-        float[] interest,
+        Span<float> interest,
         float frameTime,
         ref bool forceSteer)
     {
@@ -274,7 +272,8 @@ public sealed partial class NPCSteeringSystem
         }
 
         // If not in LOS and no path then get a new one fam.
-        if (!inLos && steering.CurrentPath.Count == 0)
+        if ((!inLos && steering.ArriveOnLineOfSight && steering.CurrentPath.Count == 0) ||
+            (!steering.ArriveOnLineOfSight && steering.CurrentPath.Count == 0))
         {
             needsPath = true;
         }
@@ -465,12 +464,12 @@ public sealed partial class NPCSteeringSystem
         int layer,
         int mask,
         TransformComponent xform,
-        float[] danger)
+        Span<float> danger)
     {
-        var objectRadius = 0.15f;
+        var objectRadius = 0.25f;
         var detectionRadius = MathF.Max(0.35f, agentRadius + objectRadius);
         var ents = _entSetPool.Get();
-        _lookup.GetEntitiesInRange(uid, detectionRadius, ents, LookupFlags.Static);
+        _lookup.GetEntitiesInRange(uid, detectionRadius, ents, LookupFlags.Dynamic | LookupFlags.Static);
 
         foreach (var ent in ents)
         {
@@ -478,6 +477,7 @@ public sealed partial class NPCSteeringSystem
             if (!_physicsQuery.TryGetComponent(ent, out var otherBody) ||
                 !otherBody.Hard ||
                 !otherBody.CanCollide ||
+                otherBody.BodyType == BodyType.KinematicController ||
                 (mask & otherBody.CollisionLayer) == 0x0 &&
                 (layer & otherBody.CollisionMask) == 0x0)
             {
@@ -506,7 +506,7 @@ public sealed partial class NPCSteeringSystem
             }
             else
             {
-                weight = distance / detectionRadius;
+                weight = (detectionRadius - distance) / detectionRadius;
             }
 
             if (obstacleDirection == Vector2.Zero)
@@ -541,7 +541,7 @@ public sealed partial class NPCSteeringSystem
         int mask,
         PhysicsComponent body,
         TransformComponent xform,
-        float[] danger)
+        Span<float> danger)
     {
         var objectRadius = 0.25f;
         var detectionRadius = MathF.Max(0.35f, agentRadius + objectRadius);
@@ -614,4 +614,35 @@ public sealed partial class NPCSteeringSystem
     // TODO: Alignment
 
     // TODO: Cohesion
+    private void Blend(NPCSteeringComponent steering, float frameTime, Span<float> interest, Span<float> danger)
+    {
+        /*
+         * Future sloth notes:
+         * Pathfinder cleanup:
+            - Cleanup whatever the fuck is happening in pathfinder
+            - Use Flee for melee behavior / actions and get the seek direction from that rather than bulldozing
+            - Must always have a path
+            - Path should return the full version + the snipped version
+            - Pathfinder needs to do diagonals
+            - Next node is either <current node + 1> or <nearest node + 1> (on the full path)
+            - If greater than <1.5m distance> repath
+         */
+
+        // IDK why I didn't do this sooner but blending is a lot better than lastdir for fixing stuttering.
+        const float BlendWeight = 10f;
+        var blendValue = Math.Min(1f, frameTime * BlendWeight);
+
+        for (var i = 0; i < InterestDirections; i++)
+        {
+            var currentInterest = interest[i];
+            var lastInterest = steering.Interest[i];
+            var interestDiff = (currentInterest - lastInterest) * blendValue;
+            steering.Interest[i] = lastInterest + interestDiff;
+
+            var currentDanger = danger[i];
+            var lastDanger = steering.Danger[i];
+            var dangerDiff = (currentDanger - lastDanger) * blendValue;
+            steering.Danger[i] = lastDanger + dangerDiff;
+        }
+    }
 }

Content.Server/NPC/Systems/NPCSteeringSystem.cs

@@ -3,7 +3,6 @@ using System.Threading;
 using System.Threading.Tasks;
 using Content.Server.Administration.Managers;
 using Content.Server.DoAfter;
-using Content.Server.Doors.Systems;
 using Content.Server.NPC.Components;
 using Content.Server.NPC.Events;
 using Content.Server.NPC.Pathfinding;
@@ -28,7 +27,6 @@ using Robust.Shared.Timing;
 using Robust.Shared.Utility;
 using Content.Shared.Prying.Systems;
 using Microsoft.Extensions.ObjectPool;
-using Robust.Shared.Threading;
 
 namespace Content.Server.NPC.Systems;
 
@@ -315,8 +313,6 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
             return;
         }
 
-        var interest = steering.Interest;
-        var danger = steering.Danger;
         var agentRadius = steering.Radius;
         var worldPos = _transform.GetWorldPosition(xform);
         var (layer, mask) = _physics.GetHardCollision(uid);
@@ -328,13 +324,10 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
         var body = _physicsQuery.GetComponent(uid);
         var dangerPoints = steering.DangerPoints;
         dangerPoints.Clear();
+        Span<float> interest = stackalloc float[InterestDirections];
+        Span<float> danger = stackalloc float[InterestDirections];
 
-        for (var i = 0; i < InterestDirections; i++)
+        // TODO: This should be fly
-        {
-            steering.Interest[i] = 0f;
-            steering.Danger[i] = 0f;
-        }
-
         steering.CanSeek = true;
 
         var ev = new NPCSteeringEvent(steering, xform, worldPos, offsetRot);
@@ -347,6 +340,7 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
             SetDirection(mover, steering, Vector2.Zero);
             return;
         }
+
         DebugTools.Assert(!float.IsNaN(interest[0]));
 
         // Don't steer too frequently to avoid twitchiness.
@@ -354,7 +348,7 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
         // I think doing this after all the ops above is best?
         // Originally I had it way above but sometimes mobs would overshoot their tile targets.
 
-        if (!forceSteer && steering.NextSteer > curTime)
+        if (!forceSteer)
         {
             SetDirection(mover, steering, steering.LastSteerDirection, false);
             return;
@@ -366,11 +360,8 @@ 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.
+        // Blend last and current tick
-        if (steering.LastSteerIndex != -1)
+        Blend(steering, frameTime, interest, danger);
-        {
-            interest[steering.LastSteerIndex] *= 1.1f;
-        }
 
         // Remove the danger map from the interest map.
         var desiredDirection = -1;
@@ -378,7 +369,7 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
 
         for (var i = 0; i < InterestDirections; i++)
         {
-            var adjustedValue = Math.Clamp(interest[i] - danger[i], 0f, 1f);
+            var adjustedValue = Math.Clamp(steering.Interest[i] - steering.Danger[i], 0f, 1f);
 
             if (adjustedValue > desiredValue)
             {
@@ -394,9 +385,7 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
             resultDirection = new Angle(desiredDirection * InterestRadians).ToVec();
         }
 
-        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);
     }