private void DispatchAStarDebug(SharedAiDebug.AStarRouteDebug routeDebug)
{
var mapManager = IoCManager.Resolve <IMapManager>();
var route = new List <Vector2>();
foreach (var tile in routeDebug.Route)
{
var tileGrid = mapManager.GetGrid(tile.GridIndex).GridTileToLocal(tile.GridIndices);
route.Add(mapManager.GetGrid(tile.GridIndex).LocalToWorld(tileGrid).Position);
}
var cameFrom = new Dictionary <Vector2, Vector2>();
foreach (var(from, to) in routeDebug.CameFrom)
{
var tileOneGrid = mapManager.GetGrid(from.GridIndex).GridTileToLocal(from.GridIndices);
var tileOneWorld = mapManager.GetGrid(from.GridIndex).LocalToWorld(tileOneGrid).Position;
var tileTwoGrid = mapManager.GetGrid(to.GridIndex).GridTileToLocal(to.GridIndices);
var tileTwoWorld = mapManager.GetGrid(to.GridIndex).LocalToWorld(tileTwoGrid).Position;
cameFrom.Add(tileOneWorld, tileTwoWorld);
}
var gScores = new Dictionary <Vector2, float>();
foreach (var(tile, score) in routeDebug.GScores)
{
var tileGrid = mapManager.GetGrid(tile.GridIndex).GridTileToLocal(tile.GridIndices);
gScores.Add(mapManager.GetGrid(tile.GridIndex).LocalToWorld(tileGrid).Position, score);
}
var closedTiles = new List <Vector2>();
foreach (var tile in routeDebug.ClosedTiles)
{
var tileGrid = mapManager.GetGrid(tile.GridIndex).GridTileToLocal(tile.GridIndices);
closedTiles.Add(mapManager.GetGrid(tile.GridIndex).LocalToWorld(tileGrid).Position);
}
var systemMessage = new SharedAiDebug.AStarRouteMessage(
routeDebug.EntityUid,
route,
cameFrom,
gScores,
closedTiles,
routeDebug.TimeTaken
);
EntityManager.EntityNetManager.SendSystemNetworkMessage(systemMessage);
}
private void DispatchAStarDebug(SharedAiDebug.AStarRouteDebug routeDebug)
{
var mapManager = IoCManager.Resolve <IMapManager>();
var route = new List <Vector2>();
foreach (var tile in routeDebug.Route)
{
var tileGrid = mapManager.GetGrid(tile.GridIndex).GridTileToLocal(tile.GridIndices);
route.Add(tileGrid.ToMapPos(EntityManager));
}
var cameFrom = new Dictionary <Vector2, Vector2>();
foreach (var(from, to) in routeDebug.CameFrom)
{
var tileOneGrid = mapManager.GetGrid(from.GridIndex).GridTileToLocal(from.GridIndices);
var tileOneWorld = tileOneGrid.ToMapPos(EntityManager);
var tileTwoGrid = mapManager.GetGrid(to.GridIndex).GridTileToLocal(to.GridIndices);
var tileTwoWorld = tileTwoGrid.ToMapPos(EntityManager);
cameFrom[tileOneWorld] = tileTwoWorld;
}
var gScores = new Dictionary <Vector2, float>();
foreach (var(tile, score) in routeDebug.GScores)
{
var tileGrid = mapManager.GetGrid(tile.GridIndex).GridTileToLocal(tile.GridIndices);
gScores[tileGrid.ToMapPos(EntityManager)] = score;
}
var systemMessage = new SharedAiDebug.AStarRouteMessage(
routeDebug.EntityUid,
route,
cameFrom,
gScores,
routeDebug.TimeTaken
);
RaiseNetworkEvent(systemMessage);
}
protected override async Task <Queue <TileRef>?> Process()
{
if (_startNode == null ||
_endNode == null ||
Status == JobStatus.Finished)
{
return(null);
}
// If we couldn't get a nearby node that's good enough
if (!PathfindingHelpers.TryEndNode(ref _endNode, _pathfindingArgs))
{
return(null);
}
var frontier = new PriorityQueue <ValueTuple <float, PathfindingNode> >(new PathfindingComparer());
var costSoFar = new Dictionary <PathfindingNode, float>();
var cameFrom = new Dictionary <PathfindingNode, PathfindingNode>();
PathfindingNode?currentNode = null;
frontier.Add((0.0f, _startNode));
costSoFar[_startNode] = 0.0f;
var routeFound = false;
var count = 0;
while (frontier.Count > 0)
{
// Handle whether we need to pause if we've taken too long
count++;
if (count % 20 == 0 && count > 0)
{
await SuspendIfOutOfTime();
if (_startNode == null || _endNode == null)
{
return(null);
}
}
// Actual pathfinding here
(_, currentNode) = frontier.Take();
if (currentNode.Equals(_endNode))
{
routeFound = true;
break;
}
foreach (var nextNode in currentNode.GetNeighbors())
{
// If tile is untraversable it'll be null
var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode);
if (tileCost == null)
{
continue;
}
// So if we're going NE then that means either N or E needs to be free to actually get there
var direction = PathfindingHelpers.RelativeDirection(nextNode, currentNode);
if (!PathfindingHelpers.DirectionTraversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, currentNode, direction))
{
continue;
}
// f = g + h
// gScore is distance to the start node
// hScore is distance to the end node
var gScore = costSoFar[currentNode] + tileCost.Value;
if (costSoFar.TryGetValue(nextNode, out var nextValue) && gScore >= nextValue)
{
continue;
}
cameFrom[nextNode] = currentNode;
costSoFar[nextNode] = 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
// The closer the fScore is to the actual distance then the better the pathfinder will be
// (i.e. somewhere between 1 and infinite)
// Can use hierarchical pathfinder or whatever to improve the heuristic but this is fine for now.
var fScore = gScore + PathfindingHelpers.OctileDistance(_endNode, nextNode) * (1.0f + 1.0f / 1000.0f);
frontier.Add((fScore, nextNode));
}
}
if (!routeFound)
{
return(null);
}
DebugTools.AssertNotNull(currentNode);
var route = PathfindingHelpers.ReconstructPath(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 debugCameFrom = new Dictionary <TileRef, TileRef>(cameFrom.Count);
var debugGScores = new Dictionary <TileRef, float>(costSoFar.Count);
foreach (var(node, parent) in cameFrom)
{
debugCameFrom.Add(node.TileRef, parent.TileRef);
}
foreach (var(node, score) in costSoFar)
{
debugGScores.Add(node.TileRef, score);
}
var debugRoute = new SharedAiDebug.AStarRouteDebug(
_pathfindingArgs.Uid,
route,
debugCameFrom,
debugGScores,
DebugTime);
DebugRoute.Invoke(debugRoute);
}
#endif
return(route);
}
protected override async Task <Queue <TileRef> > Process()
{
if (_startNode == null ||
_endNode == null ||
Status == JobStatus.Finished)
{
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>();
PathfindingNode currentNode = null;
openTiles.Add((0.0f, _startNode));
gScores[_startNode] = 0.0f;
var routeFound = false;
var count = 0;
while (openTiles.Count > 0)
{
count++;
if (count % 20 == 0 && count > 0)
{
await SuspendIfOutOfTime();
}
if (_startNode == null || _endNode == null)
{
return(null);
}
(_, currentNode) = openTiles.Take();
if (currentNode.Equals(_endNode))
{
routeFound = true;
break;
}
closedTiles.Add(currentNode);
foreach (var(direction, nextNode) in currentNode.Neighbors)
{
if (closedTiles.Contains(nextNode))
{
continue;
}
// If tile is untraversable it'll be null
var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode);
if (tileCost == null || !PathfindingHelpers.DirectionTraversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, currentNode, direction))
{
continue;
}
var gScore = gScores[currentNode] + tileCost.Value;
if (gScores.TryGetValue(nextNode, out var nextValue) && gScore >= nextValue)
{
continue;
}
cameFrom[nextNode] = currentNode;
gScores[nextNode] = 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[nextNode] + PathfindingHelpers.OctileDistance(_endNode, nextNode) * (1.0f + 1.0f / 1000.0f);
openTiles.Add((fScore, nextNode));
}
}
if (!routeFound)
{
return(null);
}
var route = PathfindingHelpers.ReconstructPath(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 debugCameFrom = new Dictionary <TileRef, TileRef>(cameFrom.Count);
var debugGScores = new Dictionary <TileRef, float>(gScores.Count);
var debugClosedTiles = new HashSet <TileRef>(closedTiles.Count);
foreach (var(node, parent) in cameFrom)
{
debugCameFrom.Add(node.TileRef, parent.TileRef);
}
foreach (var(node, score) in gScores)
{
debugGScores.Add(node.TileRef, score);
}
foreach (var node in closedTiles)
{
debugClosedTiles.Add(node.TileRef);
}
var debugRoute = new SharedAiDebug.AStarRouteDebug(
_pathfindingArgs.Uid,
route,
debugCameFrom,
debugGScores,
debugClosedTiles,
DebugTime);
DebugRoute.Invoke(debugRoute);
}
#endif
return(route);
}
