void FindPath(Vector3 startPosition, Vector3 targetPosition)
{
Node startNode = grid.PositionConvertNode(startPosition);
Node targetNode = grid.PositionConvertNode(targetPosition);
List <Node> openNodes = new List <Node> ();
List <Node> closedNodes = new List <Node> ();
openNodes.Add(startNode);
//Searches through all open nodes to find the node with lowest fCost
while (openNodes.Count > 0 && targetNode.inBounds == true)
{
//Purely here for the first step
Node activeNode = openNodes [0];
//The slowest part of the whole code, it crashes it occasionally because it is so inefficient
for (int i = 1; i < openNodes.Count; i++)
{
if (openNodes [i].fCost < activeNode.fCost || activeNode.fCost == openNodes [i].fCost && openNodes [i].hCost < activeNode.hCost)
{
activeNode = openNodes [i];
}
}
openNodes.Remove(activeNode);
closedNodes.Add(activeNode);
if (activeNode == targetNode)
{
ReturnPath(startNode, targetNode);
return;
}
foreach (Node adjacent in grid.GetAdjacent(activeNode))
{
if (!adjacent.inBounds || closedNodes.Contains(adjacent))
{
continue;
}
int movementCost = activeNode.gCost + FindHCost(activeNode, adjacent);
if (movementCost < adjacent.gCost || !openNodes.Contains(adjacent))
{
adjacent.gCost = movementCost;
adjacent.hCost = FindHCost(adjacent, targetNode);
adjacent.parentNode = activeNode;
if (!openNodes.Contains(adjacent))
{
openNodes.Add(adjacent);
}
}
}
}
}
