public List <PathNode> FindPath(int startX, int startZ, int endX, int endZ) { PathNode startNode = grid.GetGridObject(startX, startZ); PathNode endNode = grid.GetGridObject(endX, endZ); openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth(); x++) { for (int z = 0; z < grid.GetHeight(); z++) { PathNode pathNode = grid.GetGridObject(x, z); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.previousNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistance(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { //Reached final node return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { closedList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistance(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.previousNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistance(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } //Out of Nodes on the openList Debug.LogError("No path found"); return(null); }