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); } } } } }