805a5f1689
There was some extra bloat in the path graph updates. Now the queue should also just run if it gets too big regardless. Un-anchored physics objects are no longer a hard fail for pathfinding. Add AccessReader support so open / close doors show up for pathfinding AI also ensure they call the operator's shutdown when they're shutdown so that should cancel the pathfinding job. I tried to split these into 2 commits but they were kinda coupled together Co-authored-by: Metal Gear Sloth <metalgearsloth@gmail.com>
390 lines
15 KiB
C#
390 lines
15 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
using Content.Server.GameObjects.EntitySystems.JobQueues;
|
|
using Content.Server.GameObjects.EntitySystems.Pathfinding;
|
|
using Content.Shared.AI;
|
|
using Robust.Shared.Log;
|
|
using Robust.Shared.Map;
|
|
using Robust.Shared.Maths;
|
|
using Robust.Shared.Utility;
|
|
|
|
namespace Content.Server.GameObjects.EntitySystems.AI.Pathfinding.Pathfinders
|
|
{
|
|
public class JpsPathfindingJob : Job<Queue<TileRef>>
|
|
{
|
|
public static event Action<SharedAiDebug.JpsRouteDebug> DebugRoute;
|
|
|
|
private PathfindingNode _startNode;
|
|
private PathfindingNode _endNode;
|
|
private PathfindingArgs _pathfindingArgs;
|
|
|
|
public JpsPathfindingJob(double maxTime,
|
|
PathfindingNode startNode,
|
|
PathfindingNode endNode,
|
|
PathfindingArgs pathfindingArgs,
|
|
CancellationToken cancellationToken) : base(maxTime, cancellationToken)
|
|
{
|
|
_startNode = startNode;
|
|
_endNode = endNode;
|
|
_pathfindingArgs = pathfindingArgs;
|
|
}
|
|
|
|
protected override async Task<Queue<TileRef>> Process()
|
|
{
|
|
// VERY similar to A*; main difference is with the neighbor tiles you look for jump nodes instead
|
|
if (_startNode == null ||
|
|
_endNode == null)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// If we couldn't get a nearby node that's good enough
|
|
if (!PathfindingHelpers.TryEndNode(ref _endNode, _pathfindingArgs))
|
|
{
|
|
return null;
|
|
}
|
|
|
|
var openTiles = new PriorityQueue<ValueTuple<float, PathfindingNode>>(new PathfindingComparer());
|
|
var gScores = new Dictionary<PathfindingNode, float>();
|
|
var cameFrom = new Dictionary<PathfindingNode, PathfindingNode>();
|
|
var closedTiles = new HashSet<PathfindingNode>();
|
|
|
|
#if DEBUG
|
|
var jumpNodes = new HashSet<PathfindingNode>();
|
|
#endif
|
|
|
|
PathfindingNode currentNode = null;
|
|
openTiles.Add((0, _startNode));
|
|
gScores[_startNode] = 0.0f;
|
|
var routeFound = false;
|
|
var count = 0;
|
|
|
|
while (openTiles.Count > 0)
|
|
{
|
|
count++;
|
|
|
|
// JPS probably getting a lot fewer nodes than A* is
|
|
if (count % 5 == 0 && count > 0)
|
|
{
|
|
await SuspendIfOutOfTime();
|
|
}
|
|
|
|
(_, currentNode) = openTiles.Take();
|
|
if (currentNode.Equals(_endNode))
|
|
{
|
|
routeFound = true;
|
|
break;
|
|
}
|
|
|
|
foreach (var (direction, _) in currentNode.Neighbors)
|
|
{
|
|
var jumpNode = GetJumpPoint(currentNode, direction, _endNode);
|
|
|
|
if (jumpNode != null && !closedTiles.Contains(jumpNode))
|
|
{
|
|
closedTiles.Add(jumpNode);
|
|
#if DEBUG
|
|
jumpNodes.Add(jumpNode);
|
|
#endif
|
|
// GetJumpPoint should already check if we can traverse to the node
|
|
var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, jumpNode);
|
|
|
|
if (tileCost == null)
|
|
{
|
|
throw new InvalidOperationException();
|
|
}
|
|
|
|
var gScore = gScores[currentNode] + tileCost.Value;
|
|
|
|
if (gScores.TryGetValue(jumpNode, out var nextValue) && gScore >= nextValue)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
cameFrom[jumpNode] = currentNode;
|
|
gScores[jumpNode] = gScore;
|
|
// pFactor is tie-breaker where the fscore is otherwise equal.
|
|
// See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties
|
|
// There's other ways to do it but future consideration
|
|
var fScore = gScores[jumpNode] + PathfindingHelpers.OctileDistance(_endNode, jumpNode) * (1.0f + 1.0f / 1000.0f);
|
|
openTiles.Add((fScore, jumpNode));
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!routeFound)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
var route = PathfindingHelpers.ReconstructJumpPath(cameFrom, currentNode);
|
|
if (route.Count == 1)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
#if DEBUG
|
|
// Need to get data into an easier format to send to the relevant clients
|
|
if (DebugRoute != null && route.Count > 0)
|
|
{
|
|
var debugJumpNodes = new HashSet<TileRef>(jumpNodes.Count);
|
|
|
|
foreach (var node in jumpNodes)
|
|
{
|
|
debugJumpNodes.Add(node.TileRef);
|
|
}
|
|
|
|
var debugRoute = new SharedAiDebug.JpsRouteDebug(
|
|
_pathfindingArgs.Uid,
|
|
route,
|
|
debugJumpNodes,
|
|
DebugTime);
|
|
|
|
DebugRoute.Invoke(debugRoute);
|
|
}
|
|
#endif
|
|
|
|
return route;
|
|
}
|
|
|
|
private PathfindingNode GetJumpPoint(PathfindingNode currentNode, Direction direction, PathfindingNode endNode)
|
|
{
|
|
var count = 0;
|
|
|
|
while (count < 1000)
|
|
{
|
|
count++;
|
|
var nextNode = currentNode.GetNeighbor(direction);
|
|
|
|
// We'll do opposite DirectionTraversable just because of how the method's setup
|
|
// Nodes should be 2-way anyway.
|
|
if (nextNode == null ||
|
|
PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode) == null)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
if (nextNode == endNode)
|
|
{
|
|
return endNode;
|
|
}
|
|
|
|
// Horizontal and vertical are treated the same i.e.
|
|
// They only check in their specific direction
|
|
// (So Going North means you check NorthWest and NorthEast to see if we're a jump point)
|
|
|
|
// Diagonals also check the cardinal directions at the same time at the same time
|
|
|
|
// See https://harablog.wordpress.com/2011/09/07/jump-point-search/ for original description
|
|
switch (direction)
|
|
{
|
|
case Direction.East:
|
|
if (IsCardinalJumpPoint(direction, nextNode))
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
break;
|
|
case Direction.NorthEast:
|
|
if (IsDiagonalJumpPoint(direction, nextNode))
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
if (GetJumpPoint(nextNode, Direction.North, endNode) != null || GetJumpPoint(nextNode, Direction.East, endNode) != null)
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
break;
|
|
case Direction.North:
|
|
if (IsCardinalJumpPoint(direction, nextNode))
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
break;
|
|
case Direction.NorthWest:
|
|
if (IsDiagonalJumpPoint(direction, nextNode))
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
if (GetJumpPoint(nextNode, Direction.North, endNode) != null || GetJumpPoint(nextNode, Direction.West, endNode) != null)
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
break;
|
|
case Direction.West:
|
|
if (IsCardinalJumpPoint(direction, nextNode))
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
break;
|
|
case Direction.SouthWest:
|
|
if (IsDiagonalJumpPoint(direction, nextNode))
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
if (GetJumpPoint(nextNode, Direction.South, endNode) != null || GetJumpPoint(nextNode, Direction.West, endNode) != null)
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
break;
|
|
case Direction.South:
|
|
if (IsCardinalJumpPoint(direction, nextNode))
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
break;
|
|
case Direction.SouthEast:
|
|
if (IsDiagonalJumpPoint(direction, nextNode))
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
if (GetJumpPoint(nextNode, Direction.South, endNode) != null || GetJumpPoint(nextNode, Direction.East, endNode) != null)
|
|
{
|
|
return nextNode;
|
|
}
|
|
|
|
break;
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(direction), direction, null);
|
|
}
|
|
|
|
currentNode = nextNode;
|
|
}
|
|
|
|
Logger.WarningS("pathfinding", "Recursion found in JPS pathfinder");
|
|
return null;
|
|
}
|
|
|
|
private bool IsDiagonalJumpPoint(Direction direction, PathfindingNode currentNode)
|
|
{
|
|
// If we're going diagonally need to check all cardinals.
|
|
// I just just using casts int casts and offset to make it smaller but brain no workyand it wasn't working.
|
|
// From NorthEast we check (Closed / Open) S - SE, W - NW
|
|
|
|
PathfindingNode openNeighborOne;
|
|
PathfindingNode closedNeighborOne;
|
|
PathfindingNode openNeighborTwo;
|
|
PathfindingNode closedNeighborTwo;
|
|
|
|
switch (direction)
|
|
{
|
|
case Direction.NorthEast:
|
|
openNeighborOne = currentNode.GetNeighbor(Direction.SouthEast);
|
|
closedNeighborOne = currentNode.GetNeighbor(Direction.South);
|
|
|
|
openNeighborTwo = currentNode.GetNeighbor(Direction.NorthWest);
|
|
closedNeighborTwo = currentNode.GetNeighbor(Direction.West);
|
|
break;
|
|
case Direction.SouthEast:
|
|
openNeighborOne = currentNode.GetNeighbor(Direction.NorthEast);
|
|
closedNeighborOne = currentNode.GetNeighbor(Direction.North);
|
|
|
|
openNeighborTwo = currentNode.GetNeighbor(Direction.SouthWest);
|
|
closedNeighborTwo = currentNode.GetNeighbor(Direction.West);
|
|
break;
|
|
case Direction.SouthWest:
|
|
openNeighborOne = currentNode.GetNeighbor(Direction.NorthWest);
|
|
closedNeighborOne = currentNode.GetNeighbor(Direction.North);
|
|
|
|
openNeighborTwo = currentNode.GetNeighbor(Direction.SouthEast);
|
|
closedNeighborTwo = currentNode.GetNeighbor(Direction.East);
|
|
break;
|
|
case Direction.NorthWest:
|
|
openNeighborOne = currentNode.GetNeighbor(Direction.SouthWest);
|
|
closedNeighborOne = currentNode.GetNeighbor(Direction.South);
|
|
|
|
openNeighborTwo = currentNode.GetNeighbor(Direction.NorthEast);
|
|
closedNeighborTwo = currentNode.GetNeighbor(Direction.East);
|
|
break;
|
|
default:
|
|
throw new ArgumentOutOfRangeException();
|
|
}
|
|
|
|
if ((closedNeighborOne == null || PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, closedNeighborOne) == null)
|
|
&& openNeighborOne != null && PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, openNeighborOne) != null)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
if ((closedNeighborTwo == null || PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, closedNeighborTwo) == null)
|
|
&& openNeighborTwo != null && PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, openNeighborTwo) != null)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check to see if the node is a jump point (only works for cardinal directions)
|
|
/// </summary>
|
|
private bool IsCardinalJumpPoint(Direction direction, PathfindingNode currentNode)
|
|
{
|
|
PathfindingNode openNeighborOne;
|
|
PathfindingNode closedNeighborOne;
|
|
PathfindingNode openNeighborTwo;
|
|
PathfindingNode closedNeighborTwo;
|
|
|
|
switch (direction)
|
|
{
|
|
case Direction.North:
|
|
openNeighborOne = currentNode.GetNeighbor(Direction.NorthEast);
|
|
closedNeighborOne = currentNode.GetNeighbor(Direction.East);
|
|
|
|
openNeighborTwo = currentNode.GetNeighbor(Direction.NorthWest);
|
|
closedNeighborTwo = currentNode.GetNeighbor(Direction.West);
|
|
break;
|
|
case Direction.East:
|
|
openNeighborOne = currentNode.GetNeighbor(Direction.NorthEast);
|
|
closedNeighborOne = currentNode.GetNeighbor(Direction.North);
|
|
|
|
openNeighborTwo = currentNode.GetNeighbor(Direction.SouthEast);
|
|
closedNeighborTwo = currentNode.GetNeighbor(Direction.South);
|
|
break;
|
|
case Direction.South:
|
|
openNeighborOne = currentNode.GetNeighbor(Direction.SouthEast);
|
|
closedNeighborOne = currentNode.GetNeighbor(Direction.East);
|
|
|
|
openNeighborTwo = currentNode.GetNeighbor(Direction.SouthWest);
|
|
closedNeighborTwo = currentNode.GetNeighbor(Direction.West);
|
|
break;
|
|
case Direction.West:
|
|
openNeighborOne = currentNode.GetNeighbor(Direction.NorthWest);
|
|
closedNeighborOne = currentNode.GetNeighbor(Direction.North);
|
|
|
|
openNeighborTwo = currentNode.GetNeighbor(Direction.SouthWest);
|
|
closedNeighborTwo = currentNode.GetNeighbor(Direction.South);
|
|
break;
|
|
default:
|
|
throw new ArgumentOutOfRangeException();
|
|
}
|
|
|
|
if ((closedNeighborOne == null || !PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, closedNeighborOne)) &&
|
|
(openNeighborOne != null && PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, openNeighborOne)))
|
|
{
|
|
return true;
|
|
}
|
|
|
|
if ((closedNeighborTwo == null || !PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, closedNeighborTwo)) &&
|
|
(openNeighborTwo != null && PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, openNeighborTwo)))
|
|
{
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
}
|
|
}
|