public List <PathNode> FindPath(int startX, int startZ, int endX, int endZ)
{
nodeMap[startX, startZ] = new PathNode {
xValue = startX, zValue = startZ
};
PathNode startNode = nodeMap[startX, startZ];
nodeMap[endX, endZ] = new PathNode {
xValue = endX, zValue = endZ
};
PathNode endNode = nodeMap[endX, endZ];
openList = new List <PathNode> {
startNode
};
closedList = new List <PathNode>();
startNode.gCost = 0;
startNode.hCost = CalculateDistance(startNode, endNode);
startNode.CalculateCost();
for (int i = 0; i < mapSize; i++)
{
for (int j = 0; j < mapSize; j++)
{
nodeMap[i, j] = new PathNode {
xValue = i, zValue = j, gCost = int.MaxValue, previousNode = null
};
nodeMap[i, j].CalculateCost();
}
}
while (openList.Count > 0)
{
PathNode currentNode = GetLowestFCost(openList);
if (currentNode.xValue == endNode.xValue && currentNode.zValue == endNode.zValue)
{
return(CalculatePath(nodeMap[endX, endZ]));
}
openList.Remove(currentNode);
closedList.Add(currentNode);
foreach (PathNode neighbourNode in GetNeighbourList(currentNode))
{
if (closedList.Contains(neighbourNode))
{
continue;
}
int newGCost = currentNode.gCost + CalculateDistance(currentNode, neighbourNode);
if (newGCost < neighbourNode.gCost)
{
neighbourNode.previousNode = currentNode;
neighbourNode.gCost = newGCost;
neighbourNode.hCost = CalculateDistance(neighbourNode, endNode);
neighbourNode.CalculateCost();
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
return(null);
}
