// Get a snapshot of the final path public void FinalPathSnapshot(NodeGrid <Node> grid, List <Node> path) { GridSnapshotAction gridSnapshotAction = new GridSnapshotAction(); gridSnapshotAction.AddAction(HideVisuals); for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { for (int z = 0; z < grid.GetDepth(); z++) { Node node = grid.GetGridObject(x, y, z); int g = node.g; int h = node.h; int f = node.f; Vector3 gridPos = new Vector3(node.x, node.y, node.z) * grid.GetNodeSize() + Vector3.one * grid.GetNodeSize() * 5f; bool isInPath = path.Contains(node); int tmpX = x; int tmpY = y; int tmpZ = z; gridSnapshotAction.AddAction(() => { Transform visualNode = visualNodeArray[tmpX, tmpY, tmpZ]; SetupVisualNode(visualNode, g, h, f); Material material; if (isInPath) { material = nodeSearching; } else { material = nodeDefault; } visualNode.GetComponent <MeshRenderer>().material = material; }); } } } gridSnapshotActions.Add(gridSnapshotAction); }
// Draw walls on unwalkable nodes void DrawWalls() { for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { for (int z = 0; z < grid.GetDepth(); z++) { Node node = grid.GetGridObject(x, y, z); // If the node is not walkable draw a wall if (!node.isWalkable) { Object.Instantiate(wallObject, grid.GetPosition(x, y, z) + new Vector3(5, 5, 5), new Quaternion()); } } } } }
// Find path between start and end coordinates public List <Node> FindPath(int startX, int startY, int startZ, int endX, int endY, int endZ) { startNode = grid.GetGridObject(startX, startY, startZ); Node endNode = grid.GetGridObject(endX, endY, endZ); closedList = new List <Node>(); openList = new List <Node> { startNode }; // Calculate g and f cost for all nodes for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { for (int z = 0; z < grid.GetDepth(); z++) { Node node = grid.GetGridObject(x, y, z); // Make g cost infinite node.g = int.MaxValue; node.GetF(); // Clear data from previous path node.lastNode = null; } } } startNode.g = 0; startNode.h = GetDistance(startNode, endNode); startNode.GetF(); StepVisual.Instance.ClearSnapshots(); StepVisual.Instance.TakeSnapshot(grid, startNode, openList, closedList); while (openList.Count > 0) { // Get the node with the least f cost Node current = GetCheapestNode(openList); if (current == endNode) { StepVisual.Instance.TakeSnapshot(grid, current, openList, closedList); StepVisual.Instance.FinalPathSnapshot(grid, GetPath(endNode)); return(GetPath(endNode)); } foreach (Node neighbor in GetNeighbors(current)) { if (closedList.Contains(neighbor)) { continue; } // If node is weighted, add move penalty if (neighbor.isWeighted) { Debug.Log("neighbor is weighted"); neighbor.g += 10; } // Only add to neighbor list if node is walkable if (!neighbor.isWalkable) { closedList.Add(neighbor); continue; } neighbor.lastNode = current; neighbor.h = GetDistance(neighbor, endNode); int tentativeG = current.g + GetDistance(current, neighbor); if (neighbor.isWeighted) { tentativeG += 10; } if (!openList.Contains(neighbor)) { neighbor.lastNode = current; neighbor.g = tentativeG; neighbor.h = GetDistance(neighbor, endNode); neighbor.GetF(); openList.Add(neighbor); } StepVisual.Instance.TakeSnapshot(grid, current, openList, closedList); } openList.Remove(current); closedList.Add(current); } return(null); }
void SetupBFSearch() { bFSearch = new BFSearch(width, height, depth, nodeSize, origin); bFSearchGrid = bFSearch.GetGrid(); // Add walls manually bFSearch.GetNode(2, 2, 0).SetWalkable(!bFSearch.GetNode(2, 2, 0).isWalkable); bFSearch.GetNode(2, 3, 0).SetWalkable(!bFSearch.GetNode(2, 3, 0).isWalkable); bFSearch.GetNode(2, 4, 0).SetWalkable(!bFSearch.GetNode(2, 4, 0).isWalkable); bFSearch.GetNode(3, 4, 0).SetWalkable(!bFSearch.GetNode(3, 4, 0).isWalkable); bFSearch.GetNode(4, 4, 0).SetWalkable(!bFSearch.GetNode(4, 4, 0).isWalkable); bFSearch.GetNode(5, 4, 0).SetWalkable(!bFSearch.GetNode(5, 4, 0).isWalkable); bFSearch.GetNode(6, 4, 0).SetWalkable(!bFSearch.GetNode(6, 4, 0).isWalkable); bFSearch.GetNode(6, 3, 0).SetWalkable(!bFSearch.GetNode(6, 3, 0).isWalkable); bFSearch.GetNode(6, 2, 0).SetWalkable(!bFSearch.GetNode(6, 2, 0).isWalkable); bFSearch.GetNode(1, 9, 0).SetWalkable(!bFSearch.GetNode(1, 9, 0).isWalkable); bFSearch.GetNode(1, 8, 0).SetWalkable(!bFSearch.GetNode(1, 8, 0).isWalkable); bFSearch.GetNode(3, 7, 0).SetWalkable(!bFSearch.GetNode(3, 7, 0).isWalkable); bFSearch.GetNode(4, 7, 0).SetWalkable(!bFSearch.GetNode(4, 7, 0).isWalkable); bFSearch.GetNode(5, 7, 0).SetWalkable(!bFSearch.GetNode(5, 7, 0).isWalkable); bFSearch.GetNode(6, 7, 0).SetWalkable(!bFSearch.GetNode(6, 7, 0).isWalkable); bFSearch.GetNode(7, 7, 0).SetWalkable(!bFSearch.GetNode(7, 7, 0).isWalkable); bFSearch.GetNode(8, 7, 0).SetWalkable(!bFSearch.GetNode(8, 7, 0).isWalkable); // Add weighted tiles manually bFSearch.GetNode(0, 4, 0).SetWeighted(!bFSearch.GetNode(0, 4, 0).isWeighted); bFSearch.GetNode(1, 4, 0).SetWeighted(!bFSearch.GetNode(1, 4, 0).isWeighted); // Draw walls for (int x = 0; x < bFSearchGrid.GetWidth(); x++) { for (int y = 0; y < bFSearchGrid.GetHeight(); y++) { for (int z = 0; z < bFSearchGrid.GetDepth(); z++) { Node node = bFSearchGrid.GetGridObject(x, y, z); // If the node is not walkable draw a wall if (!node.isWalkable) { Object.Instantiate(wallObject, bFSearchGrid.GetPosition(x, y, z) + new Vector3(5, 5, 5), new Quaternion()); } if (node.isWeighted) { Object.Instantiate(weightedObject, bFSearchGrid.GetPosition(x, y, z) + new Vector3(5, 5, 5), new Quaternion()); } } } } // Create visual nodes stepVisual.Setup(bFSearchGrid); // Show goal node Object.Instantiate(goalObject, bFSearchGrid.GetPosition(goalX, goalY, goalZ) + new Vector3(5, 5, 5), new Quaternion()); Object.Instantiate(goalObject, bFSearchGrid.GetPosition(startX, startY, startZ) + new Vector3(5, 5, 5), new Quaternion()); currentAlgorithm = "bFSearch"; }
// Find path between start and end coordinates public List <Node> FindPath(int startX, int startY, int startZ, int endX, int endY, int endZ) { // Get starting node Node startNode = grid.GetGridObject(startX, startY, startZ); // Get last node Node endNode = grid.GetGridObject(endX, endY, endZ); openList = new List <Node> { startNode }; closedList = new List <Node>(); // Calculate g and f cost for all nodes for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { for (int z = 0; z < grid.GetDepth(); z++) { Node node = grid.GetGridObject(x, y, z); // Make g cost infinite node.g = int.MaxValue; node.GetF(); // Clear data from previous path node.lastNode = null; } } } startNode.g = 0; startNode.h = GetDistance(startNode, endNode); startNode.GetF(); // Get starting snapshot StepVisual.Instance.ClearSnapshots(); StepVisual.Instance.TakeSnapshot(grid, startNode, openList, closedList); // cycle until open list empty while (openList.Count > 0) { // Get the node with the least f cost Node current = GetCheapestNode(openList); // Get path to last node if last node if (current == endNode) { // Take snapshot StepVisual.Instance.TakeSnapshot(grid, current, openList, closedList); StepVisual.Instance.FinalPathSnapshot(grid, GetPath(endNode)); return(GetPath(endNode)); } // Remove current node from open list and put into closed list openList.Remove(current); closedList.Add(current); // Cycle node neighbours foreach (Node neighbor in GetNeighbors(current)) { if (closedList.Contains(neighbor)) { continue; } // If node is weighted, add move penalty if (neighbor.isWeighted) { neighbor.g += 10; } // Only add to neighbor list if node is walkable if (!neighbor.isWalkable) { closedList.Add(neighbor); continue; } int tentativeG = current.g + GetDistance(current, neighbor); if (neighbor.isWeighted) { tentativeG += 10; } // If there is a better path if (tentativeG < neighbor.g) { neighbor.lastNode = current; neighbor.g = tentativeG; neighbor.h = GetDistance(neighbor, endNode); neighbor.GetF(); // If already searched if (!openList.Contains(neighbor)) { openList.Add(neighbor); } } StepVisual.Instance.TakeSnapshot(grid, current, openList, closedList); } } // Out of nodes return(null); }