tommy/tbd-station-14
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pathfinder rework (#11452)

Browse Source
This commit is contained in:
metalgearsloth
2022-09-30 14:39:48 +10:00
committed by GitHub
parent fd3b29fb03
commit f456ad911e
80 changed files with 3606 additions and 4374 deletions
Download Patch File Download Diff File Expand all files Collapse all files

344
Content.Server/NPC/Systems/NPCSteeringSystem.cs
View File

@@ -1,17 +1,19 @@
using System.Linq;
using System.Threading;
using Content.Server.CPUJob.JobQueues;
using Content.Server.Doors.Systems;
using Content.Server.NPC.Components;
using Content.Server.NPC.Pathfinding;
using Content.Server.NPC.Pathfinding.Pathfinders;
using Content.Shared.Access.Systems;
using Content.Shared.CCVar;
using Content.Shared.Interaction;
using Content.Shared.Movement.Components;
using Content.Shared.Movement.Systems;
using Content.Shared.NPC;
using Content.Shared.Weapons.Melee;
using Robust.Shared.Configuration;
using Robust.Shared.Map;
using Robust.Shared.Physics;
using Robust.Shared.Physics.Components;
using Robust.Shared.Physics.Systems;
using Robust.Shared.Random;
using Robust.Shared.Timing;
namespace Content.Server.NPC.Systems
@@ -22,18 +24,22 @@ namespace Content.Server.NPC.Systems
[Dependency] private readonly IConfigurationManager _configManager = default!;
[Dependency] private readonly IGameTiming _timing = default!;
[Dependency] private readonly IMapManager _mapManager = default!;
[Dependency] private readonly AccessReaderSystem _accessReader = default!;
[Dependency] private readonly IRobustRandom _random = default!;
[Dependency] private readonly DoorSystem _doors = default!;
[Dependency] private readonly PathfindingSystem _pathfindingSystem = default!;
[Dependency] private readonly SharedPhysicsSystem _physics = default!;
[Dependency] private readonly SharedInteractionSystem _interaction = default!;
[Dependency] private readonly SharedMeleeWeaponSystem _melee = default!;
[Dependency] private readonly SharedMoverController _mover = default!;
// This will likely get moved onto an abstract pathfinding node that specifies the max distance allowed from the coordinate.
private const float TileTolerance = 0.4f;
private const float TileTolerance = 0.40f;
private bool _enabled;
public override void Initialize()
{
base.Initialize();
UpdatesBefore.Add(typeof(SharedPhysicsSystem));
InitializeAvoidance();
_configManager.OnValueChanged(CCVars.NPCEnabled, SetNPCEnabled, true);
@@ -68,22 +74,37 @@ namespace Content.Server.NPC.Systems
/// <summary>
/// Adds the AI to the steering system to move towards a specific target
/// </summary>
public NPCSteeringComponent Register(EntityUid uid, EntityCoordinates coordinates)
public NPCSteeringComponent Register(EntityUid uid, EntityCoordinates coordinates, NPCSteeringComponent? component = null)
{
if (TryComp<NPCSteeringComponent>(uid, out var comp))
if (Resolve(uid, ref component, false))
{
comp.PathfindToken?.Cancel();
comp.PathfindToken = null;
comp.CurrentPath.Clear();
component.PathfindToken?.Cancel();
component.PathfindToken = null;
component.CurrentPath.Clear();
}
else
{
comp = AddComp<NPCSteeringComponent>(uid);
component = AddComp<NPCSteeringComponent>(uid);
component.Flags = _pathfindingSystem.GetFlags(uid);
}
EnsureComp<NPCRVOComponent>(uid);
comp.Coordinates = coordinates;
return comp;
component.Coordinates = coordinates;
return component;
}
/// <summary>
/// Attempts to register the entity. Does nothing if the coordinates already registered.
/// </summary>
public bool TryRegister(EntityUid uid, EntityCoordinates coordinates, NPCSteeringComponent? component = null)
{
if (Resolve(uid, ref component, false) && component.Coordinates.Equals(coordinates))
{
return false;
}
Register(uid, coordinates, component);
return true;
}
/// <summary>
@@ -101,7 +122,6 @@ namespace Content.Server.NPC.Systems
component.PathfindToken?.Cancel();
component.PathfindToken = null;
component.Pathfind = null;
RemComp<NPCRVOComponent>(uid);
RemComp<NPCSteeringComponent>(uid);
}
@@ -120,15 +140,21 @@ namespace Content.Server.NPC.Systems
var npcs = EntityQuery<NPCSteeringComponent, ActiveNPCComponent, InputMoverComponent, TransformComponent>()
.ToArray();
// TODO: Do this in parallel. This will require pathfinder refactor to not use jobqueue.
// TODO: Do this in parallel.
// Main obstacle is requesting a new path needs to be done synchronously
foreach (var (steering, _, mover, xform) in npcs)
{
Steer(steering, mover, xform, modifierQuery, bodyQuery, frameTime);
}
}
private void SetDirection(InputMoverComponent component, Vector2 value)
private void SetDirection(InputMoverComponent component, NPCSteeringComponent steering, Vector2 value, bool clear = true)
{
if (clear && value.Equals(Vector2.Zero))
{
steering.CurrentPath.Clear();
}
component.CurTickSprintMovement = value;
component.LastInputTick = _timing.CurTick;
component.LastInputSubTick = ushort.MaxValue;
@@ -145,6 +171,13 @@ namespace Content.Server.NPC.Systems
EntityQuery<PhysicsComponent> bodyQuery,
float frameTime)
{
if (Deleted(steering.Coordinates.EntityId))
{
SetDirection(mover, steering, Vector2.Zero);
steering.Status = SteeringStatus.NoPath;
return;
}
var ourCoordinates = xform.Coordinates;
var destinationCoordinates = steering.Coordinates;
@@ -152,54 +185,46 @@ namespace Content.Server.NPC.Systems
if (xform.Coordinates.TryDistance(EntityManager, destinationCoordinates, out var distance) &&
distance <= steering.Range)
{
SetDirection(mover, Vector2.Zero);
SetDirection(mover, steering, Vector2.Zero);
steering.Status = SteeringStatus.InRange;
return;
}
// No path set from pathfinding or the likes.
if (steering.Status == SteeringStatus.NoPath)
{
SetDirection(mover, steering, Vector2.Zero);
return;
}
// Can't move at all, just noop input.
if (!mover.CanMove)
{
SetDirection(mover, Vector2.Zero);
SetDirection(mover, steering, Vector2.Zero);
steering.Status = SteeringStatus.Moving;
return;
}
// If we were pathfinding then try to update our path.
if (steering.Pathfind != null)
// Grab the target position, either the next path node or our end goal.
// TODO: Some situations we may not want to move at our target without a path.
var targetCoordinates = GetTargetCoordinates(steering);
var needsPath = false;
// If the next node is invalid then get new ones
if (!targetCoordinates.IsValid(EntityManager))
{
switch (steering.Pathfind.Status)
if (steering.CurrentPath.TryPeek(out var poly) &&
(poly.Data.Flags & PathfindingBreadcrumbFlag.Invalid) != 0x0)
{
case JobStatus.Waiting:
case JobStatus.Running:
case JobStatus.Pending:
case JobStatus.Paused:
break;
case JobStatus.Finished:
steering.CurrentPath.Clear();
if (steering.Pathfind.Result != null)
{
PrunePath(ourCoordinates, steering.Pathfind.Result);
foreach (var node in steering.Pathfind.Result)
{
steering.CurrentPath.Enqueue(node);
}
}
steering.Pathfind = null;
steering.PathfindToken = null;
break;
default:
throw new ArgumentOutOfRangeException();
steering.CurrentPath.Dequeue();
// Try to get the next node temporarily.
targetCoordinates = GetTargetCoordinates(steering);
needsPath = true;
}
}
// Grab the target position, either the path or our end goal.
// TODO: Some situations we may not want to move at our target without a path.
var targetCoordinates = GetTargetCoordinates(steering);
var arrivalDistance = TileTolerance;
// Need to be pretty close if it's just a node to make sure LOS for door bashes or the likes.
float arrivalDistance;
if (targetCoordinates.Equals(steering.Coordinates))
{
@@ -207,6 +232,10 @@ namespace Content.Server.NPC.Systems
// If it's a pathfinding node it might be different to the destination.
arrivalDistance = steering.Range;
}
else
{
arrivalDistance = SharedInteractionSystem.InteractionRange - 0.8f;
}
// Check if mapids match.
var targetMap = targetCoordinates.ToMap(EntityManager);
@@ -214,73 +243,87 @@ namespace Content.Server.NPC.Systems
if (targetMap.MapId != ourMap.MapId)
{
SetDirection(mover, Vector2.Zero);
SetDirection(mover, steering, Vector2.Zero);
steering.Status = SteeringStatus.NoPath;
steering.CurrentTarget = targetCoordinates;
return;
}
var direction = targetMap.Position - ourMap.Position;
if (steering.Owner == new EntityUid(15315))
{
}
// Are we in range
if (direction.Length <= arrivalDistance)
{
// It was just a node, not the target, so grab the next destination (either the target or next node).
if (steering.CurrentPath.Count > 0)
// Node needs some kind of special handling like access or smashing.
if (steering.CurrentPath.TryPeek(out var node))
{
steering.CurrentPath.Dequeue();
var status = TryHandleFlags(steering, node, bodyQuery);
// Alright just adjust slightly and grab the next node so we don't stop moving for a tick.
// TODO: If it's the last node just grab the target instead.
targetCoordinates = GetTargetCoordinates(steering);
targetMap = targetCoordinates.ToMap(EntityManager);
// Can't make it again.
if (ourMap.MapId != targetMap.MapId)
// TODO: Need to handle re-pathing in case the target moves around.
switch (status)
{
SetDirection(mover, Vector2.Zero);
steering.Status = SteeringStatus.NoPath;
steering.CurrentTarget = targetCoordinates;
case SteeringObstacleStatus.Completed:
break;
case SteeringObstacleStatus.Failed:
// TODO: Blacklist the poly for next query
SetDirection(mover, steering, Vector2.Zero);
steering.Status = SteeringStatus.NoPath;
return;
case SteeringObstacleStatus.Continuing:
SetDirection(mover, steering, Vector2.Zero, false);
CheckPath(steering, xform, needsPath, distance);
return;
default:
throw new ArgumentOutOfRangeException();
}
}
// Otherwise it's probably regular pathing so just keep going a bit more to get to tile centre
if (direction.Length <= TileTolerance)
{
// It was just a node, not the target, so grab the next destination (either the target or next node).
if (steering.CurrentPath.Count > 0)
{
steering.CurrentPath.Dequeue();
// Alright just adjust slightly and grab the next node so we don't stop moving for a tick.
// TODO: If it's the last node just grab the target instead.
targetCoordinates = GetTargetCoordinates(steering);
targetMap = targetCoordinates.ToMap(EntityManager);
// Can't make it again.
if (ourMap.MapId != targetMap.MapId)
{
SetDirection(mover, steering, Vector2.Zero);
steering.Status = SteeringStatus.NoPath;
return;
}
// Gonna resume now business as usual
direction = targetMap.Position - ourMap.Position;
}
else
{
// This probably shouldn't happen as we check above but eh.
SetDirection(mover, steering, Vector2.Zero);
steering.Status = SteeringStatus.InRange;
return;
}
// Gonna resume now business as usual
direction = targetMap.Position - ourMap.Position;
}
else
{
// This probably shouldn't happen as we check above but eh.
SetDirection(mover, Vector2.Zero);
steering.Status = SteeringStatus.InRange;
steering.CurrentTarget = targetCoordinates;
return;
}
}
// Do we have no more nodes to follow OR has the target moved sufficiently? If so then re-path.
var needsPath = steering.CurrentPath.Count == 0;
// TODO: Probably need partial planning support i.e. patch from the last node to where the target moved to.
if (!needsPath)
{
var lastNode = steering.CurrentPath.Last();
// I know this is bad and doesn't account for tile size
// However with the path I'm going to change it to return pathfinding nodes which include coordinates instead.
var lastCoordinate = new EntityCoordinates(lastNode.GridUid, (Vector2) lastNode.GridIndices + 0.5f);
if (lastCoordinate.TryDistance(EntityManager, steering.Coordinates, out var lastDistance) &&
lastDistance > steering.RepathRange)
{
needsPath = true;
}
needsPath = steering.CurrentPath.Count == 0 || (steering.CurrentPath.Peek().Data.Flags & PathfindingBreadcrumbFlag.Invalid) != 0x0;
}
// Request the new path.
if (needsPath && bodyQuery.TryGetComponent(steering.Owner, out var body))
{
RequestPath(steering, xform, body);
}
// TODO: Probably need partial planning support i.e. patch from the last node to where the target moved to.
CheckPath(steering, xform, needsPath, distance);
modifierQuery.TryGetComponent(steering.Owner, out var modifier);
var moveSpeed = GetSprintSpeed(steering.Owner, modifier);
@@ -293,9 +336,8 @@ namespace Content.Server.NPC.Systems
if (tickMovement.Equals(0f))
{
SetDirection(mover, Vector2.Zero);
SetDirection(mover, steering, Vector2.Zero);
steering.Status = SteeringStatus.NoPath;
steering.CurrentTarget = targetCoordinates;
return;
}
@@ -307,45 +349,59 @@ namespace Content.Server.NPC.Systems
input *= maxDistance / tickMovement;
}
// TODO: This isn't going to work for space.
if (_mapManager.TryGetGrid(xform.GridUid, out var grid))
// We have the input in world terms but need to convert it back to what movercontroller is doing.
input = (-_mover.GetParentGridAngle(mover)).RotateVec(input);
SetDirection(mover, steering, input);
}
private void CheckPath(NPCSteeringComponent steering, TransformComponent xform, bool needsPath, float targetDistance)
{
if (!needsPath)
{
input = (-grid.WorldRotation).RotateVec(input);
// If the target has sufficiently moved.
var lastNode = GetCoordinates(steering.CurrentPath.Last());
if (lastNode.TryDistance(EntityManager, steering.Coordinates, out var lastDistance) &&
lastDistance > steering.RepathRange)
{
needsPath = true;
}
}
SetDirection(mover, input);
steering.CurrentTarget = targetCoordinates;
// Request the new path.
if (needsPath)
{
RequestPath(steering, xform, targetDistance);
}
}
/// <summary>
/// We may be pathfinding and moving at the same time in which case early nodes may be out of date.
/// </summary>
/// <param name="coordinates">Our coordinates we are pruning from</param>
/// <param name="nodes">Path we're pruning</param>
public void PrunePath(EntityCoordinates coordinates, Queue<TileRef> nodes)
public void PrunePath(MapCoordinates mapCoordinates, Vector2 direction, Queue<PathPoly> nodes)
{
if (nodes.Count == 0)
return;
// Right now the pathfinder gives EVERY TILE back but ideally it won't someday, it'll just give straightline ones.
// For now, we just prune up until the closest node + 1 extra.
var closest = ((Vector2) nodes.Peek().GridIndices + 0.5f - coordinates.Position).Length;
// TODO: Need to handle multi-grid and stuff.
// Prune the first node as it's irrelevant.
nodes.Dequeue();
while (nodes.TryPeek(out var node))
{
// TODO: Tile size
var nodePosition = (Vector2) node.GridIndices + 0.5f;
var length = (coordinates.Position - nodePosition).Length;
if (!node.Data.IsFreeSpace)
break;
if (length < closest)
var nodeMap = node.Coordinates.ToMap(EntityManager);
// If any nodes are 'behind us' relative to the target we'll prune them.
// This isn't perfect but should fix most cases of stutter stepping.
if (nodeMap.MapId == mapCoordinates.MapId &&
Vector2.Dot(direction, nodeMap.Position - mapCoordinates.Position) < 0f)
{
closest = length;
nodes.Dequeue();
continue;
}
nodes.Dequeue();
break;
}
}
@@ -360,44 +416,62 @@ namespace Content.Server.NPC.Systems
// Even if we're at the last node may not be able to head to target in case we get stuck on a corner or the likes.
if (steering.CurrentPath.Count >= 1 && steering.CurrentPath.TryPeek(out var nextTarget))
{
return new EntityCoordinates(nextTarget.GridUid, (Vector2) nextTarget.GridIndices + 0.5f);
return GetCoordinates(nextTarget);
}
return steering.Coordinates;
}
private EntityCoordinates GetCoordinates(PathPoly poly)
{
if (!poly.IsValid())
return EntityCoordinates.Invalid;
return new EntityCoordinates(poly.GraphUid, poly.Box.Center);
}
/// <summary>
/// Get a new job from the pathfindingsystem
/// </summary>
private void RequestPath(NPCSteeringComponent steering, TransformComponent xform, PhysicsComponent? body)
private async void RequestPath(NPCSteeringComponent steering, TransformComponent xform, float targetDistance)
{
// If we already have a pathfinding request then don't grab another.
if (steering.Pathfind != null)
return;
if (!_mapManager.TryGetGrid(xform.GridUid, out var grid))
// If we're in range then just beeline them; this can avoid stutter stepping and is an easy way to look nicer.
if (steering.Pathfind || targetDistance < steering.RepathRange)
return;
steering.PathfindToken = new CancellationTokenSource();
var startTile = grid.GetTileRef(xform.Coordinates);
var endTile = grid.GetTileRef(steering.Coordinates);
var collisionMask = 0;
if (body != null)
var flags = _pathfindingSystem.GetFlags(steering.Owner);
var result = await _pathfindingSystem.GetPath(
steering.Owner,
xform.Coordinates,
steering.Coordinates,
steering.Range,
steering.PathfindToken.Token,
flags);
if (result.Result == PathResult.NoPath)
{
collisionMask = body.CollisionMask;
steering.CurrentPath.Clear();
steering.PathfindToken = null;
steering.FailedPathCount++;
if (steering.FailedPathCount >= NPCSteeringComponent.FailedPathLimit)
{
steering.Status = SteeringStatus.NoPath;
}
return;
}
var access = _accessReader.FindAccessTags(steering.Owner);
var targetPos = steering.Coordinates.ToMap(EntityManager);
var ourPos = xform.MapPosition;
steering.Pathfind = _pathfindingSystem.RequestPath(new PathfindingArgs(
steering.Owner,
access,
collisionMask,
startTile,
endTile,
steering.Range
), steering.PathfindToken.Token);
PrunePath(ourPos, targetPos.Position - ourPos.Position, result.Path);
steering.CurrentPath = result.Path;
steering.PathfindToken = null;
}
// TODO: Move these to movercontroller