diff --git a/Content.Server/NPC/Components/NPCSteeringComponent.cs b/Content.Server/NPC/Components/NPCSteeringComponent.cs index fef446cb606bab..9772393b2df934 100644 --- a/Content.Server/NPC/Components/NPCSteeringComponent.cs +++ b/Content.Server/NPC/Components/NPCSteeringComponent.cs @@ -45,6 +45,9 @@ public sealed class NPCSteeringComponent : Component [DataField("nextSteer", customTypeSerializer:typeof(TimeOffsetSerializer))] public TimeSpan NextSteer = TimeSpan.Zero; + [DataField("lastSteerIndex")] + public int LastSteerIndex = -1; + [DataField("lastSteerDirection")] public Vector2 LastSteerDirection = Vector2.Zero; diff --git a/Content.Server/NPC/HTN/PrimitiveTasks/Operators/MoveToOperator.cs b/Content.Server/NPC/HTN/PrimitiveTasks/Operators/MoveToOperator.cs index dd7ab059f124f2..5d22862c2a4727 100644 --- a/Content.Server/NPC/HTN/PrimitiveTasks/Operators/MoveToOperator.cs +++ b/Content.Server/NPC/HTN/PrimitiveTasks/Operators/MoveToOperator.cs @@ -146,7 +146,7 @@ public override void Startup(NPCBlackboard blackboard) _steering.PrunePath(uid, mapCoords, targetCoordinates.ToMapPos(_entManager, _transform) - mapCoords.Position, result.Path); } - comp.CurrentPath = result.Path; + comp.CurrentPath = new Queue(result.Path); } } diff --git a/Content.Server/NPC/Pathfinding/PathRequest.cs b/Content.Server/NPC/Pathfinding/PathRequest.cs index 43e4773e3a857b..04d24806783e7d 100644 --- a/Content.Server/NPC/Pathfinding/PathRequest.cs +++ b/Content.Server/NPC/Pathfinding/PathRequest.cs @@ -17,7 +17,7 @@ public abstract class PathRequest public Task Task => Tcs.Task; public readonly TaskCompletionSource Tcs; - public Queue Polys = new(); + public List Polys = new(); public bool Started = false; @@ -103,9 +103,9 @@ public BFSPathRequest( public sealed class PathResultEvent { public PathResult Result; - public readonly Queue Path; + public readonly List Path; - public PathResultEvent(PathResult result, Queue path) + public PathResultEvent(PathResult result, List path) { Result = result; Path = path; diff --git a/Content.Server/NPC/Pathfinding/PathfindingSystem.Common.cs b/Content.Server/NPC/Pathfinding/PathfindingSystem.Common.cs index 13523b81ccf94b..82a9467ce2b3d0 100644 --- a/Content.Server/NPC/Pathfinding/PathfindingSystem.Common.cs +++ b/Content.Server/NPC/Pathfinding/PathfindingSystem.Common.cs @@ -24,7 +24,7 @@ public int Compare((float, PathPoly) x, (float, PathPoly) y) private static readonly PathComparer PathPolyComparer = new(); - private Queue ReconstructPath(Dictionary path, PathPoly currentNodeRef) + private List ReconstructPath(Dictionary path, PathPoly currentNodeRef) { var running = new List { currentNodeRef }; while (path.ContainsKey(currentNodeRef)) @@ -35,10 +35,8 @@ private Queue ReconstructPath(Dictionary path, Pat running.Add(currentNodeRef); } - running = Simplify(running); running.Reverse(); - var result = new Queue(running); - return result; + return running; } private float GetTileCost(PathRequest request, PathPoly start, PathPoly end) diff --git a/Content.Server/NPC/Pathfinding/PathfindingSystem.cs b/Content.Server/NPC/Pathfinding/PathfindingSystem.cs index f8d7c725f89226..4fcac8f7c793f5 100644 --- a/Content.Server/NPC/Pathfinding/PathfindingSystem.cs +++ b/Content.Server/NPC/Pathfinding/PathfindingSystem.cs @@ -238,7 +238,7 @@ public async Task GetRandomPath( PathFlags flags = PathFlags.None) { if (!TryComp(entity, out var start)) - return new PathResultEvent(PathResult.NoPath, new Queue()); + return new PathResultEvent(PathResult.NoPath, new List()); var layer = 0; var mask = 0; @@ -252,7 +252,7 @@ public async Task GetRandomPath( var path = await GetPath(request); if (path.Result != PathResult.Path) - return new PathResultEvent(PathResult.NoPath, new Queue()); + return new PathResultEvent(PathResult.NoPath, new List()); return new PathResultEvent(PathResult.Path, path.Path); } @@ -280,14 +280,13 @@ public async Task GetRandomPath( return 0f; var distance = 0f; - var node = path.Path.Dequeue(); - var lastNode = node; + var lastNode = path.Path[0]; - do + for (var i = 1; i < path.Path.Count; i++) { + var node = path.Path[i]; distance += GetTileCost(request, lastNode, node); - lastNode = node; - } while (path.Path.TryDequeue(out node)); + } return distance; } @@ -301,7 +300,7 @@ public async Task GetPath( { if (!TryComp(entity, out var xform) || !TryComp(target, out var targetXform)) - return new PathResultEvent(PathResult.NoPath, new Queue()); + return new PathResultEvent(PathResult.NoPath, new List()); var request = GetRequest(entity, xform.Coordinates, targetXform.Coordinates, range, cancelToken, flags); return await GetPath(request); @@ -471,7 +470,7 @@ private async Task GetPath( if (!request.Task.IsCompletedSuccessfully) { - return new PathResultEvent(PathResult.NoPath, new Queue()); + return new PathResultEvent(PathResult.NoPath, new List()); } // Same context as do_after and not synchronously blocking soooo diff --git a/Content.Server/NPC/Systems/NPCSteeringSystem.Context.cs b/Content.Server/NPC/Systems/NPCSteeringSystem.Context.cs index 758a6ce481d41d..4b221fd651fb82 100644 --- a/Content.Server/NPC/Systems/NPCSteeringSystem.Context.cs +++ b/Content.Server/NPC/Systems/NPCSteeringSystem.Context.cs @@ -292,13 +292,38 @@ private void CheckPath(EntityUid uid, NPCSteeringComponent steering, TransformCo /// /// We may be pathfinding and moving at the same time in which case early nodes may be out of date. /// - public void PrunePath(EntityUid uid, MapCoordinates mapCoordinates, Vector2 direction, Queue nodes) + public void PrunePath(EntityUid uid, MapCoordinates mapCoordinates, Vector2 direction, List nodes) { if (nodes.Count <= 1) return; - // Prune the first node as it's irrelevant (normally it is our node so we don't want to backtrack). - nodes.Dequeue(); + // Work out if we're inside any nodes, then use the next one as the starting point. + var index = 0; + var found = false; + + for (var i = 0; i < nodes.Count; i++) + { + var node = nodes[i]; + var matrix = _transform.GetWorldMatrix(node.GraphUid); + + // Always want to prune the poly itself so we point to the next poly and don't backtrack. + if (matrix.TransformBox(node.Box).Contains(mapCoordinates.Position)) + { + index = i + 1; + found = true; + break; + } + } + + if (found) + { + nodes.RemoveRange(0, index); + _pathfindingSystem.Simplify(nodes); + return; + } + + // Otherwise, take the node after the nearest node. + // TODO: Really need layer support CollisionGroup mask = 0; @@ -310,11 +335,11 @@ public void PrunePath(EntityUid uid, MapCoordinates mapCoordinates, Vector2 dire // If we have to backtrack (for example, we're behind a table and the target is on the other side) // Then don't consider pruning. var goal = nodes.Last().Coordinates.ToMap(EntityManager, _transform); - var canPrune = - _interaction.InRangeUnobstructed(mapCoordinates, goal, (goal.Position - mapCoordinates.Position).Length() + 0.1f, mask); - while (nodes.TryPeek(out var node)) + for (var i = 0; i < nodes.Count; i++) { + var node = nodes[i]; + if (!node.Data.IsFreeSpace) break; @@ -322,16 +347,17 @@ public void PrunePath(EntityUid uid, MapCoordinates mapCoordinates, Vector2 dire // 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 (canPrune && - nodeMap.MapId == mapCoordinates.MapId && + if (nodeMap.MapId == mapCoordinates.MapId && Vector2.Dot(direction, nodeMap.Position - mapCoordinates.Position) < 0f) { - nodes.Dequeue(); + nodes.RemoveAt(i); continue; } break; } + + _pathfindingSystem.Simplify(nodes); } /// @@ -382,7 +408,7 @@ private void CollisionAvoidance( TransformComponent xform, float[] danger) { - var objectRadius = 0.10f; + var objectRadius = 0.15f; var detectionRadius = MathF.Max(0.35f, agentRadius + objectRadius); foreach (var ent in _lookup.GetEntitiesInRange(uid, detectionRadius, LookupFlags.Static)) @@ -430,7 +456,7 @@ private void CollisionAvoidance( for (var i = 0; i < InterestDirections; i++) { var dot = Vector2.Dot(norm, Directions[i]); - danger[i] = MathF.Max(dot * weight * 0.9f, danger[i]); + danger[i] = MathF.Max(dot * weight, danger[i]); } } diff --git a/Content.Server/NPC/Systems/NPCSteeringSystem.cs b/Content.Server/NPC/Systems/NPCSteeringSystem.cs index 7b1d21164aabd0..9eea5907058b53 100644 --- a/Content.Server/NPC/Systems/NPCSteeringSystem.cs +++ b/Content.Server/NPC/Systems/NPCSteeringSystem.cs @@ -368,6 +368,12 @@ private void Steer( Separation(uid, offsetRot, worldPos, agentRadius, layer, mask, body, xform, danger); + // Prioritise whichever direction we went last tick if it's a tie-breaker. + if (steering.LastSteerIndex != -1) + { + interest[steering.LastSteerIndex] *= 1.1f; + } + // Remove the danger map from the interest map. var desiredDirection = -1; var desiredValue = 0f; @@ -392,6 +398,7 @@ private void Steer( steering.NextSteer = curTime + TimeSpan.FromSeconds(1f / NPCSteeringComponent.SteeringFrequency); steering.LastSteerDirection = resultDirection; + steering.LastSteerIndex = desiredDirection; DebugTools.Assert(!float.IsNaN(resultDirection.X)); SetDirection(mover, steering, resultDirection, false); } @@ -425,14 +432,6 @@ private async void RequestPath(EntityUid uid, NPCSteeringComponent steering, Tra _interaction.InRangeUnobstructed(uid, steering.Coordinates.EntityId, range: 30f, (CollisionGroup) physics.CollisionMask)) { steering.CurrentPath.Clear(); - // Enqueue our poly as it will be pruned later. - var ourPoly = _pathfindingSystem.GetPoly(xform.Coordinates); - - if (ourPoly != null) - { - steering.CurrentPath.Enqueue(ourPoly); - } - steering.CurrentPath.Enqueue(targetPoly); return; } @@ -468,7 +467,7 @@ private async void RequestPath(EntityUid uid, NPCSteeringComponent steering, Tra var ourPos = xform.MapPosition; PrunePath(uid, ourPos, targetPos.Position - ourPos.Position, result.Path); - steering.CurrentPath = result.Path; + steering.CurrentPath = new Queue(result.Path); } // TODO: Move these to movercontroller