Add utility AI (#806)
Co-authored-by: Pieter-Jan Briers <pieterjan.briers@gmail.com> Co-authored-by: Metal Gear Sloth <metalgearsloth@gmail.com> Co-authored-by: Pieter-Jan Briers <pieterjan.briers+git@gmail.com>
This commit is contained in:
metalgearsloth
@@ -0,0 +1,389 @@
|
||||
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 (!Utils.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 = Utils.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] + Utils.OctileDistance(_endNode, jumpNode) * (1.0f + 1.0f / 1000.0f);
|
||||
openTiles.Add((fScore, jumpNode));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!routeFound)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var route = Utils.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 ||
|
||||
Utils.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 || Utils.GetTileCost(_pathfindingArgs, currentNode, closedNeighborOne) == null)
|
||||
&& openNeighborOne != null && Utils.GetTileCost(_pathfindingArgs, currentNode, openNeighborOne) != null)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
if ((closedNeighborTwo == null || Utils.GetTileCost(_pathfindingArgs, currentNode, closedNeighborTwo) == null)
|
||||
&& openNeighborTwo != null && Utils.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 || !Utils.Traversable(_pathfindingArgs.CollisionMask, closedNeighborOne.CollisionMask)) &&
|
||||
(openNeighborOne != null && Utils.Traversable(_pathfindingArgs.CollisionMask, openNeighborOne.CollisionMask)))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
if ((closedNeighborTwo == null || !Utils.Traversable(_pathfindingArgs.CollisionMask, closedNeighborTwo.CollisionMask)) &&
|
||||
(openNeighborTwo != null && Utils.Traversable(_pathfindingArgs.CollisionMask, openNeighborTwo.CollisionMask)))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user