private void ProfilingEnqueue(Priority_Queue.FastPriorityQueue <CellRefNode> queue, CellRefNode node, int priority)
{
ProfilerStart("Enqueue");
if (queue.Contains(node))
{
queue.UpdatePriority(node, priority);
}
else
{
queue.Enqueue(node, priority);
}
ProfilerEnd("Enqueue");
}
/// <summary>
/// Initiates or resumes RRA* pathfinding on the region grid with the given target.
///
/// The grid is made up of nodes, where each region link is represented by two nodes (either end of the link).
/// All nodes of all links that share a region are considered to share edges, with the cost of the edge
/// being the octaline distance between the two cells.
/// </summary>
/// <returns>The region link closest to the target cell</returns>
/// <param name="targetCell">Target cell.</param>
private LinkNode ReverseResumableAStar(CellRef targetCell)
{
Region targetRegion = regionGrid.GetValidRegionAt_NoRebuild(targetCell);
// Rebuild the open set based on the new target
LinkNode[] cachedNodes = rraOpenSet.ToArray(); // Cache the nodes because we'll be messing with the queue
foreach (LinkNode link in cachedNodes)
{
rraOpenSet.UpdatePriority(link, rraClosedSet[link] + RRAHeuristic(link, targetCell));
}
int closedNodes = 0;
while (rraOpenSet.Count > 0)
{
LinkNode currentNode = rraOpenSet.Dequeue();
//debugReplay.DrawCell(currentNode.GetCell());
// Check if we've reached our goal
if (currentNode.link.regions.Contains(targetRegion))
{
return(currentNode);
}
if (closedNodes > SearchLimit)
{
Log.Error("[Trailblazer] RRA* Heuristic closed too many cells, aborting");
return(null);
}
foreach (LinkNode neighbor in currentNode.Neighbors())
{
//DebugDrawRegionEdge(currentNode, neighbor);
//debugReplay.DrawLine(currentNode.GetCell(), neighbor.GetCell());
int moveCost = DistanceBetween(currentNode, neighbor);
// Penalize the edge if the two links don't share a pathable region
// TODO should we just totally ignore the edge instead?
if (!currentNode.CommonRegions(neighbor).Any(r => r.Allows(pathfindData.traverseParms, false)))
{
moveCost *= 50;
}
int newCost = rraClosedSet[currentNode] + moveCost;
if (!rraClosedSet.ContainsKey(neighbor) || newCost < rraClosedSet[neighbor])
{
rraClosedSet[neighbor] = newCost;
int estimatedCost = newCost + RRAHeuristic(neighbor, targetCell);
if (rraOpenSet.Contains(neighbor))
{
rraOpenSet.UpdatePriority(neighbor, estimatedCost);
}
else
{
rraOpenSet.Enqueue(neighbor, estimatedCost);
}
//DebugDrawRegionNode(neighbor, string.Format("{0} ({1})", newCost, moveCost));
}
}
//debugReplay.NextFrame();
closedNodes++;
}
Log.Error("[Trailblazer] RRA heuristic failed to reach target region " + targetRegion);
return(null);
}