public PathNode(
PathNode parentNode,
PathNode endNode,
Vector2 gridLocation,
float cost)
{
ParentNode = parentNode;
GridLocation = gridLocation;
EndNode = endNode;
DirectCost = cost;
if (!(endNode == null))
{
TotalCost = DirectCost + LinearCost();
}
}
static private void addNodeToOpenList(PathNode node)
{
int index = 0;
float cost = node.TotalCost;
while ((openList.Count() > index) &&
(cost < openList[index].TotalCost))
{
index++;
}
openList.Insert(index, node);
nodeCosts[node.GridLocation] = node.TotalCost;
nodeStatus[node.GridLocation] = NodeStatus.Open;
}
public bool IsEqualToNode(PathNode node)
{
return (GridLocation == node.GridLocation);
}
static private List<PathNode> findAdjacentNodes(
PathNode currentNode,
PathNode endNode)
{
List<PathNode> adjacentNodes = new List<PathNode>();
int X = currentNode.GridX;
int Y = currentNode.GridY;
bool upLeft = true;
bool upRight = true;
bool downLeft = true;
bool downRight = true;
if ((X > 0) && (!TileMap.IsWallTile(X - 1, Y)))
{
adjacentNodes.Add(new PathNode(
currentNode,
endNode,
new Vector2(X - 1, Y),
CostStraight + currentNode.DirectCost));
}
else
{
upLeft = false;
downLeft = false;
}
if ((X < 49) && (!TileMap.IsWallTile(X + 1, Y)))
{
adjacentNodes.Add(new PathNode(
currentNode,
endNode,
new Vector2(X + 1, Y),
CostStraight + currentNode.DirectCost));
}
else
{
upRight = false;
downRight = false;
}
if ((Y > 0) && (!TileMap.IsWallTile(X, Y - 1)))
{
adjacentNodes.Add(new PathNode(
currentNode,
endNode,
new Vector2(X, Y - 1),
CostStraight + currentNode.DirectCost));
}
else
{
upLeft = false;
upRight = false;
}
if ((Y < 49) && (!TileMap.IsWallTile(X, Y + 1)))
{
adjacentNodes.Add(new PathNode(
currentNode,
endNode,
new Vector2(X, Y + 1),
CostStraight + currentNode.DirectCost));
}
else
{
downLeft = false;
downRight = false;
}
if ((upLeft) && (!TileMap.IsWallTile(X - 1, Y - 1)))
{
adjacentNodes.Add(new PathNode(
currentNode,
endNode,
new Vector2(X - 1, Y - 1),
CostDiagonal + currentNode.DirectCost));
}
if ((upRight) && (!TileMap.IsWallTile(X + 1, Y - 1)))
{
adjacentNodes.Add(new PathNode(
currentNode,
endNode,
new Vector2(X + 1, Y - 1),
CostDiagonal + currentNode.DirectCost));
}
if ((downLeft) && (!TileMap.IsWallTile(X - 1, Y + 1)))
{
adjacentNodes.Add(new PathNode(
currentNode,
endNode,
new Vector2(X - 1, Y + 1),
CostDiagonal + currentNode.DirectCost));
}
if ((downRight) && (!TileMap.IsWallTile(X + 1, Y + 1)))
{
adjacentNodes.Add(new PathNode(
currentNode,
endNode,
new Vector2(X + 1, Y + 1),
CostDiagonal + currentNode.DirectCost));
}
return adjacentNodes;
}
static public List<Vector2> FindPath(
Vector2 startTile,
Vector2 endTile)
{
if (TileMap.IsWallTile(endTile) ||
TileMap.IsWallTile(startTile))
{
return null;
}
openList.Clear();
nodeCosts.Clear();
nodeStatus.Clear();
PathNode startNode;
PathNode endNode;
endNode = new PathNode(null, null, endTile, 0);
startNode = new PathNode(null, endNode, startTile, 0);
addNodeToOpenList(startNode);
while (openList.Count > 0)
{
PathNode currentNode = openList[openList.Count - 1];
if (currentNode.IsEqualToNode(endNode))
{
List<Vector2> bestPath = new List<Vector2>();
while (currentNode != null)
{
bestPath.Insert(0, currentNode.GridLocation);
currentNode = currentNode.ParentNode;
}
return bestPath;
}
openList.Remove(currentNode);
nodeCosts.Remove(currentNode.GridLocation);
foreach (
PathNode possibleNode in
findAdjacentNodes(currentNode, endNode))
{
if (nodeStatus.ContainsKey(possibleNode.GridLocation))
{
if (nodeStatus[possibleNode.GridLocation] ==
NodeStatus.Closed)
{
continue;
}
if (
nodeStatus[possibleNode.GridLocation] ==
NodeStatus.Open)
{
if (possibleNode.TotalCost >=
nodeCosts[possibleNode.GridLocation])
{
continue;
}
}
}
addNodeToOpenList(possibleNode);
}
nodeStatus[currentNode.GridLocation] = NodeStatus.Closed;
}
return null;
}