Files
tbd-station-14/Content.Server/NPC/Pathfinding/PathfindingSystem.cs
2022-11-18 22:07:52 +01:00

703 lines
23 KiB
C#

using System.Buffers;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;
using Content.Server.Administration.Managers;
using Content.Server.Destructible;
using Content.Server.NPC.Components;
using Content.Shared.Administration;
using Content.Shared.Interaction;
using Content.Shared.NPC;
using Robust.Server.Player;
using Robust.Shared.Enums;
using Robust.Shared.Map;
using Robust.Shared.Physics.Components;
using Robust.Shared.Physics.Systems;
using Robust.Shared.Players;
using Robust.Shared.Random;
using Robust.Shared.Timing;
using Robust.Shared.Utility;
namespace Content.Server.NPC.Pathfinding
{
/// <summary>
/// This system handles pathfinding graph updates as well as dispatches to the pathfinder
/// (90% of what it's doing is graph updates so not much point splitting the 2 roles)
/// </summary>
public sealed partial class PathfindingSystem : SharedPathfindingSystem
{
/*
* I have spent many hours looking at what pathfinding to use
* Ideally we would be able to use something grid based with hierarchy, but the problem is
* we also have triangular / diagonal walls and thindows which makes that not exactly feasible
* Recast is also overkill for our usecase, plus another lib, hence you get this.
*
* See PathfindingSystem.Grid for a description of the grid implementation.
*/
[Dependency] private readonly IAdminManager _adminManager = default!;
[Dependency] private readonly IGameTiming _timing = default!;
[Dependency] private readonly IPlayerManager _playerManager = default!;
[Dependency] private readonly IRobustRandom _random = default!;
[Dependency] private readonly DestructibleSystem _destructible = default!;
[Dependency] private readonly FixtureSystem _fixtures = default!;
private ISawmill _sawmill = default!;
private readonly Dictionary<ICommonSession, PathfindingDebugMode> _subscribedSessions = new();
private readonly List<PathRequest> _pathRequests = new(PathTickLimit);
private static readonly TimeSpan PathTime = TimeSpan.FromMilliseconds(3);
/// <summary>
/// How many paths we can process in a single tick.
/// </summary>
private const int PathTickLimit = 256;
private int _portalIndex;
private Dictionary<int, PathPortal> _portals = new();
public override void Initialize()
{
base.Initialize();
_sawmill = Logger.GetSawmill("nav");
_playerManager.PlayerStatusChanged += OnPlayerChange;
InitializeGrid();
SubscribeNetworkEvent<RequestPathfindingDebugMessage>(OnBreadcrumbs);
}
public override void Shutdown()
{
base.Shutdown();
_subscribedSessions.Clear();
_playerManager.PlayerStatusChanged -= OnPlayerChange;
}
public override void Update(float frameTime)
{
base.Update(frameTime);
UpdateGrid();
_stopwatch.Restart();
var amount = Math.Min(PathTickLimit, _pathRequests.Count);
var results = ArrayPool<PathResult>.Shared.Rent(amount);
Parallel.For(0, amount, i =>
{
// If we're over the limit (either time-sliced or hard cap).
if (_stopwatch.Elapsed >= PathTime)
{
results[i] = PathResult.Continuing;
return;
}
var request = _pathRequests[i];
try
{
switch (request)
{
case AStarPathRequest astar:
results[i] = UpdateAStarPath(astar);
break;
case BFSPathRequest bfs:
results[i] = UpdateBFSPath(_random, bfs);
break;
default:
throw new NotImplementedException();
}
}
catch (Exception)
{
results[i] = PathResult.NoPath;
throw;
}
});
var offset = 0;
// then, single-threaded cleanup.
for (var i = 0; i < amount; i++)
{
var resultIndex = i + offset;
var path = _pathRequests[resultIndex];
var result = results[i];
if (path.Task.Exception != null)
{
throw path.Task.Exception;
}
switch (result)
{
case PathResult.Continuing:
break;
case PathResult.PartialPath:
case PathResult.Path:
case PathResult.NoPath:
SendDebug(path);
// Don't use RemoveSwap because we still want to try and process them in order.
_pathRequests.RemoveAt(resultIndex);
offset--;
path.Tcs.SetResult(result);
SendRoute(path);
break;
default:
throw new NotImplementedException();
}
}
ArrayPool<PathResult>.Shared.Return(results);
}
/// <summary>
/// Creates neighbouring edges at both locations, each leading to the other.
/// </summary>
public bool TryCreatePortal(EntityCoordinates coordsA, EntityCoordinates coordsB, out int handle)
{
var mapUidA = coordsA.GetMapUid(EntityManager);
var mapUidB = coordsB.GetMapUid(EntityManager);
handle = -1;
if (mapUidA != mapUidB || mapUidA == null)
{
return false;
}
var gridUidA = coordsA.GetGridUid(EntityManager);
var gridUidB = coordsB.GetGridUid(EntityManager);
if (!TryComp<GridPathfindingComponent>(gridUidA, out var gridA) ||
!TryComp<GridPathfindingComponent>(gridUidB, out var gridB))
{
return false;
}
handle = _portalIndex++;
var portal = new PathPortal(handle, coordsA, coordsB);
_portals[handle] = portal;
var originA = GetOrigin(coordsA, gridUidA.Value);
var originB = GetOrigin(coordsB, gridUidB.Value);
gridA.PortalLookup.Add(portal, originA);
gridB.PortalLookup.Add(portal, originB);
var chunkA = GetChunk(originA, gridUidA.Value);
var chunkB = GetChunk(originB, gridUidB.Value);
chunkA.Portals.Add(portal);
chunkB.Portals.Add(portal);
// TODO: You already have the chunks
DirtyChunk(gridUidA.Value, coordsA);
DirtyChunk(gridUidB.Value, coordsB);
return true;
}
public bool RemovePortal(int handle)
{
if (!_portals.TryGetValue(handle, out var portal))
{
return false;
}
_portals.Remove(handle);
var gridUidA = portal.CoordinatesA.GetGridUid(EntityManager);
var gridUidB = portal.CoordinatesB.GetGridUid(EntityManager);
if (!TryComp<GridPathfindingComponent>(gridUidA, out var gridA) ||
!TryComp<GridPathfindingComponent>(gridUidB, out var gridB))
{
return false;
}
gridA.PortalLookup.Remove(portal);
gridB.PortalLookup.Remove(portal);
var chunkA = GetChunk(GetOrigin(portal.CoordinatesA, gridUidA.Value), gridUidA.Value, gridA);
var chunkB = GetChunk(GetOrigin(portal.CoordinatesB, gridUidB.Value), gridUidB.Value, gridB);
chunkA.Portals.Remove(portal);
chunkB.Portals.Remove(portal);
DirtyChunk(gridUidA.Value, portal.CoordinatesA);
DirtyChunk(gridUidB.Value, portal.CoordinatesB);
return true;
}
public async Task<PathResultEvent> GetRandomPath(
EntityUid entity,
float range,
float maxRange,
CancellationToken cancelToken,
int limit = 40,
PathFlags flags = PathFlags.None)
{
if (!TryComp<TransformComponent>(entity, out var start))
return new PathResultEvent(PathResult.NoPath, new Queue<PathPoly>());
var layer = 0;
var mask = 0;
if (TryComp<PhysicsComponent>(entity, out var body))
{
layer = body.CollisionLayer;
mask = body.CollisionMask;
}
var request = new BFSPathRequest(maxRange, limit, start.Coordinates, flags, range, layer, mask, cancelToken);
var path = await GetPath(request);
if (path.Result != PathResult.Path)
return new PathResultEvent(PathResult.NoPath, new Queue<PathPoly>());
return new PathResultEvent(PathResult.Path, path.Path);
}
/// <summary>
/// Gets the estimated distance from the entity to the target node.
/// </summary>
public async Task<float?> GetPathDistance(
EntityUid entity,
EntityCoordinates end,
float range,
CancellationToken cancelToken,
PathFlags flags = PathFlags.None)
{
if (!TryComp<TransformComponent>(entity, out var start))
return null;
var request = GetRequest(entity, start.Coordinates, end, range, cancelToken, flags);
var path = await GetPath(request);
if (path.Result != PathResult.Path)
return null;
if (path.Path.Count == 0)
return 0f;
var distance = 0f;
var node = path.Path.Dequeue();
var lastNode = node;
do
{
distance += GetTileCost(request, lastNode, node);
lastNode = node;
} while (path.Path.TryDequeue(out node));
return distance;
}
public async Task<PathResultEvent> GetPath(
EntityUid entity,
EntityUid target,
float range,
CancellationToken cancelToken,
PathFlags flags = PathFlags.None)
{
if (!TryComp<TransformComponent>(entity, out var xform) ||
!TryComp<TransformComponent>(target, out var targetXform))
return new PathResultEvent(PathResult.NoPath, new Queue<PathPoly>());
var request = GetRequest(entity, xform.Coordinates, targetXform.Coordinates, range, cancelToken, flags);
return await GetPath(request);
}
public async Task<PathResultEvent> GetPath(
EntityUid entity,
EntityCoordinates start,
EntityCoordinates end,
float range,
CancellationToken cancelToken,
PathFlags flags = PathFlags.None)
{
var request = GetRequest(entity, start, end, range, cancelToken, flags);
return await GetPath(request);
}
/// <summary>
/// Asynchronously gets a path.
/// </summary>
public async Task<PathResultEvent> GetPath(
EntityCoordinates start,
EntityCoordinates end,
float range,
int layer,
int mask,
CancellationToken cancelToken,
PathFlags flags = PathFlags.None)
{
// Don't allow the caller to pass in the request in case they try to do something with its data.
var request = new AStarPathRequest(start, end, flags, range, layer, mask, cancelToken);
return await GetPath(request);
}
/// <summary>
/// Raises the pathfinding result event on the entity when finished.
/// </summary>
public async void GetPathEvent(
EntityUid uid,
EntityCoordinates start,
EntityCoordinates end,
float range,
CancellationToken cancelToken,
PathFlags flags = PathFlags.None)
{
var path = await GetPath(uid, start, end, range, cancelToken);
RaiseLocalEvent(uid, path);
}
/// <summary>
/// Gets the relevant poly for the specified coordinates if it exists.
/// </summary>
public PathPoly? GetPoly(EntityCoordinates coordinates)
{
var gridUid = coordinates.GetGridUid(EntityManager);
if (!TryComp<GridPathfindingComponent>(gridUid, out var comp) ||
!TryComp<TransformComponent>(gridUid, out var xform))
{
return null;
}
var localPos = xform.InvWorldMatrix.Transform(coordinates.ToMapPos(EntityManager));
var origin = GetOrigin(localPos);
if (!TryGetChunk(origin, comp, out var chunk))
return null;
var chunkPos = new Vector2(MathHelper.Mod(localPos.X, ChunkSize), MathHelper.Mod(localPos.Y, ChunkSize));
var polys = chunk.Polygons[(int) chunkPos.X * ChunkSize + (int) chunkPos.Y];
foreach (var poly in polys)
{
if (!poly.Box.Contains(localPos))
continue;
return poly;
}
return null;
}
private PathRequest GetRequest(EntityUid entity, EntityCoordinates start, EntityCoordinates end, float range, CancellationToken cancelToken, PathFlags flags)
{
var layer = 0;
var mask = 0;
if (TryComp<PhysicsComponent>(entity, out var body))
{
layer = body.CollisionLayer;
mask = body.CollisionMask;
}
return new AStarPathRequest(start, end, flags, range, layer, mask, cancelToken);
}
public PathFlags GetFlags(EntityUid uid)
{
if (!TryComp<NPCComponent>(uid, out var npc))
{
return PathFlags.None;
}
return GetFlags(npc.Blackboard);
}
public PathFlags GetFlags(NPCBlackboard blackboard)
{
var flags = PathFlags.None;
if (blackboard.TryGetValue<bool>(NPCBlackboard.NavPry, out var pry, EntityManager) && pry)
{
flags |= PathFlags.Prying;
}
if (blackboard.TryGetValue<bool>(NPCBlackboard.NavSmash, out var smash, EntityManager) && smash)
{
flags |= PathFlags.Smashing;
}
if (blackboard.TryGetValue<bool>(NPCBlackboard.NavInteract, out var interact, EntityManager) && interact)
{
flags |= PathFlags.Interact;
}
return flags;
}
private async Task<PathResultEvent> GetPath(
PathRequest request)
{
// We could maybe try an initial quick run to avoid forcing time-slicing over ticks.
// For now it seems okay and it shouldn't block on 1 NPC anyway.
_pathRequests.Add(request);
await request.Task;
if (request.Task.Exception != null)
{
throw request.Task.Exception;
}
if (!request.Task.IsCompletedSuccessfully)
{
return new PathResultEvent(PathResult.NoPath, new Queue<PathPoly>());
}
// Same context as do_after and not synchronously blocking soooo
#pragma warning disable RA0004
var ev = new PathResultEvent(request.Task.Result, request.Polys);
#pragma warning restore RA0004
return ev;
}
#region Debug handlers
private DebugPathPoly GetDebugPoly(PathPoly poly)
{
// Create fake neighbors for it
var neighbors = new List<EntityCoordinates>(poly.Neighbors.Count);
foreach (var neighbor in poly.Neighbors)
{
neighbors.Add(neighbor.Coordinates);
}
return new DebugPathPoly()
{
GraphUid = poly.GraphUid,
ChunkOrigin = poly.ChunkOrigin,
TileIndex = poly.TileIndex,
Box = poly.Box,
Data = poly.Data,
Neighbors = neighbors,
};
}
private void SendDebug(PathRequest request)
{
if (_subscribedSessions.Count == 0)
return;
foreach (var session in _subscribedSessions)
{
if ((session.Value & PathfindingDebugMode.Routes) == 0x0)
continue;
RaiseNetworkEvent(new PathRouteMessage(request.Polys.Select(GetDebugPoly).ToList(), new Dictionary<DebugPathPoly, float>()), session.Key.ConnectedClient);
}
}
private void OnBreadcrumbs(RequestPathfindingDebugMessage msg, EntitySessionEventArgs args)
{
var pSession = (IPlayerSession) args.SenderSession;
if (!_adminManager.HasAdminFlag(pSession, AdminFlags.Debug))
{
return;
}
var sessions = _subscribedSessions.GetOrNew(args.SenderSession);
if (msg.Mode == PathfindingDebugMode.None)
{
_subscribedSessions.Remove(args.SenderSession);
return;
}
sessions = msg.Mode;
_subscribedSessions[args.SenderSession] = sessions;
if (IsCrumb(sessions))
{
SendBreadcrumbs(pSession);
}
if (IsPoly(sessions))
{
SendPolys(pSession);
}
}
private bool IsCrumb(PathfindingDebugMode mode)
{
return (mode & (PathfindingDebugMode.Breadcrumbs | PathfindingDebugMode.Crumb)) != 0x0;
}
private bool IsPoly(PathfindingDebugMode mode)
{
return (mode & (PathfindingDebugMode.Chunks | PathfindingDebugMode.Polys | PathfindingDebugMode.Poly | PathfindingDebugMode.PolyNeighbors)) != 0x0;
}
private bool IsRoute(PathfindingDebugMode mode)
{
return (mode & (PathfindingDebugMode.Routes | PathfindingDebugMode.RouteCosts)) != 0x0;
}
private void SendBreadcrumbs(ICommonSession pSession)
{
var msg = new PathBreadcrumbsMessage();
foreach (var comp in EntityQuery<GridPathfindingComponent>(true))
{
msg.Breadcrumbs.Add(comp.Owner, new Dictionary<Vector2i, List<PathfindingBreadcrumb>>(comp.Chunks.Count));
foreach (var chunk in comp.Chunks)
{
var data = GetCrumbs(chunk.Value);
msg.Breadcrumbs[comp.Owner].Add(chunk.Key, data);
}
}
RaiseNetworkEvent(msg, pSession.ConnectedClient);
}
private void SendRoute(PathRequest request)
{
if (_subscribedSessions.Count == 0)
return;
var polys = new List<DebugPathPoly>();
var costs = new Dictionary<DebugPathPoly, float>();
foreach (var poly in request.Polys)
{
polys.Add(GetDebugPoly(poly));
}
foreach (var (poly, value) in request.CostSoFar)
{
costs.Add(GetDebugPoly(poly), value);
}
var msg = new PathRouteMessage(polys, costs);
foreach (var session in _subscribedSessions)
{
if (!IsRoute(session.Value))
continue;
RaiseNetworkEvent(msg, session.Key.ConnectedClient);
}
}
private void SendPolys(ICommonSession pSession)
{
var msg = new PathPolysMessage();
foreach (var comp in EntityQuery<GridPathfindingComponent>(true))
{
msg.Polys.Add(comp.Owner, new Dictionary<Vector2i, Dictionary<Vector2i, List<DebugPathPoly>>>(comp.Chunks.Count));
foreach (var chunk in comp.Chunks)
{
var data = GetPolys(chunk.Value);
msg.Polys[comp.Owner].Add(chunk.Key, data);
}
}
RaiseNetworkEvent(msg, pSession.ConnectedClient);
}
private void SendBreadcrumbs(GridPathfindingChunk chunk, EntityUid gridUid)
{
if (_subscribedSessions.Count == 0)
return;
var msg = new PathBreadcrumbsRefreshMessage()
{
Origin = chunk.Origin,
GridUid = gridUid,
Data = GetCrumbs(chunk),
};
foreach (var session in _subscribedSessions)
{
if (!IsCrumb(session.Value))
continue;
RaiseNetworkEvent(msg, session.Key.ConnectedClient);
}
}
private void SendPolys(GridPathfindingChunk chunk, EntityUid gridUid,
List<PathPoly>[] tilePolys)
{
if (_subscribedSessions.Count == 0)
return;
var data = new Dictionary<Vector2i, List<DebugPathPoly>>(tilePolys.Length);
var extent = Math.Sqrt(tilePolys.Length);
for (var x = 0; x < extent; x++)
{
for (var y = 0; y < extent; y++)
{
var index = GetIndex(x, y);
data[new Vector2i(x, y)] = tilePolys[index].Select(GetDebugPoly).ToList();
}
}
var msg = new PathPolysRefreshMessage()
{
Origin = chunk.Origin,
GridUid = gridUid,
Polys = data,
};
foreach (var session in _subscribedSessions)
{
if (!IsPoly(session.Value))
continue;
RaiseNetworkEvent(msg, session.Key.ConnectedClient);
}
}
private List<PathfindingBreadcrumb> GetCrumbs(GridPathfindingChunk chunk)
{
var crumbs = new List<PathfindingBreadcrumb>(chunk.Points.Length);
const int extent = ChunkSize * SubStep;
for (var x = 0; x < extent; x++)
{
for (var y = 0; y < extent; y++)
{
crumbs.Add(chunk.Points[x, y]);
}
}
return crumbs;
}
private Dictionary<Vector2i, List<DebugPathPoly>> GetPolys(GridPathfindingChunk chunk)
{
var polys = new Dictionary<Vector2i, List<DebugPathPoly>>(chunk.Polygons.Length);
for (var x = 0; x < ChunkSize; x++)
{
for (var y = 0; y < ChunkSize; y++)
{
var index = GetIndex(x, y);
polys[new Vector2i(x, y)] = chunk.Polygons[index].Select(GetDebugPoly).ToList();
}
}
return polys;
}
private void OnPlayerChange(object? sender, SessionStatusEventArgs e)
{
if (e.NewStatus == SessionStatus.Connected || !_subscribedSessions.ContainsKey(e.Session))
return;
_subscribedSessions.Remove(e.Session);
}
#endregion
}
}