private CellRefNode ProfilingDequeue(Priority_Queue.FastPriorityQueue <CellRefNode> queue)
{
ProfilerStart("Dequeue");
CellRefNode node = queue.Dequeue();
ProfilerEnd("Dequeue");
return(node);
}
/// <summary>
/// Converts a CellRef to a CellRefNode. Caches nodes to ensure Contains() and similar methods function
/// properly on the priority queue.
/// </summary>
/// <returns>The node.</returns>
/// <param name="cellRef">Cell reference.</param>
private CellRefNode GetNode(CellRef cellRef)
{
if (!cellRefNodeCache.ContainsKey(cellRef))
{
cellRefNodeCache[cellRef] = new CellRefNode(cellRef);
}
return(cellRefNodeCache[cellRef]);
}
/// <summary>
/// Converts a CellRef to a CellRefNode. Caches nodes to ensure Contains() and similar methods function
/// properly on the priority queue.
/// </summary>
/// <returns>The node.</returns>
/// <param name="cellRef">Cell reference.</param>
private CellRefNode GetNode(CellRef cellRef, Direction direction)
{
if (!cellRefNodeCache.ContainsKey(cellRef))
{
cellRefNodeCache[cellRef] = new CellRefNode(cellRef);
}
cellRefNodeCache[cellRef].enterDirection = direction;
return(cellRefNodeCache[cellRef]);
}
public override PawnPath FindPath()
{
ProfilerStart("Total Time");
// Prime the closed and open sets
closedSet[startCell] = new CellNode
{
knownCost = 0,
heuristicCost = ProfilingHeuristic(startCell),
parent = null
};
ProfilerCount("Closed - Total");
if (pathfindData.CellIsInDestination(startCell))
{
ProfilerListStats();
DebugDrawFinalPath(); //TODO
//debugVisualizer.RegisterReplay(debugReplay);
return(FinalizedPath(startCell));
}
foreach (Direction direction in DirectionUtils.AllDirections)
{
CellRef neighbor = direction.From(startCell);
if (neighbor.InBounds())
{
MoveData moveData = new MoveData(neighbor, direction);
if (!ProfilingMoveIsValid(moveData))
{
continue;
}
int moveCost = ProfilingCalcMoveCost(moveData);
int heuristic = ProfilingHeuristic(neighbor);
ProfilerCount("Open - New");
ProfilerCount("Open - Total");
closedSet[neighbor] = new CellNode
{
knownCost = moveCost,
heuristicCost = heuristic,
parent = startCell
};
ProfilingEnqueue(openSet, GetNode(direction.From(startCell), direction), moveCost + heuristic);
}
}
// Main A* Loop
int closedNodes = 0;
while (openSet.Count > 0)
{
CellRefNode current = ProfilingDequeue(openSet);
debugReplay.DrawCell(current);
debugReplay.NextFrame();
ProfilerCount("Closed - Total");
// Check if we've reached our goal
if (pathfindData.CellIsInDestination(current))
{
ProfilerListStats();
DebugDrawFinalPath();//TODO
//debugVisualizer.RegisterReplay(debugReplay);
return(FinalizedPath(current));
}
// Check if we hit the searchLimit
if (closedNodes > SearchLimit)
{
Log.Warning(string.Format("[Trailblazer] {0} pathing from {1} to {2} hit search limit of {3} cells.",
pathfindData.traverseParms.pawn, startCell, destCell, SearchLimit));
ProfilerListStats();
DebugDrawFinalPath(); //TODO
//debugVisualizer.RegisterReplay(debugReplay);
return(PawnPath.NotFound);
}
foreach (Direction direction in DirectionUtils.AllBut(current.enterDirection.Opposite()))
{
CellRef neighbor = direction.From(current);
if (neighbor.InBounds())
{
//debugReplay.DrawLine(current, neighbor);
//debugReplay.NextFrame();
ProfilerCount("Moves - Total");
MoveData moveData = new MoveData(neighbor, direction);
if (!ProfilingMoveIsValid(moveData))
{
ProfilerCount("Moves - Invalid");
continue;
}
ProfilerCount("Moves - Valid");
int neighborNewCost = closedSet[current].knownCost + ProfilingCalcMoveCost(moveData);
if (!closedSet.ContainsKey(neighbor) || closedSet[neighbor].knownCost > neighborNewCost)
{
if (!closedSet.ContainsKey(neighbor))
{
closedSet[neighbor] = new CellNode
{
heuristicCost = ProfilingHeuristic(neighbor)
};
ProfilerCount("Open - New");
}
else
{
ProfilerCount("Open - Reopened");
}
closedSet[neighbor].knownCost = neighborNewCost;
closedSet[neighbor].parent = current;
ProfilingEnqueue(openSet, GetNode(neighbor, direction), closedSet[neighbor].TotalCost);
ProfilerCount("Open - Total");
ProfilerMax("Open Set Max", openSet.Count);
}
else
{
ProfilerCount("Rescanned Nodes");
}
}
}
closedNodes++;
}
Pawn pawn = pathfindData.traverseParms.pawn;
string currentJob = pawn?.CurJob?.ToString() ?? "null";
string faction = pawn?.Faction?.ToString() ?? "null";
Log.Warning(string.Format("[Trailblazer] {0} pathing from {1} to {2} ran out of cells to process.\n" +
"Job: {3}\nFaction: {4}", pawn, startCell, destCell, currentJob, faction));
ProfilerListStats();
DebugDrawFinalPath();//TODO
//debugVisualizer.RegisterReplay(debugReplay);
return(PawnPath.NotFound);
}
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 to the given target.
/// This variant of RRA* paths on the same grid as the main A* pather but only uses a subset of rules
/// </summary>
/// <returns>The region link closest to the target cell</returns>
/// <param name="targetCell">Target cell.</param>
private void ReverseResumableAStar(CellRef targetCell)
{
ProfilerStart("RRA");
ProfilerStart("RRA Reprioritize");
// Rebuild the open set based on the new target
CellRefNode[] cachedNodes = rraOpenSet.ToArray(); // Cache the nodes because we'll be messing with the queue
foreach (CellRefNode cell in cachedNodes)
{
rraOpenSet.UpdatePriority(cell, rraClosedSet[cell] + RRAHeuristic(cell, targetCell));
}
ProfilerEnd("RRA Reprioritize");
int closedNodes = 0;
while (rraOpenSet.Count > 0)
{
CellRef current = rraOpenSet.Dequeue();
debugReplay.DrawCell(current);
ProfilerCount("RRA Closed");
// Check if we've reached our goal
if (current.Equals(targetCell))
{
ProfilerEnd("RRA");
return;
}
if (closedNodes > SearchLimit)
{
Log.Error("[Trailblazer] RRA* Heuristic closed too many cells, aborting");
ProfilerEnd("RRA");
return;
}
foreach (Direction direction in DirectionUtils.AllDirections)
{
IntVec3 neighborCell = direction.From(current);
CellRef neighbor = map.GetCellRef(neighborCell);
ProfilerStart("RRA Bounds Check");
bool inBounds = neighborCell.InBounds(map);
ProfilerEnd("RRA Bounds Check");
if (inBounds)
{
MoveData moveData = new MoveData(neighbor, direction);
ProfilerStart("RRA Move Check");
bool passable = rraPathfinderGrid.MoveIsValid(moveData);
ProfilerEnd("RRA Move Check");
if (!passable)
{
continue;
}
ProfilerStart("RRA Move Cost");
int newCost = rraClosedSet[current] + rraPathfinderGrid.MoveCost(moveData);
ProfilerEnd("RRA Move Cost");
if (!rraClosedSet.ContainsKey(neighbor) || newCost < rraClosedSet[neighbor])
{
if (rraClosedSet.ContainsKey(neighbor))
{
ProfilerCount("RRA Reopened");
}
else
{
ProfilerCount("RRA New Open");
}
rraClosedSet[neighbor] = newCost;
int estimatedCost = newCost + RRAHeuristic(neighbor, targetCell);
ProfilerStart("RRA Enqueue");
CellRefNode neighborNode = GetNode(neighbor);
if (rraOpenSet.Contains(neighborNode))
{
rraOpenSet.UpdatePriority(neighborNode, estimatedCost);
}
else
{
rraOpenSet.Enqueue(neighborNode, estimatedCost);
}
ProfilerEnd("RRA Enqueue");
}
else
{
ProfilerCount("RRA Rescanned");
}
}
}
debugReplay.NextFrame();
closedNodes++;
}
Log.Error("[Trailblazer] RRA heuristic failed to reach target cell " + targetCell);
ProfilerEnd("RRA");
}