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