From 494b9e5b93fa6c242befaefba00176d0ec51a31a Mon Sep 17 00:00:00 2001 From: Vordenburg <114301317+Vordenburg@users.noreply.github.com> Date: Fri, 28 Jul 2023 02:28:00 -0400 Subject: [PATCH] Add pathfinding for dynamic bodies (#17416) Co-authored-by: metalgearsloth <31366439+metalgearsloth@users.noreply.github.com> --- .../NPC/Pathfinding/PathfindingSystem.Grid.cs | 111 ++++++++++-------- .../NPC/Pathfinding/PathfindingSystem.cs | 1 + 2 files changed, 64 insertions(+), 48 deletions(-) diff --git a/Content.Server/NPC/Pathfinding/PathfindingSystem.Grid.cs b/Content.Server/NPC/Pathfinding/PathfindingSystem.Grid.cs index a775e1f9a5..13c4aa92b8 100644 --- a/Content.Server/NPC/Pathfinding/PathfindingSystem.Grid.cs +++ b/Content.Server/NPC/Pathfinding/PathfindingSystem.Grid.cs @@ -49,6 +49,7 @@ public sealed partial class PathfindingSystem SubscribeLocalEvent(OnGridPathPause); SubscribeLocalEvent(OnGridPathShutdown); SubscribeLocalEvent(OnCollisionChange); + SubscribeLocalEvent(OnCollisionLayerChange); SubscribeLocalEvent(OnBodyTypeChange); SubscribeLocalEvent(OnTileChange); SubscribeLocalEvent(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(ev.Entity, out var xform) && + if (TryComp(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(ev.Sender, out var body) || - body.BodyType != BodyType.Static || - HasComp(ev.Sender) || - ev.OldPosition.Equals(ev.NewPosition)) + if (!TryComp(ev.Sender, out var fixtures) || + !IsBodyRelevant(fixtures) || + HasComp(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(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++) diff --git a/Content.Server/NPC/Pathfinding/PathfindingSystem.cs b/Content.Server/NPC/Pathfinding/PathfindingSystem.cs index 4fcac8f7c7..4f74b01c4e 100644 --- a/Content.Server/NPC/Pathfinding/PathfindingSystem.cs +++ b/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!;