public static Vector3?FindPath(AGrid grid, Node startNode, Node targetNode, int distance = 1) { SortedList <Node, Node> openSet = new SortedList <Node, Node>(); openSet.Add(startNode, startNode); while (openSet.Count > 0) { Node currentNode = openSet.Values[0]; currentNode.SearchState = SearchState.Closed; openSet.Remove(currentNode); if (currentNode != startNode && !currentNode.isByPassable) { return(null); } if (getDistance(currentNode, targetNode) <= distance) { return(RetracePath(startNode, currentNode)); } foreach (Node neighbour in grid.getNeighbours(currentNode)) { if (neighbour.SearchState == SearchState.Closed) { continue; } int newMovementCostToNeighbour = currentNode.gCost + getDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || neighbour.SearchState != SearchState.Open) { if (neighbour.SearchState == SearchState.Open) { openSet.Remove(neighbour); } else { neighbour.SearchState = SearchState.Open; } neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = getDistance(neighbour, targetNode); neighbour.Parent = currentNode; openSet.Add(neighbour, neighbour); } } } return(null); }
//For characters local level navigation public static Vector3?FindPath(TileBehaviour[,] tiles, Vector3 startPosition, Vector3 targetPosition, Vector3 backward) { startPosition.x = (int)startPosition.x; startPosition.y = (int)startPosition.y; startPosition.z = (int)startPosition.z; targetPosition.x = (int)targetPosition.x; targetPosition.y = (int)targetPosition.y; targetPosition.z = (int)targetPosition.z; backward.x = (int)backward.x; backward.y = (int)backward.y; backward.z = (int)backward.z; AGrid grid = new AGrid(tiles, backward); Node startNode = grid.getNode(startPosition, true); Node targetNode = grid.getNode(targetPosition, true); return(FindPath(grid, startNode, targetNode, 0)); }