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