public ANode(Vector2 txPos, ANode parent=null)
{
this.TxPos = txPos;
this.Parent = parent;
this.Child = null;
SetParent(parent);
}
public Path(ANode node, Vector2 pxStart, Vector2 pxEnd)
{
this.NodeList = new List<ANode>();
this.PxStart = pxStart;
this.PxEnd = pxEnd;
ANode currNode = node;
while (currNode != null)
{
NodeList.Add(currNode);
currNode = currNode.Parent;
}
}
/// <summary>
/// Sets the specified node as the current nodes Parent. The method will
/// automatically recalculate the total Length of the current node as well
/// as update the childe reference for the parent being set.
/// </summary>
public void SetParent(ANode parent)
{
if (parent == null)
Length = 0;
else
{
parent.Child = this;
if (IsDiagonalNeighbor(Parent))
Length = parent.Length + DIAGONAL_COST;
else
Length = parent.Length + NORMAL_COST;
}
}
/// <summary>
/// Initialize builds the array of ANodes and their list of neighbors, must be called before a path is generated
/// </summary>
/// <param name="map">TiledMap that you wish to pathfind on</param>
public void Initialize(TiledMap map)
{
// SETUP PATHFINDING NODES
Nodes = new ANode[map.txWidth, map.txHeight];
for (int y = 0; y < map.txHeight; y++)
{
for (int x = 0; x < map.txWidth; x++)
{
ANode node = new ANode();
node.TX = x;
node.TY = y;
node.Center = new Vector2(map.TileWidth * x + map.TileWidth / 2, map.TileHeight * y + map.TileHeight / 2);
Nodes[x, y] = node;
}
}
// Build the neighbors
RebuildNeighbors(map);
}
/// <summary>
/// Generates a Path from the start node to the target node using the AStar algorithm.
/// </summary>
/// <param name="start">ANode to start the path from.</param>
/// <param name="target">ANode to end the path at.</param>
/// <returns>Returns Path (stack<ANode>) that leads from the start to the target. Returns an empty Path if no path can be found.</returns>
public Path GeneratePath(ANode start, ANode target)
{
Path empty = new Path();
List<ANode> openList = new List<ANode>();
List<ANode> closedList = new List<ANode>();
bool targetFound = false;
ANode currentNode = start;
// Setup the variables before we start the loop
currentNode.Parent = null;
openList.Add(currentNode);
while (!targetFound)
{
// If there are no more nodes on the openlist list a path cannot be found
if (openList.Count == 0)
return empty;
float lowestF = 9999f;
// Set current node to the node with the lowest F score
foreach (ANode node in openList)
{
if (node.F < lowestF)
{
lowestF = node.F;
currentNode = node;
}
}
// Check if the target is found
if (target == currentNode)
{
targetFound = true;
Path path = new Path();
while (currentNode.Parent != null)
{
path.Push(currentNode);
currentNode = currentNode.Parent;
}
return path;
}
// Move the current node from open to closed list
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (ANode neighbor in currentNode.Neighbors)
{
if (neighbor.Passable)
{
if (!closedList.Contains(neighbor) && !openList.Contains(neighbor))
{
neighbor.Parent = currentNode;
neighbor.G = neighbor.Parent.G + Vector2.Distance(currentNode.Center, neighbor.Center);
neighbor.H = Vector2.Distance(currentNode.Center, target.Center);
neighbor.F = neighbor.G + neighbor.H;
openList.Add(neighbor);
}
}
}
}
return empty;
}
/// <summary>
/// Initialize builds the array of ANodes and their list of neighbors, must be called before a path is generated
/// </summary>
/// <param name="map">TiledMap that you wish to pathfind on</param>
public void Initialize(TiledMap map)
{
// SETUP PATHFINDING NODES
Nodes = new ANode[map.txWidth, map.txHeight];
for (int y = 0; y < map.txHeight; y++)
{
for (int x = 0; x < map.txWidth; x++)
{
ANode node = new ANode();
node.TX = x;
node.TY = y;
node.Center = new Vector2(map.TileWidth * x + map.TileWidth / 2, map.TileHeight * y + map.TileHeight / 2);
Nodes[x, y] = node;
}
}
// Build the neighbors
RebuildNeighbors(map);
}
/// <summary>
/// Returns a boolean value specifying if the current node is a diagonal
/// neighbour to the node specified in the parameter.
/// </summary>
public bool IsDiagonalNeighbor(ANode node)
{
Vector2 diff = TxPos - node.TxPos;
return Math.Abs(diff.X) == 1 && Math.Abs(diff.Y) == 1;
}
/// <summary>
/// Generates a Path from the start node to the target node using the AStar algorithm.
/// </summary>
/// <param name="start">ANode to start the path from.</param>
/// <param name="target">ANode to end the path at.</param>
/// <returns>Returns Path (stack<ANode>) that leads from the start to the target. Returns an empty Path if no path can be found.</returns>
public Path GeneratePath(ANode start, ANode target)
{
Path empty = new Path();
List <ANode> openList = new List <ANode>();
List <ANode> closedList = new List <ANode>();
bool targetFound = false;
ANode currentNode = start;
// Setup the variables before we start the loop
currentNode.Parent = null;
openList.Add(currentNode);
while (!targetFound)
{
// If there are no more nodes on the openlist list a path cannot be found
if (openList.Count == 0)
{
return(empty);
}
float lowestF = 9999f;
// Set current node to the node with the lowest F score
foreach (ANode node in openList)
{
if (node.F < lowestF)
{
lowestF = node.F;
currentNode = node;
}
}
// Check if the target is found
if (target == currentNode)
{
targetFound = true;
Path path = new Path();
while (currentNode.Parent != null)
{
path.Push(currentNode);
currentNode = currentNode.Parent;
}
return(path);
}
// Move the current node from open to closed list
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (ANode neighbor in currentNode.Neighbors)
{
if (neighbor.Passable)
{
if (!closedList.Contains(neighbor) && !openList.Contains(neighbor))
{
neighbor.Parent = currentNode;
neighbor.G = neighbor.Parent.G + Vector2.Distance(currentNode.Center, neighbor.Center);
neighbor.H = Vector2.Distance(currentNode.Center, target.Center);
neighbor.F = neighbor.G + neighbor.H;
openList.Add(neighbor);
}
}
}
}
return(empty);
}
private bool PathfindingValidator(ANode current, ANode target, TeeEngine engine, GameTime gameTime)
{
Tile tile = engine.Map.GetTxTopMostTile((int)target.TxPos.X, (int)target.TxPos.Y);
// Diagonal to the current tile
bool diagonalResult = true;
if (current != null && Vector2.Distance(current.TxPos, target.TxPos) > 1 )
{
Tile diagonal1 = engine.Map.GetTxTopMostTile((int)target.TxPos.X, (int)current.TxPos.Y);
Tile diagonal2 = engine.Map.GetTxTopMostTile((int)current.TxPos.X, (int)target.TxPos.Y);
diagonalResult =
diagonal1 != null && diagonal2 != null &&
!diagonal1.HasProperty("Impassable") && !diagonal2.HasProperty("Impassable");
}
return tile != null && !tile.HasProperty("Impassable") && diagonalResult;
}
/// <summary>
/// Uses the AStar algorithm to generate a Path from pxStart to pxEnd of valid ANodes to pass through.
/// What is considered to be a valid ANode is determined by the current delegate method assigned to
/// the instance's Validator property. The method will return an empty path if the location is impossible
/// to reach from the specified start location.
/// </summary>
/// <returns>Path object containing a path to the specified end location.</returns>
public Path GeneratePath(Vector2 pxStart, Vector2 pxEnd, TeeEngine engine, GameTime gameTime, NodeValidationHandler validator)
{
// Prevent from creating a dictionary each time this method is called.
_openList.Clear();
_closedList.Clear();
Vector2 TXEND = engine.Map.PxToTx(pxEnd);
Vector2 TXSTART = engine.Map.PxToTx(pxStart);
if(validator == null || !validator(null, new ANode(TXEND), engine, gameTime)) return new Path();
if(validator == null || !validator(null, new ANode(TXSTART), engine, gameTime)) return new Path();
// Working backwards allows us to follow the parent path.
_openList.Add(TXEND, new ANode(TXEND, null));
while (_openList.Count > 0)
{
int min = Int32.MaxValue;
ANode selectedNode = null;
// Select the most promising node from the open list.
foreach (ANode node in _openList.Values)
{
int G = node.Length;
int H = (int) Math.Ceiling(Vector2.Distance(node.TxPos, TXSTART)) * 10;
int length = G + H;
if (length < min)
{
min = length;
selectedNode = node;
}
}
_openList.Remove(selectedNode.TxPos);
_closedList.Add(selectedNode.TxPos, selectedNode);
if (selectedNode.TxPos == TXSTART)
return new Path(selectedNode, pxStart, pxEnd);
// Iterate through the node's neighbors.
for (int i = -1; i < 2; i++)
{
for (int j = -1; j < 2; j++)
{
if (i == 0 && j == 0) continue;
ANode node = new ANode(selectedNode.TxPos + new Vector2(i, j), selectedNode);
// If a validator has been supplied, use it to check if the current node is valid.
if ((validator == null || validator(selectedNode, node, engine, gameTime)) && !_closedList.ContainsKey(node.TxPos))
{
if (_openList.ContainsKey(node.TxPos))
{
if (_openList[node.TxPos].Length > node.Length)
_openList[node.TxPos].SetParent(node.Parent);
}
else
_openList.Add(node.TxPos, node);
}
}
}
}
return new Path();
}
