Slightly rework pathfinding obstacle avoidance and handling (#38056)

* fix: fix NPC route cost overlay mode

* feat: adjust pathfinding weights

Plus drive-by import trim.

* feat: rework pathfinding obstacle avoidance

* review by Pok

* fix merge

* Update NPCSteeringSystem.Obstacles.cs

---------

Co-authored-by: Pok245 <113675512+Pok27@users.noreply.github.com>

Content.Client/NPC/PathfindingSystem.cs

@@ -551,6 +551,9 @@ namespace Content.Client.NPC
 
                 foreach (var route in _system.Routes)
                 {
+                    if (route.Message.Costs.Values.Count == 0)
+                        continue;
+
                     var highestGScore = route.Message.Costs.Values.Max();
 
                     foreach (var (node, cost) in route.Message.Costs)

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

@@ -1,8 +1,4 @@
-using Content.Shared.Gravity;
-using Content.Shared.Maps;
 using Content.Shared.NPC;
-using Robust.Shared.Map.Components;
-using Robust.Shared.Spawners;
 
 namespace Content.Server.NPC.Pathfinding;
 
@@ -61,18 +57,20 @@ public sealed partial class PathfindingSystem
 
             // TODO: Handling power + door prying
             // Door we should be able to open
-            if (isDoor && !isAccess && (request.Flags & PathFlags.Interact) != 0x0)
+            if (isDoor)
             {
-                modifier += 0.5f;
-            }
-            // Door we can force open one way or another
-            else if (isDoor && isAccess && (request.Flags & PathFlags.Prying) != 0x0)
-            {
-                modifier += 10f;
+                if (!isAccess && (request.Flags & PathFlags.Interact) != 0x0)
+                    modifier += 0.5f;
+                else if (isAccess && (request.Flags & PathFlags.Prying) != 0x0)
+                    modifier += 10f;
+                else
+                    // Last ditch—try to bump the door if it's the only feasible option.
+                    modifier += 20f;
             }
             else if ((request.Flags & PathFlags.Smashing) != 0x0 && end.Data.Damage > 0f)
             {
-                modifier += 10f + end.Data.Damage / 100f;
+                // Breaking stuff should be usually last resort, especially because we WILL try to punch walls.
+                modifier += 10f + end.Data.Damage / 10f;
             }
             else if (isClimb && (request.Flags & PathFlags.Climbing) != 0x0)
             {

Content.Server/NPC/Systems/NPCCombatSystem.Melee.cs

@@ -97,6 +97,12 @@ public sealed partial class NPCCombatSystem
             return;
         }
 
+        if (!_interaction.InRangeUnobstructed(uid, component.Target, distance + 0.1f))
+        {
+            component.Status = CombatStatus.TargetUnreachable;
+            return;
+        }
+
         if (weapon.NextAttack > curTime || !Enabled)
             return;
 

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

@@ -1,9 +1,11 @@
+using System.Diagnostics.CodeAnalysis;
 using System.Linq;
 using System.Numerics;
-using Content.Server.Examine;
 using Content.Server.NPC.Components;
 using Content.Server.NPC.Pathfinding;
-using Content.Shared.Climbing;
+using Content.Shared.Climbing.Components;
+using Content.Shared.CombatMode;
+using Content.Shared.Doors.Components;
 using Content.Shared.Interaction;
 using Content.Shared.Movement.Components;
 using Content.Shared.NPC;
@@ -208,19 +210,9 @@ public sealed partial class NPCSteeringSystem
             {
                 // Ignore stuck while handling obstacles.
                 ResetStuck(steering, ourCoordinates);
-                SteeringObstacleStatus status;
 
                 // Breaking behaviours and the likes.
-                lock (_obstacles)
-                {
-                    // We're still coming to a stop so wait for the do_after.
-                    if (body.LinearVelocity.LengthSquared() > 0.01f)
-                    {
-                        return true;
-                    }
-
-                    status = TryHandleFlags(uid, steering, node);
-                }
+                var status = TryHandleFlags(uid, steering, node);
 
                 // TODO: Need to handle re-pathing in case the target moves around.
                 switch (status)
@@ -497,6 +489,7 @@ public sealed partial class NPCSteeringSystem
     /// </summary>
     private void CollisionAvoidance(
         EntityUid uid,
+        NPCSteeringComponent steering,
         Angle offsetRot,
         Vector2 worldPos,
         float agentRadius,
@@ -509,20 +502,10 @@ public sealed partial class NPCSteeringSystem
         var detectionRadius = MathF.Max(0.35f, agentRadius + objectRadius);
         var ents = _entSetPool.Get();
         _lookup.GetEntitiesInRange(uid, detectionRadius, ents, LookupFlags.Dynamic | LookupFlags.Static | LookupFlags.Approximate);
-
+        // Filter out stuff that we can pry/vault/bash through
+        FilterObstacleEntities((uid, steering), mask, layer, ents, false);
         foreach (var ent in ents)
         {
-            // TODO: If we can access the door or smth.
-            if (!_physicsQuery.TryGetComponent(ent, out var otherBody) ||
-                !otherBody.Hard ||
-                !otherBody.CanCollide ||
-                otherBody.BodyType == BodyType.KinematicController ||
-                (mask & otherBody.CollisionLayer) == 0x0 &&
-                (layer & otherBody.CollisionMask) == 0x0)
-            {
-                continue;
-            }
-
             var xformB = _xformQuery.GetComponent(ent);
 
             if (!_physics.TryGetNearest(uid, ent,
@@ -564,6 +547,107 @@ public sealed partial class NPCSteeringSystem
         _entSetPool.Return(ents);
     }
 
+    /// <summary>
+    /// Filter down a list of entities into what can be considered obstacles.
+    ///
+    /// If allObstacles is set to false, don't filter out obstacles that can
+    /// be avoided by prying/vaulting/punching.
+    /// </summary>
+    private void FilterObstacleEntities(
+        Entity<NPCSteeringComponent> ent,
+        int mask,
+        int layer,
+        HashSet<EntityUid> nearbyEntities,
+        bool allObstacles = true)
+    {
+        if (!ent.Comp.CurrentPath.TryPeek(out var poly))
+            return;
+
+        var climbing = CompOrNull<ClimbingComponent>(ent.Owner);
+        var combatMode = CompOrNull<CombatModeComponent>(ent.Owner);
+
+        var checkDoors = (poly.Data.Flags & PathfindingBreadcrumbFlag.Door) != 0x0;
+        var checkClimbs = (poly.Data.Flags & PathfindingBreadcrumbFlag.Climb) != 0x0 &&
+                          (ent.Comp.Flags & PathFlags.Climbing) != 0x0 &&
+                          climbing != null;
+        var checkSmash = (ent.Comp.Flags & PathFlags.Smashing) != 0x0 &&
+                         combatMode != null &&
+                         _melee.TryGetWeapon(ent, out _, out var weapon) &&
+                         weapon.NextAttack <= _timing.CurTime;
+
+
+        nearbyEntities.RemoveWhere(nearbyEnt =>
+        {
+            // Get rid of stuff we can phase through.
+            if (!_physicsQuery.TryGetComponent(nearbyEnt, out var otherBody) ||
+                !otherBody.Hard ||
+                !otherBody.CanCollide ||
+                otherBody.BodyType == BodyType.KinematicController ||
+                (mask & otherBody.CollisionLayer) == 0x0 &&
+                (layer & otherBody.CollisionMask) == 0x0)
+            {
+                return true;
+            }
+
+            // If we just care about physical obstacles then this entity's checks are done.
+            if (allObstacles)
+                return false;
+
+            // If we're walking into a door we can handle...
+            if (checkDoors &&
+                CanHandleDoor(ent, poly.Data.Flags, nearbyEnt))
+                return true;
+
+            // Then check climbability.
+            if (checkClimbs &&
+                CanHandleClimb((ent, climbing!), nearbyEnt, out _))
+                return true;
+
+            // Check if we can smash. Should also check if we can even damage the entity at some point.
+            return checkSmash && _destructibleQuery.HasComponent(nearbyEnt);
+        });
+    }
+
+    private bool CanHandleDoor(Entity<NPCSteeringComponent> ent,
+        PathfindingBreadcrumbFlag flags,
+        EntityUid doorUid,
+        bool allowPrying = true)
+    {
+        if (!_doorQuery.TryComp(doorUid, out var door))
+            return false;
+
+        if (door.State == DoorState.Opening)
+            return true;
+
+        var isAccessRequired = (flags & PathfindingBreadcrumbFlag.Access) != 0x0 &&
+                               !_access.IsAllowed(ent, doorUid);
+        var canInteract = (ent.Comp.Flags & PathFlags.Interact) != 0x0;
+
+        // If not access locked we're fine if it can be bumped open or we can interact.
+        if (!isAccessRequired
+            && (door.BumpOpen || canInteract))
+            return true;
+
+        // Last possibility is that we can pry it open.
+        return allowPrying && (ent.Comp.Flags & PathFlags.Prying) != 0x0;
+    }
+
+    private bool CanHandleClimb(
+        Entity<ClimbingComponent> ent,
+        EntityUid climbableUid,
+        [NotNullWhen(true)] out ClimbableComponent? climbable)
+    {
+        if (!_climbableQuery.TryComp(climbableUid, out climbable))
+            return false;
+
+        // We're already climbing something, we're fine.
+        if (ent.Comp.IsClimbing || ent.Comp.NextTransition != null)
+            return true;
+
+        // Actually check if we can climb this
+        return _climb.CanVault(climbable, ent, climbableUid, out _);
+    }
+
     #endregion
 
     #region Dynamic Avoidance

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

@@ -58,160 +58,129 @@ public sealed partial class NPCSteeringSystem
 
         // TODO: Should cache the fact we're doing this somewhere.
         // See https://github.com/space-wizards/space-station-14/issues/11475
-        if ((poly.Data.CollisionLayer & mask) != 0x0 ||
-            (poly.Data.CollisionMask & layer) != 0x0)
+        if ((poly.Data.CollisionLayer & mask) == 0x0 &&
+            (poly.Data.CollisionMask & layer) == 0x0)
+            return SteeringObstacleStatus.Completed;
+
+        var id = component.DoAfterId;
+
+        // Still doing what we were doing before.
+        var doAfterStatus = _doAfter.GetStatus(id);
+
+        switch (doAfterStatus)
         {
-            var id = component.DoAfterId;
-
-            // Still doing what we were doing before.
-            var doAfterStatus = _doAfter.GetStatus(id);
-
-            switch (doAfterStatus)
-            {
-                case DoAfterStatus.Running:
-                    return SteeringObstacleStatus.Continuing;
-                case DoAfterStatus.Cancelled:
-                    return SteeringObstacleStatus.Failed;
-            }
-
-            var obstacleEnts = new List<EntityUid>();
-
-            GetObstacleEntities(poly, mask, layer, obstacleEnts);
-            var isDoor = (poly.Data.Flags & PathfindingBreadcrumbFlag.Door) != 0x0;
-            var isAccessRequired = (poly.Data.Flags & PathfindingBreadcrumbFlag.Access) != 0x0;
-            var isClimbable = (poly.Data.Flags & PathfindingBreadcrumbFlag.Climb) != 0x0;
-
-            // Just walk into it stupid
-            if (isDoor && !isAccessRequired)
-            {
-                // ... At least if it's not a bump open.
-                foreach (var ent in obstacleEnts)
-                {
-                    if (!_doorQuery.TryGetComponent(ent, out var door))
-                        continue;
-
-                    if (!door.BumpOpen && (component.Flags & PathFlags.Interact) != 0x0)
-                    {
-                        if (door.State != DoorState.Opening)
-                        {
-                            _interaction.InteractionActivate(uid, ent);
-                            return SteeringObstacleStatus.Continuing;
-                        }
-                    }
-                }
-
-                // If we get to here then didn't succeed for reasons.
-            }
-
-            if ((component.Flags & PathFlags.Prying) != 0x0 && isDoor)
-            {
-                // Get the relevant obstacle
-                foreach (var ent in obstacleEnts)
-                {
-                    if (_doorQuery.TryGetComponent(ent, out var door) && door.State != DoorState.Open)
-                    {
-                        // TODO: Use the verb.
-
-                        if (door.State != DoorState.Opening)
-                            _pryingSystem.TryPry(ent, uid, out id, uid);
-
-                        component.DoAfterId = id;
-                        return SteeringObstacleStatus.Continuing;
-                    }
-                }
-
-                if (obstacleEnts.Count == 0)
-                    return SteeringObstacleStatus.Completed;
-            }
-            // Try climbing obstacles
-            else if ((component.Flags & PathFlags.Climbing) != 0x0 && isClimbable)
-            {
-                if (TryComp<ClimbingComponent>(uid, out var climbing))
-                {
-                    if (climbing.IsClimbing)
-                    {
-                        return SteeringObstacleStatus.Completed;
-                    }
-                    else if (climbing.NextTransition != null)
-                    {
-                        return SteeringObstacleStatus.Continuing;
-                    }
-
-                    // Get the relevant obstacle
-                    foreach (var ent in obstacleEnts)
-                    {
-                        if (_climbableQuery.TryGetComponent(ent, out var table) &&
-                            _climb.CanVault(table, uid, uid, out _) &&
-                            _climb.TryClimb(uid, uid, ent, out id, table, climbing))
-                        {
-                            component.DoAfterId = id;
-                            return SteeringObstacleStatus.Continuing;
-                        }
-                    }
-                }
-
-                if (obstacleEnts.Count == 0)
-                    return SteeringObstacleStatus.Completed;
-            }
-            // Try smashing obstacles.
-            else if ((component.Flags & PathFlags.Smashing) != 0x0)
-            {
-                if (_melee.TryGetWeapon(uid, out _, out var meleeWeapon) && meleeWeapon.NextAttack <= _timing.CurTime && TryComp<CombatModeComponent>(uid, out var combatMode))
-                {
-                    _combat.SetInCombatMode(uid, true, combatMode);
-                    // TODO: This is a hack around grilles and windows.
-                    _random.Shuffle(obstacleEnts);
-                    var attackResult = false;
-
-                    foreach (var ent in obstacleEnts)
-                    {
-                        // TODO: Validate we can damage it
-                        if (_destructibleQuery.HasComponent(ent))
-                        {
-                            attackResult = _melee.AttemptLightAttack(uid, uid, meleeWeapon, ent);
-                            break;
-                        }
-                    }
-
-                    _combat.SetInCombatMode(uid, false, combatMode);
-
-                    // Blocked or the likes?
-                    if (!attackResult)
-                        return SteeringObstacleStatus.Failed;
-
-                    if (obstacleEnts.Count == 0)
-                        return SteeringObstacleStatus.Completed;
-
-                    return SteeringObstacleStatus.Continuing;
-                }
-            }
-
-            return SteeringObstacleStatus.Failed;
+            case DoAfterStatus.Running:
+                return SteeringObstacleStatus.Continuing;
+            case DoAfterStatus.Cancelled:
+                return SteeringObstacleStatus.Failed;
         }
 
-        return SteeringObstacleStatus.Completed;
-    }
-
-    private void GetObstacleEntities(PathPoly poly, int mask, int layer, List<EntityUid> ents)
-    {
-        // TODO: Can probably re-use this from pathfinding or something
         if (!TryComp<MapGridComponent>(poly.GraphUid, out var grid))
-        {
-            return;
-        }
+            return SteeringObstacleStatus.Completed;
 
-        foreach (var ent in _mapSystem.GetLocalAnchoredEntities(poly.GraphUid, grid, poly.Box))
+        var obstacleEnts = new HashSet<EntityUid>(_mapSystem.GetLocalAnchoredEntities(poly.GraphUid, grid, poly.Box));
+        FilterObstacleEntities((uid, component), mask, layer, obstacleEnts);
+
+        // Nothing actually near us.
+        if (obstacleEnts.Count == 0)
+            return SteeringObstacleStatus.Completed;
+
+        var isDoor = (poly.Data.Flags & PathfindingBreadcrumbFlag.Door) != 0x0;
+        var isClimbable = (poly.Data.Flags & PathfindingBreadcrumbFlag.Climb) != 0x0;
+
+        // Just walk into it stupid.
+        if (isDoor)
         {
-            if (!_physicsQuery.TryGetComponent(ent, out var body) ||
-                !body.Hard ||
-                !body.CanCollide ||
-                (body.CollisionMask & layer) == 0x0 && (body.CollisionLayer & mask) == 0x0)
+            foreach (var ent in obstacleEnts)
             {
-                continue;
+                // Includes checking if we have access:
+                if (!CanHandleDoor((uid, component), poly.Data.Flags, ent, false))
+                    continue;
+
+                // Interacts are bit nicer than bumps, so try interacting regardless.
+                _interaction.InteractionActivate(uid, ent);
+
+                return SteeringObstacleStatus.Continuing;
             }
 
-            ents.Add(ent);
+            // Couldn't normal-open the door, and can't pry. Give up.
+            if ((component.Flags & PathFlags.Prying) == 0x0)
+                return SteeringObstacleStatus.Failed;
+
+            foreach (var ent in obstacleEnts)
+            {
+                if (!CanHandleDoor((uid, component), poly.Data.Flags, ent))
+                    continue;
+
+                // Should be able to pry from CanHandleDoor:
+                // TODO: Use the verb.
+                _pryingSystem.TryPry(ent, uid, out id, uid);
+
+                if (id == null)
+                    return SteeringObstacleStatus.Failed;
+
+                component.DoAfterId = id;
+                return SteeringObstacleStatus.Continuing;
+            }
         }
+        // Try climbing obstacles
+        else if ((component.Flags & PathFlags.Climbing) != 0x0 && isClimbable)
+        {
+            if (!TryComp<ClimbingComponent>(uid, out var climbing))
+                return SteeringObstacleStatus.Failed;
+
+            if (climbing.IsClimbing)
+                return SteeringObstacleStatus.Completed;
+
+            if (climbing.NextTransition != null)
+                return SteeringObstacleStatus.Continuing;
+
+            // Get the relevant obstacle.
+            foreach (var ent in obstacleEnts)
+            {
+                if (CanHandleClimb((uid, climbing), ent, out var climbable) &&
+                    _climb.TryClimb(uid, uid, ent, out id, climbable, climbing))
+                {
+                    component.DoAfterId = id;
+                    return SteeringObstacleStatus.Continuing;
+                }
+            }
+        }
+        // Try smashing obstacles.
+        else if ((component.Flags & PathFlags.Smashing) != 0x0)
+        {
+            // Check we have a weapon, can (probably) swing it, and have combat mode.
+            if (!_melee.TryGetWeapon(uid, out var weaponUid, out var weaponComp) ||
+                weaponComp.NextAttack > _timing.CurTime ||
+                !TryComp<CombatModeComponent>(uid, out var combatMode))
+                return SteeringObstacleStatus.Failed;
+
+            _combat.SetInCombatMode(uid, true, combatMode);
+
+            var attackResult = false;
+            foreach (var ent in obstacleEnts)
+            {
+                if (!_destructibleQuery.HasComponent(ent) ||
+                    !_interaction.InRangeUnobstructed(uid, ent, weaponComp.Range))
+                    continue;
+
+                if (_melee.AttemptLightAttack(uid, weaponUid, weaponComp, ent))
+                {
+                    attackResult = true;
+                    break;
+                }
+            }
+
+            _combat.SetInCombatMode(uid, false, combatMode);
+
+            // Blocked or the likes?
+            if (!attackResult)
+                return SteeringObstacleStatus.Failed;
+
+            return SteeringObstacleStatus.Continuing;
+        }
+
+        return SteeringObstacleStatus.Failed;
     }
 
     private enum SteeringObstacleStatus : byte

Content.Server/NPC/Systems/NPCSteeringSystem.cs

@@ -6,6 +6,7 @@ using Content.Server.DoAfter;
 using Content.Server.NPC.Components;
 using Content.Server.NPC.Events;
 using Content.Server.NPC.Pathfinding;
+using Content.Shared.Access.Systems;
 using Content.Shared.CCVar;
 using Content.Shared.Climbing.Systems;
 using Content.Shared.CombatMode;
@@ -54,6 +55,7 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
     [Dependency] private IConfigurationManager _configManager = default!;
     [Dependency] private IGameTiming _timing = default!;
     [Dependency] private IRobustRandom _random = default!;
+    [Dependency] private AccessReaderSystem _access = default!;
     [Dependency] private ClimbSystem _climb = default!;
     [Dependency] private DoAfterSystem _doAfter = default!;
     [Dependency] private EntityLookupSystem _lookup = default!;
@@ -90,8 +92,6 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
 
     private readonly HashSet<ICommonSession> _subscribedSessions = new();
 
-    private object _obstacles = new();
-
     private int _activeSteeringCount;
 
     public override void Initialize()
@@ -373,7 +373,7 @@ public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem
         }
 
         // Avoid static objects like walls
-        CollisionAvoidance(uid, offsetRot, worldPos, agentRadius, layer, mask, xform, danger);
+        CollisionAvoidance(uid, steering, offsetRot, worldPos, agentRadius, layer, mask, xform, danger);
         DebugTools.Assert(!float.IsNaN(danger[0]));
 
         Separation(uid, offsetRot, worldPos, agentRadius, layer, mask, body, xform, danger);