static public float Heuristic(TileMapGraphNode a, TileMapGraphNode b)
{
Vector2 aPos = a.GetTileMapPosition();
Vector2 bPos = b.GetTileMapPosition();
return(Mathf.Abs(aPos.x - bPos.x) + Mathf.Abs(aPos.y - bPos.y));
}
/// <summary>
/// Calculate the movement cost between two GraphNodes
/// </summary>
public float CalculateMovementCost(TileMapGraphNode from, TileMapGraphNode to)
{
if (to.IsWall())
{
return(Mathf.Inf);
}
return(from.GetTileMapPosition().DistanceSquaredTo(to.GetTileMapPosition()));
}
private void PopulateTiles()
{
Vector2I pos = new Vector2I(mapBounds.Position);
Vector2I size = new Vector2I(mapBounds.Size);
for (int x = 0; x < size.x; x++)
{
for (int y = 0; y < size.y; y++)
{
Vector2I tileMapPos = new Vector2I(pos.x + x, pos.y + y);
tileGraph[x, y] = new TileMapGraphNode(
tileMapPos,
new Vector2I(x, y),
wallMap.MapToWorld(tileMapPos.AsVector2()),
wallMap.GetCell(tileMapPos.x, tileMapPos.y) != -1
);
}
}
}
private void BuildGraph()
{
Vector2I pos = new Vector2I(mapBounds.Position);
Vector2I size = new Vector2I(mapBounds.Size);
for (int x = 0; x < size.x; x++)
{
for (int y = 0; y < size.y; y++)
{
TileMapGraphNode currentTile = tileGraph[x, y];
if (currentTile != null && !currentTile.IsWall())
{
// NORTH
if (y > 0 && !tileGraph[x, y - 1].IsWall())
{
currentTile.SetNeighbor(tileGraph[x, y - 1], NeighborDirection.NORTH);
}
// NORTH_EAST
if (y > 0 && x < size.x - 1 && !tileGraph[x + 1, y - 1].IsWall())
{
currentTile.SetNeighbor(tileGraph[x + 1, y - 1], NeighborDirection.NORTH_EAST);
}
// NORTH_WEST
if (y > 0 && x > 0 && !tileGraph[x - 1, y - 1].IsWall())
{
currentTile.SetNeighbor(tileGraph[x - 1, y - 1], NeighborDirection.NORTH_WEST);
}
// SOUTH
if (y < size.y - 1 && !tileGraph[x, y + 1].IsWall())
{
currentTile.SetNeighbor(tileGraph[x, y + 1], NeighborDirection.SOUTH);
}
// SOUTH_EAST
if (y < size.y - 1 && x < size.x - 1 && !tileGraph[x + 1, y + 1].IsWall())
{
currentTile.SetNeighbor(tileGraph[x + 1, y + 1], NeighborDirection.SOUTH_EAST);
}
// SOUTH_WEST
if (y < size.y - 1 && x > 0 && !tileGraph[x - 1, y + 1].IsWall())
{
currentTile.SetNeighbor(tileGraph[x - 1, y + 1], NeighborDirection.SOUTH_WEST);
}
// EAST
if (x < size.x - 1 && !tileGraph[x + 1, y].IsWall())
{
currentTile.SetNeighbor(tileGraph[x + 1, y], NeighborDirection.EAST);
}
// WEST
if (x > 0 && !tileGraph[x - 1, y].IsWall())
{
currentTile.SetNeighbor(tileGraph[x - 1, y], NeighborDirection.WEST);
}
}
}
}
}
public void SetNeighbor(TileMapGraphNode neighbor, NeighborDirection direction)
{
neighbors[(int)direction] = neighbor;
}
/// <summary>
/// Generate a path from Start to Goal using the AStar pathfinding algorithm.
/// </summary>
/// <param name="mapGraph">TileMapGraph to search</param>
/// <param name="start">Starting TileMapGraphNode</param>
/// <param name="goal">Ending TileMapGraphNode</param>
/// <returns>Null if no path, or a List of TileMapGraphNodes, that form a path from the start(inclusive) to the
/// goal (inclusive)</returns>
public static List <TileMapGraphNode> GeneratePath(
TileMapGraph mapGraph,
TileMapGraphNode start,
TileMapGraphNode goal
)
{
if (start == null || goal == null || goal.IsWall())
{
return(null);
}
var cameFrom = new Dictionary <TileMapGraphNode, TileMapGraphNode>();
var costSoFar = new Dictionary <TileMapGraphNode, float>();
var frontier = new FastPriorityQueue <TileGraphNodeQueueNode>(250);
frontier.Enqueue(new TileGraphNodeQueueNode(start), 0);
cameFrom[start] = start;
costSoFar[start] = 0;
// Walk the graph from start to goal
while (frontier.Count > 0)
{
TileMapGraphNode current = frontier.Dequeue().node;
if (current.Equals(goal))
{
break;
}
foreach (TileMapGraphNode next in current.GetNeighbors())
{
if (next != null)
{
float newCost = costSoFar[current] + mapGraph.CalculateMovementCost(current, next);
if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])
{
costSoFar[next] = newCost;
float priority = newCost + Heuristic(next, goal);
frontier.Enqueue(new TileGraphNodeQueueNode(next), priority);
cameFrom[next] = current;
}
}
}
}
// Build the path from nodes
List <TileMapGraphNode> path = null;
if (cameFrom.ContainsKey(goal))
{
path = new List <TileMapGraphNode>();
TileMapGraphNode current = goal;
while (current != start)
{
path.Add(current);
current = cameFrom[current];
}
path.Add(start);
path.Reverse();
}
return(path);
}
public TileGraphNodeQueueNode(TileMapGraphNode node)
{
this.node = node;
}
