public static List <Coordinate3D> FindShortestPath(IWeightedGraph graph, Coordinate3D start, Coordinate3D goal,
MovementType unitMovementType)
{
var costSoFar = PoolProvider.CoordinateIntDictionaryPool.Get();
var cameFrom = PoolProvider.CoordinateDictionaryPool.Get();
var frontier = PoolProvider.CoordinateQueuePool.Get();
var path = graph.GetListFromPool();
frontier.Enqueue(start, 0);
cameFrom[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if (current.Equals(goal))
{
break;
}
var neighbors = graph.GetPassableNeighbors(current);
foreach (var next in neighbors)
{
var newCost = costSoFar[current] + graph.GetMovementCostForTheType(next, unitMovementType);
if (costSoFar.ContainsKey(next) && newCost >= costSoFar[next])
{
continue;
}
costSoFar[next] = newCost;
var priority = newCost + Heuristic(next, goal);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
graph.ReturnListToPool(neighbors);
}
var pathWasFound = cameFrom.ContainsKey(goal);
if (!pathWasFound)
{
return(path);
}
// Reconstructing path
var curr = goal;
while (!curr.Equals(start))
{
path.Add(curr);
curr = cameFrom[curr];
}
// Reverse it to start at actual start point
path.Reverse();
PoolProvider.CoordinateIntDictionaryPool.Return(costSoFar);
PoolProvider.CoordinateDictionaryPool.Return(cameFrom);
PoolProvider.CoordinateQueuePool.Return(frontier);
return(path);
}
public int GetMovementTypeId(MovementType movementType)
{
return(movementType.GetId());
}
public List <Coordinate2D> GetShortestPath(Coordinate2D start, Coordinate2D goal, MovementType unitMovementType)
{
var path = GetShortestPath(start.To3D(), goal.To3D(), unitMovementType);
var result = PoolProvider.OffsetCoordinateListPool.Get();
Coordinate3D.To2D(
path,
start.OffsetType,
result
);
ReturnListToPool(path);
return(result);
}
/// <summary>
/// This methods gets movement costs to the coordinate for the movement type in range = 1.
/// Used internally by path finding and range finding.
/// </summary>
/// <param name="coordinate"></param>
/// <param name="unitMovementType"></param>
/// <returns></returns>
/// <exception cref="InvalidOperationException"></exception>
public int GetMovementCostForTheType(Coordinate3D coordinate, MovementType unitMovementType)
{
var cellState = GetCellState(coordinate);
return(MovementTypes.GetMovementCost(unitMovementType, cellState.TerrainType));
}
/// <summary>
/// Finds shortest path from the start to the goal. Requires pawn's movement type to operate
/// </summary>
/// <param name="start"></param>
/// <param name="goal"></param>
/// <param name="unitMovementType"></param>
/// <returns></returns>
public List <Coordinate3D> GetShortestPath(Coordinate3D start, Coordinate3D goal, MovementType unitMovementType)
{
return(AStarSearch.FindShortestPath(this, start, goal, unitMovementType));
}
