private void UpdateHeap() { // Remove the current node from the open-list AIGraphNode currentNode = m_openHeap.RemoveTop(); // Add it to the closed-list m_closedList[currentNode.ID] = true; // Add all open links foreach (var link in currentNode.NodeLinks) { if (m_closedList[link.ID]) { continue; } // Stop-gap metric to ignore blocked cells. // TODO: To be replaced with more robust cost logic with the graph update. // TODO: Sorry about this. if (link.Metric == -1 || link.Annotation < m_minAnnotation) { continue; } // So, a better metric than this, yeah? m_openHeap.Insert(link, (m_targetPos - link.NodePosition).magnitude); int index = currentNode.ID; m_parentList[link.ID] = index; } }
public Route FindRoute(AIGraph searchGraph, AIGraphNode start, AIGraphNode end, int minAnnotation) { m_minAnnotation = minAnnotation; Route route = new Route(); m_targetPos = end.NodePosition; m_openHeap.Reset(); int maxIterations = 10000; // Pretty lazy, but C# defaults a bool array to false m_closedList = new bool[searchGraph.Nodes.Count]; m_parentList = new int[searchGraph.Nodes.Count]; // No such excuse for ints... m_openHeap.Insert(start, (end.NodePosition - start.NodePosition).magnitude); int iterationCount = 0; while (m_openHeap.HasItems() && m_openHeap.GetTop().NodePosition != end.NodePosition && iterationCount < maxIterations) { UpdateHeap(); iterationCount++; } if (!m_openHeap.HasItems()) { Debug.Log("No route found"); return(route); } if (iterationCount == maxIterations) { Debug.LogWarning("No route found: Max iterations"); } else { // Unwind the route. int currentID = m_parentList[m_openHeap.GetTop().ID]; route.m_routePoints.Add(end); while (currentID != 0) { AIGraphNode node = searchGraph.Nodes[currentID]; route.m_routePoints.Add(node); currentID = m_parentList[currentID]; } route = TrimRoute(route); route.m_routePoints.Reverse(); } return(route); }
public AIGraphNode AddNode(Vector2 position) { AIGraphNode newNode = new AIGraphNode(); newNode.ID = m_nodes.Count; newNode.NodePosition = position; m_nodes.Add(newNode); return(newNode); }
// Create all the cells of the grid, populate initial blocker counts and link all nodes. public void RebuildGraphs() { m_defaultGraph = new AIGraph(); m_defaultNodes = new AIGraphNode[m_numCellsX][]; m_blockers = new ClearanceBlockers[m_numCellsX][]; // Create the cells for (int x = 0; x < m_numCellsX; x++) { m_defaultNodes[x] = new AIGraphNode[m_numCellsY]; m_blockers[x] = new ClearanceBlockers[m_numCellsY]; for (int y = 0; y < m_numCellsY; y++) { m_defaultNodes[x][y] = m_defaultGraph.AddNode(m_gridStart + (new Vector2(x, y) * m_cellSize)); m_blockers[x][y] = new ClearanceBlockers(); // Iterate the rows down and left, incrementing blocker counts when a blocked cell is encountered. for (int clearance = 1; clearance < MaxClearance; clearance++) { for (int xIndex = 0; xIndex < clearance; xIndex++) { // Check bounds, then blockers if ((x + xIndex) >= m_numCellsX || (y + clearance) >= m_numCellsY || (m_cells[x + xIndex, y + clearance].m_contentsMask & (1 << (int)GridCellContentsType.Wall)) != 0) { m_blockers[x][y].blockerCount[clearance]++; } } for (int yIndex = 0; yIndex < (clearance + 1); yIndex++) { // Check bounds, then blockers if ((x + clearance) >= m_numCellsX || (y + yIndex) >= m_numCellsY || (m_cells[x + clearance, y + yIndex].m_contentsMask & (1 << (int)GridCellContentsType.Wall)) != 0) { m_blockers[x][y].blockerCount[clearance]++; } } } // Re-calculate the annotation for the current cell now that blocker-counts are set. UpdateCellAnnotation(x, y); // If the cell itself is blocked, handle that if ((m_cells[x, y].m_contentsMask & (1 << (int)GridCellContentsType.Wall)) != 0) { m_defaultNodes[x][y].Annotation = 0; m_blockers[x][y].blockerCount[0]++; } } } // Link all the nodes for (int x = 0; x < m_numCellsX; x++) { for (int y = 0; y < m_numCellsY; y++) { if (x > 0) { m_defaultNodes[x][y].NodeLinks.Add(m_defaultNodes[x - 1][y]); } if (x < m_numCellsX - 1) { m_defaultNodes[x][y].NodeLinks.Add(m_defaultNodes[x + 1][y]); } if (y > 0) { m_defaultNodes[x][y].NodeLinks.Add(m_defaultNodes[x][y - 1]); } if (y < m_numCellsY - 1) { m_defaultNodes[x][y].NodeLinks.Add(m_defaultNodes[x][y + 1]); } if (x > 0 && y > 0) { m_defaultNodes[x][y].NodeLinks.Add(m_defaultNodes[x - 1][y - 1]); } if (x < m_numCellsX - 1 && y > 0) { m_defaultNodes[x][y].NodeLinks.Add(m_defaultNodes[x + 1][y - 1]); } if (x > 0 && y < m_numCellsY - 1) { m_defaultNodes[x][y].NodeLinks.Add(m_defaultNodes[x - 1][y + 1]); } if (y < m_numCellsY - 1 && x < m_numCellsX - 1) { m_defaultNodes[x][y].NodeLinks.Add(m_defaultNodes[x + 1][y + 1]); } } } }