Add pathfinding for dynamic bodies (#17416)

Co-authored-by: metalgearsloth <31366439+metalgearsloth@users.noreply.github.com>

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

@@ -49,6 +49,7 @@ public sealed partial class PathfindingSystem
         SubscribeLocalEvent<GridPathfindingComponent, EntityUnpausedEvent>(OnGridPathPause);
         SubscribeLocalEvent<GridPathfindingComponent, ComponentShutdown>(OnGridPathShutdown);
         SubscribeLocalEvent<CollisionChangeEvent>(OnCollisionChange);
+        SubscribeLocalEvent<CollisionLayerChangeEvent>(OnCollisionLayerChange);
         SubscribeLocalEvent<PhysicsBodyTypeChangedEvent>(OnBodyTypeChange);
         SubscribeLocalEvent<TileChangedEvent>(OnTileChange);
         SubscribeLocalEvent<MoveEvent>(OnMoveEvent);
@@ -236,17 +237,18 @@ public sealed partial class PathfindingSystem
         }
     }
 
-    private bool IsBodyRelevant(PhysicsComponent body)
+    private bool IsBodyRelevant(FixturesComponent fixtures)
     {
-        if (!body.Hard || body.BodyType != BodyType.Static)
+        foreach (var fixture in fixtures.Fixtures.Values)
         {
-            return false;
-        }
+            if (!fixture.Hard)
+                continue;
 
-        if ((body.CollisionMask & PathfindingCollisionLayer) != 0x0 ||
-            (body.CollisionLayer & PathfindingCollisionMask) != 0x0)
-        {
-            return true;
+            if ((fixture.CollisionMask & PathfindingCollisionLayer) != 0x0 ||
+                (fixture.CollisionLayer & PathfindingCollisionMask) != 0x0)
+            {
+                return true;
+            }
         }
 
         return false;
@@ -254,35 +256,42 @@ public sealed partial class PathfindingSystem
 
     private void OnCollisionChange(ref CollisionChangeEvent ev)
     {
-        if (!IsBodyRelevant(ev.Body))
-            return;
-
         var xform = Transform(ev.Body.Owner);
 
         if (xform.GridUid == null)
             return;
 
         // This will also rebuild on door open / closes which I think is good?
-        DirtyChunk(xform.GridUid.Value, xform.Coordinates);
+        var aabb = _lookup.GetAABBNoContainer(ev.Body.Owner, xform.Coordinates.Position, xform.LocalRotation);
+        DirtyChunkArea(xform.GridUid.Value, aabb);
+    }
+
+    private void OnCollisionLayerChange(ref CollisionLayerChangeEvent ev)
+    {
+        var xform = Transform(ev.Body.Owner);
+
+        if (xform.GridUid == null)
+            return;
+
+        var aabb = _lookup.GetAABBNoContainer(ev.Body.Owner, xform.Coordinates.Position, xform.LocalRotation);
+        DirtyChunkArea(xform.GridUid.Value, aabb);
     }
 
     private void OnBodyTypeChange(ref PhysicsBodyTypeChangedEvent ev)
     {
-        if (ev.Component.CanCollide &&
-            IsBodyRelevant(ev.Component) &&
-            TryComp<TransformComponent>(ev.Entity, out var xform) &&
+        if (TryComp<TransformComponent>(ev.Entity, out var xform) &&
             xform.GridUid != null)
         {
-            DirtyChunk(xform.GridUid.Value, xform.Coordinates);
+            var aabb = _lookup.GetAABBNoContainer(ev.Entity, xform.Coordinates.Position, xform.LocalRotation);
+            DirtyChunkArea(xform.GridUid.Value, aabb);
         }
     }
 
     private void OnMoveEvent(ref MoveEvent ev)
     {
-        if (!TryComp<PhysicsComponent>(ev.Sender, out var body) ||
-            body.BodyType != BodyType.Static ||
-            HasComp<MapGridComponent>(ev.Sender) ||
-            ev.OldPosition.Equals(ev.NewPosition))
+        if (!TryComp<FixturesComponent>(ev.Sender, out var fixtures) ||
+            !IsBodyRelevant(fixtures) ||
+            HasComp<MapGridComponent>(ev.Sender))
         {
             return;
         }
@@ -292,34 +301,16 @@ public sealed partial class PathfindingSystem
             ? gridUid
             : ev.OldPosition.GetGridUid(EntityManager);
 
-        // Not on a grid at all so just ignore.
-        if (oldGridUid == gridUid && oldGridUid == null)
+        if (oldGridUid != null && oldGridUid != gridUid)
         {
-            return;
-        }
-
-        if (oldGridUid != null && gridUid != null)
-        {
-            // If the chunk hasn't changed then just dirty that one.
-            var oldOrigin = GetOrigin(ev.OldPosition, oldGridUid.Value);
-            var origin = GetOrigin(ev.NewPosition, gridUid.Value);
-
-            if (oldOrigin == origin)
-            {
-                // TODO: Don't need to transform again numpty.
-                DirtyChunk(oldGridUid.Value, ev.NewPosition);
-                return;
-            }
-        }
-
-        if (oldGridUid != null)
-        {
-            DirtyChunk(oldGridUid.Value, ev.OldPosition);
+            var aabb = _lookup.GetAABBNoContainer(ev.Sender, ev.OldPosition.Position, ev.OldRotation);
+            DirtyChunkArea(oldGridUid.Value, aabb);
         }
 
         if (gridUid != null)
         {
-            DirtyChunk(gridUid.Value, ev.NewPosition);
+            var aabb = _lookup.GetAABBNoContainer(ev.Sender, ev.NewPosition.Position, ev.NewRotation);
+            DirtyChunkArea(gridUid.Value, aabb);
         }
     }
 
@@ -362,6 +353,30 @@ public sealed partial class PathfindingSystem
         chunks.Add(GetOrigin(coordinates, gridUid));
     }
 
+    private void DirtyChunkArea(EntityUid gridUid, Box2 aabb)
+    {
+        if (!TryComp<GridPathfindingComponent>(gridUid, out var comp))
+            return;
+
+        var currentTime = _timing.CurTime;
+
+        if (comp.NextUpdate < currentTime)
+            comp.NextUpdate = currentTime + UpdateCooldown;
+
+        var chunks = comp.DirtyChunks;
+
+        // This assumes you never have bounds equal to or larger than 2 * ChunkSize.
+        var corners = new Vector2[] { aabb.BottomLeft, aabb.TopRight, aabb.BottomRight, aabb.TopLeft };
+        foreach (var corner in corners)
+        {
+            var sampledPoint = new Vector2i(
+                (int) Math.Floor((corner.X) / ChunkSize),
+                (int) Math.Floor((corner.Y) / ChunkSize));
+
+            chunks.Add(sampledPoint);
+        }
+    }
+
     private GridPathfindingChunk GetChunk(Vector2i origin, EntityUid uid, GridPathfindingComponent? component = null)
     {
         if (!Resolve(uid, ref component))
@@ -442,18 +457,18 @@ public sealed partial class PathfindingSystem
                 // var isBorder = x < 0 || y < 0 || x == ChunkSize - 1 || y == ChunkSize - 1;
 
                 tileEntities.Clear();
-                var anchored = grid.GetAnchoredEntitiesEnumerator(tilePos);
+                var available = _lookup.GetEntitiesIntersecting(tile);
 
-                while (anchored.MoveNext(out var ent))
+                foreach (var ent in available)
                 {
                     // Irrelevant for pathfinding
-                    if (!physicsQuery.TryGetComponent(ent, out var body) ||
-                        !IsBodyRelevant(body))
+                    if (!fixturesQuery.TryGetComponent(ent, out var fixtures) ||
+                        !IsBodyRelevant(fixtures))
                     {
                         continue;
                     }
 
-                    tileEntities.Add(ent.Value);
+                    tileEntities.Add(ent);
                 }
 
                 for (var subX = 0; subX < SubStep; subX++)

Content.Server/NPC/Pathfinding/PathfindingSystem.cs

@@ -44,6 +44,7 @@ namespace Content.Server.NPC.Pathfinding
         [Dependency] private readonly IPlayerManager _playerManager = default!;
         [Dependency] private readonly IRobustRandom _random = default!;
         [Dependency] private readonly DestructibleSystem _destructible = default!;
+        [Dependency] private readonly EntityLookupSystem _lookup = default!;
         [Dependency] private readonly FixtureSystem _fixtures = default!;
         [Dependency] private readonly SharedPhysicsSystem _physics = default!;