diff --git a/Content.Client/NPC/NPCSteeringComponent.cs b/Content.Client/NPC/NPCSteeringComponent.cs new file mode 100644 index 0000000000..3ae227fd77 --- /dev/null +++ b/Content.Client/NPC/NPCSteeringComponent.cs @@ -0,0 +1,14 @@ +namespace Content.Client.NPC; + +[RegisterComponent] +public sealed class NPCSteeringComponent : Component +{ + /* Not hooked up to the server component as it's used for debugging only. + */ + + public Vector2 Direction; + + public float[] DangerMap = Array.Empty(); + public float[] InterestMap = Array.Empty(); + public List DangerPoints = new(); +} diff --git a/Content.Client/NPC/NPCSteeringSystem.cs b/Content.Client/NPC/NPCSteeringSystem.cs new file mode 100644 index 0000000000..8bfdcea841 --- /dev/null +++ b/Content.Client/NPC/NPCSteeringSystem.cs @@ -0,0 +1,120 @@ +using Content.Client.Physics.Controllers; +using Content.Shared.Movement.Components; +using Content.Shared.NPC; +using Content.Shared.NPC.Events; +using Robust.Client.Graphics; +using Robust.Shared.Enums; + +namespace Content.Client.NPC; + +public sealed class NPCSteeringSystem : SharedNPCSteeringSystem +{ + [Dependency] private readonly IOverlayManager _overlay = default!; + + public bool DebugEnabled + { + get => _debugEnabled; + set + { + if (_debugEnabled == value) + return; + + _debugEnabled = value; + + if (_debugEnabled) + { + _overlay.AddOverlay(new NPCSteeringOverlay(EntityManager)); + RaiseNetworkEvent(new RequestNPCSteeringDebugEvent() + { + Enabled = true + }); + } + else + { + _overlay.RemoveOverlay(); + RaiseNetworkEvent(new RequestNPCSteeringDebugEvent() + { + Enabled = false + }); + + foreach (var comp in EntityQuery(true)) + { + RemCompDeferred(comp.Owner); + } + } + } + } + + private bool _debugEnabled; + + public override void Initialize() + { + base.Initialize(); + SubscribeNetworkEvent(OnDebugEvent); + } + + private void OnDebugEvent(NPCSteeringDebugEvent ev) + { + if (!DebugEnabled) + return; + + foreach (var data in ev.Data) + { + if (!Exists(data.EntityUid)) + continue; + + var comp = EnsureComp(data.EntityUid); + comp.Direction = data.Direction; + comp.DangerMap = data.Danger; + comp.InterestMap = data.Interest; + comp.DangerPoints = data.DangerPoints; + } + } +} + +public sealed class NPCSteeringOverlay : Overlay +{ + public override OverlaySpace Space => OverlaySpace.WorldSpace; + + private readonly IEntityManager _entManager; + + public NPCSteeringOverlay(IEntityManager entManager) + { + _entManager = entManager; + } + + protected override void Draw(in OverlayDrawArgs args) + { + foreach (var (comp, mover, xform) in _entManager.EntityQuery(true)) + { + if (xform.MapID != args.MapId) + { + continue; + } + + var (worldPos, worldRot) = xform.GetWorldPositionRotation(); + + if (!args.WorldAABB.Contains(worldPos)) + continue; + + args.WorldHandle.DrawCircle(worldPos, 1f, Color.Green, false); + var rotationOffset = _entManager.System().GetParentGridAngle(mover); + + foreach (var point in comp.DangerPoints) + { + args.WorldHandle.DrawCircle(point, 0.1f, Color.Red.WithAlpha(0.6f)); + } + + for (var i = 0; i < SharedNPCSteeringSystem.InterestDirections; i++) + { + var danger = comp.DangerMap[i]; + var interest = comp.InterestMap[i]; + var angle = Angle.FromDegrees(i * (360 / SharedNPCSteeringSystem.InterestDirections)); + args.WorldHandle.DrawLine(worldPos, worldPos + (rotationOffset + angle).RotateVec(new Vector2(interest, 0f)), Color.LimeGreen); + args.WorldHandle.DrawLine(worldPos, worldPos + (rotationOffset + angle).RotateVec(new Vector2(danger, 0f)), Color.Red); + } + + args.WorldHandle.DrawLine(worldPos, worldPos + rotationOffset.RotateVec(comp.Direction), Color.Cyan); + } + } +} diff --git a/Content.Server/NPC/Components/NPCSteeringComponent.cs b/Content.Server/NPC/Components/NPCSteeringComponent.cs index 84ae5d3b56..a86d9ecd12 100644 --- a/Content.Server/NPC/Components/NPCSteeringComponent.cs +++ b/Content.Server/NPC/Components/NPCSteeringComponent.cs @@ -1,6 +1,6 @@ using System.Threading; -using Content.Server.CPUJob.JobQueues; using Content.Server.NPC.Pathfinding; +using Content.Shared.NPC; using Robust.Shared.Map; namespace Content.Server.NPC.Components; @@ -11,6 +11,43 @@ namespace Content.Server.NPC.Components; [RegisterComponent] public sealed class NPCSteeringComponent : Component { + #region Context Steering + + /// + /// Used to override seeking behavior for context steering. + /// + [ViewVariables] + public bool CanSeek = true; + + /// + /// Radius for collision avoidance. + /// + [ViewVariables(VVAccess.ReadWrite)] + public float Radius = 0.35f; + + [ViewVariables] + public readonly float[] Interest = new float[SharedNPCSteeringSystem.InterestDirections]; + + [ViewVariables] + public readonly float[] Danger = new float[SharedNPCSteeringSystem.InterestDirections]; + + // TODO: Update radius, also danger points debug only + public readonly List DangerPoints = new(); + + #endregion + + /// + /// How many times per second we're allowed to update our steering frequency. + /// + public const byte SteerFrequency = 10; + + /// + /// Last time the NPC steered. + /// + public TimeSpan LastTimeSteer; + + public Vector2 LastSteer; + /// /// Have we currently requested a path. /// diff --git a/Content.Server/NPC/Events/NPCSteeringEvent.cs b/Content.Server/NPC/Events/NPCSteeringEvent.cs new file mode 100644 index 0000000000..b94e09cff3 --- /dev/null +++ b/Content.Server/NPC/Events/NPCSteeringEvent.cs @@ -0,0 +1,24 @@ +using Content.Server.NPC.Components; + +namespace Content.Server.NPC.Events; + +/// +/// Raised directed on an NPC when steering. +/// +[ByRefEvent] +public readonly record struct NPCSteeringEvent( + NPCSteeringComponent Steering, + float[] Interest, + float[] Danger, + float AgentRadius, + Angle OffsetRotation, + Vector2 WorldPosition) +{ + public readonly NPCSteeringComponent Steering = Steering; + public readonly float[] Interest = Interest; + public readonly float[] Danger = Danger; + + public readonly float AgentRadius = AgentRadius; + public readonly Angle OffsetRotation = OffsetRotation; + public readonly Vector2 WorldPosition = WorldPosition; +} diff --git a/Content.Server/NPC/HTN/PrimitiveTasks/Operators/NPCCombatOperator.cs b/Content.Server/NPC/HTN/PrimitiveTasks/Operators/NPCCombatOperator.cs index 1a17136831..a556c8ccb3 100644 --- a/Content.Server/NPC/HTN/PrimitiveTasks/Operators/NPCCombatOperator.cs +++ b/Content.Server/NPC/HTN/PrimitiveTasks/Operators/NPCCombatOperator.cs @@ -92,7 +92,8 @@ public abstract class NPCCombatOperator : HTNOperator { if (mobQuery.TryGetComponent(target, out var mobState) && mobState.CurrentState > DamageState.Alive || - target == existingTarget) + target == existingTarget || + target == owner) { continue; } diff --git a/Content.Server/NPC/Pathfinding/PathfindingSystem.cs b/Content.Server/NPC/Pathfinding/PathfindingSystem.cs index 504f3c443f..7b3692cfae 100644 --- a/Content.Server/NPC/Pathfinding/PathfindingSystem.cs +++ b/Content.Server/NPC/Pathfinding/PathfindingSystem.cs @@ -60,7 +60,7 @@ namespace Content.Server.NPC.Pathfinding private const int PathTickLimit = 256; private int _portalIndex; - private Dictionary _portals = new(); + private readonly Dictionary _portals = new(); public override void Initialize() { @@ -317,6 +317,21 @@ namespace Content.Server.NPC.Pathfinding return await GetPath(request); } + /// + /// Gets a path in a thread-safe way. + /// + public async Task GetPathSafe( + 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, true); + } + /// /// Asynchronously gets a path. /// @@ -428,12 +443,22 @@ namespace Content.Server.NPC.Pathfinding } private async Task GetPath( - PathRequest request) + PathRequest request, bool safe = false) { // 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); + if (safe) + { + lock (_pathRequests) + { + _pathRequests.Add(request); + } + } + else + { + _pathRequests.Add(request); + } await request.Task; diff --git a/Content.Server/NPC/Systems/FactionSystem.cs b/Content.Server/NPC/Systems/FactionSystem.cs index d177fe8f2e..241c1c4bdb 100644 --- a/Content.Server/NPC/Systems/FactionSystem.cs +++ b/Content.Server/NPC/Systems/FactionSystem.cs @@ -139,6 +139,14 @@ namespace Content.Server.NPC.Systems } } + public bool IsFriendly(EntityUid uidA, EntityUid uidB, FactionComponent? factionA = null, FactionComponent? factionB = null) + { + if (!Resolve(uidA, ref factionA, false) || !Resolve(uidB, ref factionB, false)) + return false; + + return factionA.Factions.Overlaps(factionB.Factions) || factionA.FriendlyFactions.Overlaps(factionB.Factions); + } + /// /// Makes the source faction friendly to the target faction, 1-way. /// diff --git a/Content.Server/NPC/Systems/NPCCombatSystem.Melee.cs b/Content.Server/NPC/Systems/NPCCombatSystem.Melee.cs index befa0e1e98..514130eaac 100644 --- a/Content.Server/NPC/Systems/NPCCombatSystem.Melee.cs +++ b/Content.Server/NPC/Systems/NPCCombatSystem.Melee.cs @@ -1,7 +1,7 @@ using Content.Server.CombatMode; using Content.Server.NPC.Components; -using Content.Shared.MobState; -using Content.Shared.MobState.Components; +using Content.Server.NPC.Events; +using Content.Shared.NPC; using Content.Shared.Weapons.Melee; using Robust.Shared.Map; using Robust.Shared.Physics.Components; @@ -17,6 +17,52 @@ public sealed partial class NPCCombatSystem { SubscribeLocalEvent(OnMeleeStartup); SubscribeLocalEvent(OnMeleeShutdown); + SubscribeLocalEvent(OnMeleeSteering); + } + + private void OnMeleeSteering(EntityUid uid, NPCMeleeCombatComponent component, ref NPCSteeringEvent args) + { + args.Steering.CanSeek = true; + + if (TryComp(component.Weapon, out var weapon)) + { + var cdRemaining = weapon.NextAttack - _timing.CurTime; + + // If CD remaining then backup. + if (cdRemaining < TimeSpan.FromSeconds(1f / weapon.AttackRate) * 0.5f) + return; + + if (!_physics.TryGetNearestPoints(uid, component.Target, out _, out var pointB)) + return; + + var idealDistance = weapon.Range * 1.25f; + var obstacleDirection = pointB - args.WorldPosition; + var obstacleDistance = obstacleDirection.Length; + + if (obstacleDistance > idealDistance) + { + // Don't want to get too far. + return; + } + + args.Steering.CanSeek = false; + obstacleDirection = args.OffsetRotation.RotateVec(obstacleDirection); + var norm = obstacleDirection.Normalized; + + var weight = (obstacleDistance <= args.AgentRadius + ? 1f + : (idealDistance - obstacleDistance) / idealDistance); + + for (var i = 0; i < SharedNPCSteeringSystem.InterestDirections; i++) + { + var result = -Vector2.Dot(norm, NPCSteeringSystem.Directions[i]) * weight; + + if (result < 0f) + continue; + + args.Interest[i] = MathF.Max(args.Interest[i], result); + } + } } private void OnMeleeShutdown(EntityUid uid, NPCMeleeCombatComponent component, ComponentShutdown args) @@ -107,7 +153,7 @@ public sealed partial class NPCCombatSystem // Gets unregistered on component shutdown. _steering.TryRegister(component.Owner, new EntityCoordinates(component.Target, Vector2.Zero), steering); - if (weapon.NextAttack > curTime) + if (weapon.NextAttack > curTime || !Enabled) return; if (_random.Prob(component.MissChance) && diff --git a/Content.Server/NPC/Systems/NPCCombatSystem.Ranged.cs b/Content.Server/NPC/Systems/NPCCombatSystem.Ranged.cs index 42b0011bbb..1ae9737939 100644 --- a/Content.Server/NPC/Systems/NPCCombatSystem.Ranged.cs +++ b/Content.Server/NPC/Systems/NPCCombatSystem.Ranged.cs @@ -140,7 +140,7 @@ public sealed partial class NPCCombatSystem // TODO: Check if we can face - if (!_gun.CanShoot(gun)) + if (!Enabled || !_gun.CanShoot(gun)) continue; EntityCoordinates targetCordinates; diff --git a/Content.Server/NPC/Systems/NPCCombatSystem.cs b/Content.Server/NPC/Systems/NPCCombatSystem.cs index 06d0172b39..a06fd6a287 100644 --- a/Content.Server/NPC/Systems/NPCCombatSystem.cs +++ b/Content.Server/NPC/Systems/NPCCombatSystem.cs @@ -2,6 +2,7 @@ using Content.Server.Interaction; using Content.Server.Weapons.Ranged.Systems; using Content.Shared.Weapons.Melee; using Robust.Shared.Map; +using Robust.Shared.Physics.Systems; using Robust.Shared.Random; using Robust.Shared.Timing; @@ -20,8 +21,14 @@ public sealed partial class NPCCombatSystem : EntitySystem [Dependency] private readonly SharedAudioSystem _audio = default!; [Dependency] private readonly NPCSteeringSystem _steering = default!; [Dependency] private readonly SharedMeleeWeaponSystem _melee = default!; + [Dependency] private readonly SharedPhysicsSystem _physics = default!; [Dependency] private readonly SharedTransformSystem _transform = default!; + /// + /// If disabled we'll move into range but not attack. + /// + public bool Enabled = true; + public override void Initialize() { base.Initialize(); diff --git a/Content.Server/NPC/Systems/NPCSteeringSystem.Context.cs b/Content.Server/NPC/Systems/NPCSteeringSystem.Context.cs new file mode 100644 index 0000000000..ff99b0fd0c --- /dev/null +++ b/Content.Server/NPC/Systems/NPCSteeringSystem.Context.cs @@ -0,0 +1,425 @@ +using System.Linq; +using Content.Server.NPC.Components; +using Content.Server.NPC.Pathfinding; +using Content.Shared.Interaction; +using Content.Shared.Movement.Components; +using Content.Shared.NPC; +using Robust.Shared.Map; +using Robust.Shared.Physics.Components; + +namespace Content.Server.NPC.Systems; + +public sealed partial class NPCSteeringSystem +{ + private void ApplySeek(float[] interest, Vector2 direction, float weight) + { + if (weight == 0f || direction == Vector2.Zero) + return; + + var directionAngle = (float) direction.ToAngle().Theta; + + for (var i = 0; i < InterestDirections; i++) + { + if (interest[i].Equals(-1f)) + continue; + + var angle = i * InterestRadians; + var dot = MathF.Cos(directionAngle - angle); + dot = (dot + 1) * 0.5f; + interest[i] += dot * weight; + } + } + + #region Seek + + /// + /// Attempts to head to the target destination, either via the next pathfinding node or the final target. + /// + private bool TrySeek( + EntityUid uid, + InputMoverComponent mover, + NPCSteeringComponent steering, + PhysicsComponent body, + TransformComponent xform, + Angle offsetRot, + float moveSpeed, + float[] interest, + EntityQuery bodyQuery, + float frameTime) + { + var ourCoordinates = xform.Coordinates; + var destinationCoordinates = steering.Coordinates; + + // We've arrived, nothing else matters. + if (xform.Coordinates.TryDistance(EntityManager, destinationCoordinates, out var distance) && + distance <= steering.Range) + { + steering.Status = SteeringStatus.InRange; + return true; + } + + // Grab the target position, either the next path node or our end goal.. + var targetCoordinates = GetTargetCoordinates(steering); + var needsPath = false; + + // If the next node is invalid then get new ones + if (!targetCoordinates.IsValid(EntityManager)) + { + if (steering.CurrentPath.TryPeek(out var poly) && + (poly.Data.Flags & PathfindingBreadcrumbFlag.Invalid) != 0x0) + { + steering.CurrentPath.Dequeue(); + // Try to get the next node temporarily. + targetCoordinates = GetTargetCoordinates(steering); + needsPath = true; + } + } + + // 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)) + { + // What's our tolerance for arrival. + // 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); + var ourMap = ourCoordinates.ToMap(EntityManager); + + if (targetMap.MapId != ourMap.MapId) + { + steering.Status = SteeringStatus.NoPath; + return false; + } + + var direction = targetMap.Position - ourMap.Position; + + // Are we in range + if (direction.Length <= arrivalDistance) + { + // Node needs some kind of special handling like access or smashing. + if (steering.CurrentPath.TryPeek(out var node)) + { + var status = TryHandleFlags(steering, node, bodyQuery); + + // TODO: Need to handle re-pathing in case the target moves around. + switch (status) + { + case SteeringObstacleStatus.Completed: + break; + case SteeringObstacleStatus.Failed: + // TODO: Blacklist the poly for next query + steering.Status = SteeringStatus.NoPath; + return false; + case SteeringObstacleStatus.Continuing: + CheckPath(steering, xform, needsPath, distance); + return true; + 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 false; + } + + // Gonna resume now business as usual + direction = targetMap.Position - ourMap.Position; + } + else + { + // This probably shouldn't happen as we check above but eh. + steering.Status = SteeringStatus.NoPath; + return false; + } + } + } + + // Do we have no more nodes to follow OR has the target moved sufficiently? If so then re-path. + if (!needsPath) + { + needsPath = steering.CurrentPath.Count == 0 || (steering.CurrentPath.Peek().Data.Flags & PathfindingBreadcrumbFlag.Invalid) != 0x0; + } + + // TODO: Probably need partial planning support i.e. patch from the last node to where the target moved to. + CheckPath(steering, xform, needsPath, distance); + + // If we don't have a path yet then do nothing; this is to avoid stutter-stepping if it turns out there's no path + // available but we assume there was. + if (steering is { Pathfind: true, CurrentPath.Count: 0 }) + return true; + + if (moveSpeed == 0f || direction == Vector2.Zero) + { + steering.Status = SteeringStatus.NoPath; + return false; + } + + var input = direction.Normalized; + var tickMovement = moveSpeed * frameTime; + + // We have the input in world terms but need to convert it back to what movercontroller is doing. + input = offsetRot.RotateVec(input); + var norm = input.Normalized; + var weight = MapValue(direction.Length, tickMovement * 0.5f, tickMovement * 0.75f); + + ApplySeek(interest, norm, weight); + + // Prefer our current direction + if (weight > 0f && body.LinearVelocity.LengthSquared > 0f) + { + const float SameDirectionWeight = 0.1f; + norm = body.LinearVelocity.Normalized; + + ApplySeek(interest, norm, SameDirectionWeight); + } + + return true; + } + + + private void CheckPath(NPCSteeringComponent steering, TransformComponent xform, bool needsPath, float targetDistance) + { + if (!_pathfinding) + { + steering.CurrentPath.Clear(); + steering.PathfindToken?.Cancel(); + steering.PathfindToken = null; + return; + } + + if (!needsPath) + { + // 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; + } + } + + // Request the new path. + if (needsPath) + { + RequestPath(steering, xform, targetDistance); + } + } + + /// + /// We may be pathfinding and moving at the same time in which case early nodes may be out of date. + /// + public void PrunePath(MapCoordinates mapCoordinates, Vector2 direction, Queue nodes) + { + if (nodes.Count == 0) + return; + + // Prune the first node as it's irrelevant. + nodes.Dequeue(); + + while (nodes.TryPeek(out var node)) + { + if (!node.Data.IsFreeSpace) + break; + + 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) + { + nodes.Dequeue(); + continue; + } + + break; + } + } + + /// + /// Get the coordinates we should be heading towards. + /// + private EntityCoordinates GetTargetCoordinates(NPCSteeringComponent steering) + { + // Depending on what's going on we may return the target or a pathfind node. + + // 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 (_pathfinding && steering.CurrentPath.Count >= 1 && steering.CurrentPath.TryPeek(out var nextTarget)) + { + return GetCoordinates(nextTarget); + } + + return steering.Coordinates; + } + + /// + /// Gets the fraction this value is between min and max + /// + /// + private float MapValue(float value, float minValue, float maxValue) + { + if (maxValue > minValue) + { + var mapped = (value - minValue) / (maxValue - minValue); + return Math.Clamp(mapped, 0f, 1f); + } + + return value >= minValue ? 1f : 0f; + } + + #endregion + + #region Static Avoidance + + /// + /// Tries to avoid static blockers such as walls. + /// + private void CollisionAvoidance( + EntityUid uid, + Angle offsetRot, + Vector2 worldPos, + float agentRadius, + float moveSpeed, + PhysicsComponent body, + TransformComponent xform, + float[] danger, + List dangerPoints, + EntityQuery bodyQuery, + EntityQuery xformQuery) + { + var detectionRadius = agentRadius + moveSpeed; + + foreach (var ent in _lookup.GetEntitiesInRange(uid, detectionRadius, LookupFlags.Static)) + { + // TODO: If we can access the door or smth. + if (ent == uid || + !bodyQuery.TryGetComponent(ent, out var otherBody) || + !otherBody.Hard || + !otherBody.CanCollide || + ((body.CollisionMask & otherBody.CollisionLayer) == 0x0 && + (body.CollisionLayer & otherBody.CollisionMask) == 0x0)) + { + continue; + } + + if (!_physics.TryGetNearestPoints(uid, ent, out var pointA, out var pointB, xform, xformQuery.GetComponent(ent))) + continue; + + var obstacleDirection = (pointB - worldPos); + var obstableDistance = obstacleDirection.Length; + + if (obstableDistance > detectionRadius) + continue; + + dangerPoints.Add(pointB); + obstacleDirection = offsetRot.RotateVec(obstacleDirection); + var norm = obstacleDirection.Normalized; + var weight = obstableDistance <= agentRadius ? 1f : (detectionRadius - obstableDistance) / detectionRadius; + + for (var i = 0; i < InterestDirections; i++) + { + var dot = Vector2.Dot(norm, Directions[i]); + danger[i] = MathF.Max(dot * weight, danger[i]); + } + } + + } + + #endregion + + #region Dynamic Avoidance + + /// + /// Tries to avoid mobs of the same faction. + /// + private void Separation( + EntityUid uid, + Angle offsetRot, + Vector2 worldPos, + float agentRadius, + PhysicsComponent body, + TransformComponent xform, + float[] interest, + float[] danger, + EntityQuery bodyQuery, + EntityQuery xformQuery) + { + var detectionRadius = agentRadius + 0.1f; + var ourVelocity = body.LinearVelocity; + var factionQuery = GetEntityQuery(); + factionQuery.TryGetComponent(uid, out var ourFaction); + + foreach (var ent in _lookup.GetEntitiesInRange(uid, detectionRadius, LookupFlags.Dynamic)) + { + // TODO: If we can access the door or smth. + if (ent == uid || + !bodyQuery.TryGetComponent(ent, out var otherBody) || + !otherBody.Hard || + !otherBody.CanCollide || + (body.CollisionMask & otherBody.CollisionLayer) == 0x0 && + (body.CollisionLayer & otherBody.CollisionMask) == 0x0 || + !factionQuery.TryGetComponent(ent, out var otherFaction) || + !_faction.IsFriendly(uid, ent, ourFaction, otherFaction) || + Vector2.Dot(otherBody.LinearVelocity, ourVelocity) < 0f) + { + continue; + } + + var xformB = xformQuery.GetComponent(ent); + + if (!_physics.TryGetNearestPoints(uid, ent, out var pointA, out var pointB, xform, xformB)) + { + continue; + } + + var obstacleDirection = (pointB - worldPos); + var obstableDistance = obstacleDirection.Length; + + if (obstableDistance > detectionRadius) + continue; + + obstacleDirection = offsetRot.RotateVec(obstacleDirection); + var norm = obstacleDirection.Normalized; + var weight = obstableDistance <= agentRadius ? 1f : (detectionRadius - obstableDistance) / detectionRadius; + weight *= 1.5f; + + for (var i = 0; i < InterestDirections; i++) + { + var dot = Vector2.Dot(norm, Directions[i]); + danger[i] = MathF.Max(dot * weight, danger[i]); + } + } + } + + #endregion + + // TODO: Alignment + + // TODO: Cohesion +} diff --git a/Content.Server/NPC/Systems/NPCSteeringSystem.cs b/Content.Server/NPC/Systems/NPCSteeringSystem.cs index a4e13510af..37ca616f55 100644 --- a/Content.Server/NPC/Systems/NPCSteeringSystem.cs +++ b/Content.Server/NPC/Systems/NPCSteeringSystem.cs @@ -1,31 +1,54 @@ using System.Linq; using System.Threading; +using System.Threading.Tasks; +using Content.Server.Administration.Managers; using Content.Server.Doors.Systems; using Content.Server.NPC.Components; +using Content.Server.NPC.Events; using Content.Server.NPC.Pathfinding; 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.NPC.Events; using Content.Shared.Weapons.Melee; +using Robust.Server.Player; using Robust.Shared.Configuration; using Robust.Shared.Map; using Robust.Shared.Physics.Components; using Robust.Shared.Physics.Systems; +using Robust.Shared.Player; +using Robust.Shared.Players; using Robust.Shared.Random; +using Robust.Shared.Threading; using Robust.Shared.Timing; +using Robust.Shared.Utility; namespace Content.Server.NPC.Systems { - public sealed partial class NPCSteeringSystem : EntitySystem + public sealed partial class NPCSteeringSystem : SharedNPCSteeringSystem { - // http://www.red3d.com/cwr/papers/1999/gdc99steer.html for a steering overview + /* + * We use context steering to determine which way to move. + * This involves creating an array of possible directions and assigning a value for the desireability of each direction. + * + * There's multiple ways to implement this, e.g. you can average all directions, or you can choose the highest direction + * , or you can remove the danger map entirely and only having an interest map (AKA game endeavour). + * See http://www.gameaipro.com/GameAIPro2/GameAIPro2_Chapter18_Context_Steering_Behavior-Driven_Steering_at_the_Macro_Scale.pdf + * (though in their case it was for an F1 game so used context steering across the width of the road). + */ + + [Dependency] private readonly IAdminManager _admin = default!; [Dependency] private readonly IConfigurationManager _configManager = default!; [Dependency] private readonly IGameTiming _timing = default!; [Dependency] private readonly IMapManager _mapManager = default!; + [Dependency] private readonly IParallelManager _parallel = default!; [Dependency] private readonly IRobustRandom _random = default!; [Dependency] private readonly DoorSystem _doors = default!; + [Dependency] private readonly EntityLookupSystem _lookup = default!; + [Dependency] private readonly FactionSystem _faction = default!; + // [Dependency] private readonly MetaDataSystem _metadata = default!; [Dependency] private readonly PathfindingSystem _pathfindingSystem = default!; [Dependency] private readonly SharedInteractionSystem _interaction = default!; [Dependency] private readonly SharedMeleeWeaponSystem _melee = default!; @@ -37,19 +60,28 @@ namespace Content.Server.NPC.Systems private bool _enabled; + private bool _pathfinding = true; + + public static readonly Vector2[] Directions = new Vector2[InterestDirections]; + + private readonly HashSet _subscribedSessions = new(); + public override void Initialize() { base.Initialize(); + + for (var i = 0; i < InterestDirections; i++) + { + Directions[i] = new Angle(InterestRadians * i).ToVec(); + } + UpdatesBefore.Add(typeof(SharedPhysicsSystem)); InitializeAvoidance(); - _configManager.OnValueChanged(CCVars.NPCEnabled, SetNPCEnabled, true); + _configManager.OnValueChanged(CCVars.NPCEnabled, SetNPCEnabled); + _configManager.OnValueChanged(CCVars.NPCPathfinding, SetNPCPathfinding); SubscribeLocalEvent(OnSteeringShutdown); - } - - private void OnSteeringShutdown(EntityUid uid, NPCSteeringComponent component, ComponentShutdown args) - { - component.PathfindToken?.Cancel(); + SubscribeNetworkEvent(OnDebugRequest); } private void SetNPCEnabled(bool obj) @@ -65,6 +97,20 @@ namespace Content.Server.NPC.Systems _enabled = obj; } + private void SetNPCPathfinding(bool value) + { + _pathfinding = value; + + if (!_pathfinding) + { + foreach (var comp in EntityQuery(true)) + { + comp.PathfindToken?.Cancel(); + comp.PathfindToken = null; + } + } + } + public override void Shutdown() { base.Shutdown(); @@ -72,6 +118,22 @@ namespace Content.Server.NPC.Systems _configManager.UnsubValueChanged(CCVars.NPCEnabled, SetNPCEnabled); } + private void OnDebugRequest(RequestNPCSteeringDebugEvent msg, EntitySessionEventArgs args) + { + if (!_admin.IsAdmin((IPlayerSession) args.SenderSession)) + return; + + if (msg.Enabled) + _subscribedSessions.Add(args.SenderSession); + else + _subscribedSessions.Remove(args.SenderSession); + } + + private void OnSteeringShutdown(EntityUid uid, NPCSteeringComponent component, ComponentShutdown args) + { + component.PathfindToken?.Cancel(); + } + /// /// Adds the AI to the steering system to move towards a specific target /// @@ -137,15 +199,41 @@ namespace Content.Server.NPC.Systems // Not every mob has the modifier component so do it as a separate query. var bodyQuery = GetEntityQuery(); var modifierQuery = GetEntityQuery(); + var xformQuery = GetEntityQuery(); var npcs = EntityQuery() .ToArray(); - - // TODO: Do this in parallel. - // Main obstacle is requesting a new path needs to be done synchronously - foreach (var (steering, _, mover, xform) in npcs) + var options = new ParallelOptions() { - Steer(steering, mover, xform, modifierQuery, bodyQuery, frameTime); + MaxDegreeOfParallelism = _parallel.ParallelProcessCount, + }; + + Parallel.For(0, npcs.Length, options, i => + { + var (steering, _, mover, xform) = npcs[i]; + + Steer(steering, mover, xform, modifierQuery, bodyQuery, xformQuery, frameTime); + steering.LastSteer = mover.CurTickSprintMovement; + }); + + if (_subscribedSessions.Count > 0) + { + var data = new List(npcs.Length); + + foreach (var (steering, _, mover, _) in npcs) + { + data.Add(new NPCSteeringDebugData( + mover.Owner, + mover.CurTickSprintMovement, + steering.Interest, + steering.Danger, + steering.DangerPoints)); + } + + var filter = Filter.Empty(); + filter.AddPlayers(_subscribedSessions); + + RaiseNetworkEvent(new NPCSteeringDebugEvent(data), filter); } } @@ -170,6 +258,7 @@ namespace Content.Server.NPC.Systems TransformComponent xform, EntityQuery modifierQuery, EntityQuery bodyQuery, + EntityQuery xformQuery, float frameTime) { if (Deleted(steering.Coordinates.EntityId)) @@ -179,242 +268,93 @@ namespace Content.Server.NPC.Systems return; } - var ourCoordinates = xform.Coordinates; - var destinationCoordinates = steering.Coordinates; - - // We've arrived, nothing else matters. - if (xform.Coordinates.TryDistance(EntityManager, destinationCoordinates, out var distance) && - distance <= steering.Range) + // No path set from pathfinding or the likes. + if (steering.Status == SteeringStatus.NoPath) { SetDirection(mover, steering, Vector2.Zero); - steering.Status = SteeringStatus.InRange; return; } // Can't move at all, just noop input. if (!mover.CanMove) - { - SetDirection(mover, steering, Vector2.Zero); - steering.Status = SteeringStatus.Moving; - return; - } - - // 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)) - { - if (steering.CurrentPath.TryPeek(out var poly) && - (poly.Data.Flags & PathfindingBreadcrumbFlag.Invalid) != 0x0) - { - steering.CurrentPath.Dequeue(); - // Try to get the next node temporarily. - targetCoordinates = GetTargetCoordinates(steering); - needsPath = true; - } - } - - // 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)) - { - // What's our tolerance for arrival. - // 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); - var ourMap = ourCoordinates.ToMap(EntityManager); - - if (targetMap.MapId != ourMap.MapId) { SetDirection(mover, steering, Vector2.Zero); steering.Status = SteeringStatus.NoPath; return; } - var direction = targetMap.Position - ourMap.Position; + // TODO: Pause time + // Need it on the paused event which needs an engine PR. + var nextSteer = steering.LastTimeSteer + TimeSpan.FromSeconds(1f / NPCSteeringComponent.SteerFrequency); - // Are we in range - if (direction.Length <= arrivalDistance) + if (nextSteer > _timing.CurTime) { - // Node needs some kind of special handling like access or smashing. - if (steering.CurrentPath.TryPeek(out var node)) - { - var status = TryHandleFlags(steering, node, bodyQuery); - - // TODO: Need to handle re-pathing in case the target moves around. - switch (status) - { - 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; - } - } - } - - // Do we have no more nodes to follow OR has the target moved sufficiently? If so then re-path. - if (!needsPath) - { - needsPath = steering.CurrentPath.Count == 0 || (steering.CurrentPath.Peek().Data.Flags & PathfindingBreadcrumbFlag.Invalid) != 0x0; - } - - // TODO: Probably need partial planning support i.e. patch from the last node to where the target moved to. - CheckPath(steering, xform, needsPath, distance); - - if (steering.Pathfind && steering.CurrentPath.Count == 0) - { - SetDirection(mover, steering, Vector2.Zero, false); + SetDirection(mover, steering, steering.LastSteer, false); return; } - modifierQuery.TryGetComponent(steering.Owner, out var modifier); + steering.LastTimeSteer = _timing.CurTime; + var uid = mover.Owner; + var interest = steering.Interest; + var danger = steering.Danger; + var agentRadius = steering.Radius; + var worldPos = xform.WorldPosition; + + // Use rotation relative to parent to rotate our context vectors by. + var offsetRot = -_mover.GetParentGridAngle(mover); + modifierQuery.TryGetComponent(uid, out var modifier); var moveSpeed = GetSprintSpeed(steering.Owner, modifier); + var tickMove = moveSpeed * frameTime; + var body = bodyQuery.GetComponent(uid); + var dangerPoints = steering.DangerPoints; + dangerPoints.Clear(); - var input = direction.Normalized; + for (var i = 0; i < InterestDirections; i++) + { + steering.Interest[i] = 0f; + steering.Danger[i] = 0f; + } - // If we're going to overshoot then... don't. - // TODO: For tile / movement we don't need to get bang on, just need to make sure we don't overshoot the far end. - var tickMovement = moveSpeed * frameTime; + var ev = new NPCSteeringEvent(steering, interest, danger, agentRadius, offsetRot, worldPos); + RaiseLocalEvent(uid, ref ev); - if (tickMovement.Equals(0f)) + if (steering.CanSeek && !TrySeek(uid, mover, steering, body, xform, offsetRot, moveSpeed, interest, bodyQuery, frameTime)) { SetDirection(mover, steering, Vector2.Zero); - steering.Status = SteeringStatus.NoPath; return; } + DebugTools.Assert(!float.IsNaN(interest[0])); - // We may overshoot slightly but still be in the arrival distance which is okay. - var maxDistance = direction.Length + arrivalDistance; + // Avoid static objects like walls + CollisionAvoidance(uid, offsetRot, worldPos, agentRadius, tickMove, body, xform, danger, dangerPoints, bodyQuery, xformQuery); + DebugTools.Assert(!float.IsNaN(danger[0])); - if (tickMovement > maxDistance) + Separation(uid, offsetRot, worldPos, agentRadius, body, xform, interest, danger, bodyQuery, xformQuery); + + // Remove the danger map from the interest map. + var desiredDirection = -1; + var desiredValue = 0f; + + for (var i = 0; i < InterestDirections; i++) { - input *= maxDistance / tickMovement; - } + var adjustedValue = Math.Clamp(interest[i] - danger[i], 0f, 1f); - // 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) - { - // If the target has sufficiently moved. - var lastNode = GetCoordinates(steering.CurrentPath.Last()); - - if (lastNode.TryDistance(EntityManager, steering.Coordinates, out var lastDistance) && - lastDistance > steering.RepathRange) + if (adjustedValue > desiredValue) { - needsPath = true; + desiredDirection = i; + desiredValue = adjustedValue; } } - // Request the new path. - if (needsPath) + var resultDirection = Vector2.Zero; + + if (desiredDirection != -1) { - RequestPath(steering, xform, targetDistance); - } - } - - /// - /// We may be pathfinding and moving at the same time in which case early nodes may be out of date. - /// - public void PrunePath(MapCoordinates mapCoordinates, Vector2 direction, Queue nodes) - { - if (nodes.Count == 0) - return; - - // Prune the first node as it's irrelevant. - nodes.Dequeue(); - - while (nodes.TryPeek(out var node)) - { - if (!node.Data.IsFreeSpace) - break; - - 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) - { - nodes.Dequeue(); - continue; - } - - break; - } - } - - /// - /// Get the coordinates we should be heading towards. - /// - private EntityCoordinates GetTargetCoordinates(NPCSteeringComponent steering) - { - // Depending on what's going on we may return the target or a pathfind node. - - // 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 GetCoordinates(nextTarget); + resultDirection = new Angle(desiredDirection * InterestRadians).ToVec(); } - return steering.Coordinates; + DebugTools.Assert(!float.IsNaN(resultDirection.X)); + SetDirection(mover, steering, resultDirection, false); } private EntityCoordinates GetCoordinates(PathPoly poly) @@ -435,11 +375,21 @@ namespace Content.Server.NPC.Systems if (steering.Pathfind || targetDistance < steering.RepathRange) return; + // Short-circuit with no path. + var targetPoly = _pathfindingSystem.GetPoly(steering.Coordinates); + + if (targetPoly != null && steering.Coordinates.Position.Equals(Vector2.Zero) && _interaction.InRangeUnobstructed(steering.Owner, steering.Coordinates.EntityId)) + { + steering.CurrentPath.Clear(); + steering.CurrentPath.Enqueue(targetPoly); + return; + } + steering.PathfindToken = new CancellationTokenSource(); var flags = _pathfindingSystem.GetFlags(steering.Owner); - var result = await _pathfindingSystem.GetPath( + var result = await _pathfindingSystem.GetPathSafe( steering.Owner, xform.Coordinates, steering.Coordinates, diff --git a/Content.Shared/CCVar/CCVars.cs b/Content.Shared/CCVar/CCVars.cs index 430abca3c5..461246482d 100644 --- a/Content.Shared/CCVar/CCVars.cs +++ b/Content.Shared/CCVar/CCVars.cs @@ -537,6 +537,11 @@ namespace Content.Shared.CCVar public static readonly CVarDef NPCEnabled = CVarDef.Create("npc.enabled", true); + /// + /// Should NPCs pathfind when steering. For debug purposes. + /// + public static readonly CVarDef NPCPathfinding = CVarDef.Create("npc.pathfinding", true); + public static readonly CVarDef NPCCollisionAvoidance = CVarDef.Create("npc.collision_avoidance", true); /* diff --git a/Content.Shared/NPC/Events/NPCSteeringDebugEvent.cs b/Content.Shared/NPC/Events/NPCSteeringDebugEvent.cs new file mode 100644 index 0000000000..35f7e6493d --- /dev/null +++ b/Content.Shared/NPC/Events/NPCSteeringDebugEvent.cs @@ -0,0 +1,32 @@ +using Robust.Shared.Serialization; + +namespace Content.Shared.NPC.Events; + +/// +/// Client debug data for NPC steering +/// +[Serializable, NetSerializable] +public sealed class NPCSteeringDebugEvent : EntityEventArgs +{ + public List Data; + + public NPCSteeringDebugEvent(List data) + { + Data = data; + } +} + +[Serializable, NetSerializable] +public readonly record struct NPCSteeringDebugData( + EntityUid EntityUid, + Vector2 Direction, + float[] Interest, + float[] Danger, + List DangerPoints) +{ + public readonly EntityUid EntityUid = EntityUid; + public readonly Vector2 Direction = Direction; + public readonly float[] Interest = Interest; + public readonly float[] Danger = Danger; + public readonly List DangerPoints = DangerPoints; +} diff --git a/Content.Shared/NPC/Events/RequestNPCSteeringDebugEvent.cs b/Content.Shared/NPC/Events/RequestNPCSteeringDebugEvent.cs new file mode 100644 index 0000000000..9c9bcdd98c --- /dev/null +++ b/Content.Shared/NPC/Events/RequestNPCSteeringDebugEvent.cs @@ -0,0 +1,12 @@ +using Robust.Shared.Serialization; + +namespace Content.Shared.NPC.Events; + +/// +/// Raised from client to server to request NPC steering debug info. +/// +[Serializable, NetSerializable] +public sealed class RequestNPCSteeringDebugEvent : EntityEventArgs +{ + public bool Enabled; +} diff --git a/Content.Shared/NPC/PathfindingDebugMode.cs b/Content.Shared/NPC/PathfindingDebugMode.cs index 8e02485264..f1135b1082 100644 --- a/Content.Shared/NPC/PathfindingDebugMode.cs +++ b/Content.Shared/NPC/PathfindingDebugMode.cs @@ -23,24 +23,26 @@ public enum PathfindingDebugMode : ushort /// /// Shows all of the pathfinding polys. /// - Polys = 1 << 6, + Polys = 1 << 3, /// /// Shows the edges between pathfinding polys. /// - PolyNeighbors = 1 << 7, + PolyNeighbors = 1 << 4, /// /// Shows the nearest poly to the mouse cursor. /// - Poly = 1 << 8, + Poly = 1 << 5, /// /// Gets a path from the current attached entity to the mouse cursor. /// - Path = 1 << 9, + // Path = 1 << 6, - Routes = 1 << 10, + Routes = 1 << 6, - RouteCosts = 1 << 11, + RouteCosts = 1 << 7, + + Steering = 1 << 8, } diff --git a/Content.Shared/NPC/SharedNPCSteeringSystem.cs b/Content.Shared/NPC/SharedNPCSteeringSystem.cs new file mode 100644 index 0000000000..20554ecd60 --- /dev/null +++ b/Content.Shared/NPC/SharedNPCSteeringSystem.cs @@ -0,0 +1,16 @@ +namespace Content.Shared.NPC; + +public abstract class SharedNPCSteeringSystem : EntitySystem +{ + public const byte InterestDirections = 12; + + /// + /// How many radians between each interest direction. + /// + public const float InterestRadians = MathF.Tau / InterestDirections; + + /// + /// How many degrees between each interest direction. + /// + public const float InterestDegrees = 360f / InterestDirections; +}