More NPC steering fixes (#17042)

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

@@ -401,20 +401,37 @@ public sealed partial class NPCSteeringSystem
                 continue;
             }
 
-            if (!_physics.TryGetNearestPoints(uid, ent, out _, out var pointB, xform, xformQuery.GetComponent(ent)))
+            var xformB = xformQuery.GetComponent(ent);
+
+            if (!_physics.TryGetNearest(uid, ent, out var pointA, out var pointB, out var distance, xform, xformB))
+            {
+                continue;
+            }
+
+            if (distance > detectionRadius)
                 continue;
 
-            var obstacleDirection = pointB - worldPos;
+            var weight = 1f;
-            var obstableDistance = obstacleDirection.Length;
+            var obstacleDirection = pointB - pointA;
 
-            if (obstableDistance > detectionRadius || obstableDistance == 0f)
+            // Inside each other so just use worldPos
-                continue;
+            if (distance == 0f)
+            {
+                obstacleDirection = _transform.GetWorldPosition(xformB, xformQuery) - worldPos;
+
+                // Welp
+                if (obstacleDirection == Vector2.Zero)
+                {
+                    obstacleDirection = Vector2.One.Normalized;
+                }
+            }
+            else
+            {
+                weight = distance / detectionRadius;
+            }
 
-            dangerPoints.Add(pointB);
             obstacleDirection = offsetRot.RotateVec(obstacleDirection);
             var norm = obstacleDirection.Normalized;
-            // Weight it to 1 if we used the fallback, otherwise relative distance.
-            var weight = obstableDistance <= agentRadius ? 1f : (obstableDistance - agentRadius) / objectRadius;
 
             for (var i = 0; i < InterestDirections; i++)
             {
@@ -445,7 +462,7 @@ public sealed partial class NPCSteeringSystem
         EntityQuery<PhysicsComponent> bodyQuery,
         EntityQuery<TransformComponent> xformQuery)
     {
-        var objectRadius = 0.1f;
+        var objectRadius = 0.25f;
         var detectionRadius = MathF.Max(0.35f, agentRadius + objectRadius);
         var ourVelocity = body.LinearVelocity;
         var factionQuery = GetEntityQuery<FactionComponent>();
@@ -470,21 +487,36 @@ public sealed partial class NPCSteeringSystem
 
             var xformB = xformQuery.GetComponent(ent);
 
-            if (!_physics.TryGetNearestPoints(uid, ent, out _, out var pointB, xform, xformB))
+            if (!_physics.TryGetNearest(uid, ent, out var pointA, out var pointB, out var distance, xform, xformB))
             {
                 continue;
             }
 
-            var obstacleDirection = pointB - worldPos;
+            if (distance > detectionRadius)
-            var obstableDistance = obstacleDirection.Length;
-
-            if (obstableDistance > detectionRadius || obstableDistance == 0f)
                 continue;
 
+            var weight = 1f;
+            var obstacleDirection = pointB - pointA;
+
+            // Inside each other so just use worldPos
+            if (distance == 0f)
+            {
+                obstacleDirection = _transform.GetWorldPosition(xformB, xformQuery) - worldPos;
+
+                // Welp
+                if (obstacleDirection == Vector2.Zero)
+                {
+                    obstacleDirection = _random.NextAngle().ToVec();
+                }
+            }
+            else
+            {
+                weight = distance / detectionRadius;
+            }
+
             obstacleDirection = offsetRot.RotateVec(obstacleDirection);
             var norm = obstacleDirection.Normalized;
-            var weight = obstableDistance <= agentRadius ? 1f : (obstableDistance - agentRadius) / objectRadius;
+            weight *= 0.25f;
-            weight *= 1f;
 
             for (var i = 0; i < InterestDirections; i++)
             {

Content.Server/NPC/Systems/NPCSteeringSystem.cs

@@ -348,6 +348,17 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
         }
         DebugTools.Assert(!float.IsNaN(interest[0]));
 
+        // Don't steer too frequently to avoid twitchiness.
+        // This should also implicitly solve tie situations.
+        // 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)
+        {
+            SetDirection(mover, steering, steering.LastSteerDirection, false);
+            return;
+        }
+
         // Avoid static objects like walls
         CollisionAvoidance(uid, offsetRot, worldPos, agentRadius, layer, mask, xform, danger, dangerPoints, bodyQuery, xformQuery);
         DebugTools.Assert(!float.IsNaN(danger[0]));
@@ -376,17 +387,6 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
             resultDirection = new Angle(desiredDirection * InterestRadians).ToVec();
         }
 
-        // Don't steer too frequently to avoid twitchiness.
-        // This should also implicitly solve tie situations.
-        // 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)
-        {
-            SetDirection(mover, steering, steering.LastSteerDirection, false);
-            return;
-        }
-
         steering.NextSteer = curTime + TimeSpan.FromSeconds(1f / NPCSteeringComponent.SteeringFrequency);
         steering.LastSteerDirection = resultDirection;
         DebugTools.Assert(!float.IsNaN(resultDirection.X));