private void FindPath(Vector3 startPos, Vector3 targetPos)
{
Node startNode = grid.NodeFormWolrdPoint(startPos);
Node targetNode = grid.NodeFormWolrdPoint(targetPos);
//Debug.Log (new Vector2(startNode.GetGridX(), startNode.GetGridY()));
List <Node> openSet = new List <Node> ();
List <Node> closedSet = new List <Node> ();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet [0];
for (int i = 1; i < openSet.Count; i++)
{
if (openSet[i].GetfCost() < currentNode.GetfCost() || (openSet[i].GetfCost() == currentNode.GetfCost() && openSet[i].GethCost() < currentNode.GethCost()))
{
currentNode = openSet [i];
}
}
openSet.Remove(currentNode);
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
RetracePath(startNode, targetNode);
return;
}
foreach (Node neighbor in grid.GetNeighbors(currentNode))
{
if ((neighbor.GetWalkable() == false) || closedSet.Contains(neighbor))
{
continue;
}
int newMovementCostToNeighbor = currentNode.GetgCost() + GetDistance(currentNode, neighbor);
if (newMovementCostToNeighbor < neighbor.GetgCost() || !openSet.Contains(neighbor))
{
neighbor.SetgCost(newMovementCostToNeighbor);
neighbor.SethCost(GetDistance(neighbor, targetNode));
neighbor.SetParent(currentNode);
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
}
}
}
}
