/// <summary>
/// Returns the cell you would reach by moving in the given direction from the start cell
/// </summary>
/// <returns>The ending cell.</returns>
/// <param name="direction">The direction to move.</param>
/// <param name="start">The starting cell.</param>
public static CellRef From(this Direction direction, CellRef start)
{
switch (direction)
{
case Direction.N:
return(start.Relative(0, 1));
case Direction.NE:
return(start.Relative(1, 1));
case Direction.E:
return(start.Relative(1, 0));
case Direction.SE:
return(start.Relative(1, -1));
case Direction.S:
return(start.Relative(0, -1));
case Direction.SW:
return(start.Relative(-1, -1));
case Direction.W:
return(start.Relative(-1, 0));
case Direction.NW:
return(start.Relative(-1, 1));
default:
throw new Exception("Moved in an invalid direction");
}
}
public PathfindData(Map map, CellRef start, LocalTargetInfo dest, TraverseParms traverseParms, PathEndMode pathEndMode)
{
this.map = map;
this.start = start;
this.dest = dest;
this.traverseParms = traverseParms;
this.pathEndMode = pathEndMode;
}
/// <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]);
}
private bool CellIsPassable(CellRef cell)
{
if (!cellPassabilityCache.ContainsKey(cell))
{
cellPassabilityCache[cell] = cellPassabilityRules.All(r => r.IsPassable(cell));
}
return(cellPassabilityCache[cell]);
}
private int ProfilingHeuristic(CellRef cell)
{
ProfilerStart("Heuristic");
int heuristic = Heuristic(cell);
ProfilerEnd("Heuristic");
return(heuristic);
}
private int CellCost(CellRef cell)
{
if (!cellCostCache.ContainsKey(cell))
{
cellCostCache[cell] = cellCostRules.Sum(r => r.GetCost(cell));
}
return(cellCostCache[cell]);
}
protected override int Heuristic(CellRef cell)
{
if (!rraClosedSet.ContainsKey(cell))
{
ReverseResumableAStar(cell);
}
return(rraClosedSet[cell]);
}
private int RRAHeuristic(CellRef start, CellRef target)
{
ProfilerStart("RRA Heuristic");
int dist = DistanceBetween(start, target);
ProfilerEnd("RRA Heuristic");
return(dist);
}
/// <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]);
}
private PawnPath FinalizedPath(CellRef final)
{
PawnPath emptyPawnPath = pathfindData.map.pawnPathPool.GetEmptyPawnPath();
CellRef cell = final;
while (cell != null)
{
emptyPawnPath.AddNode(cell);
cell = closedSet[cell].parent;
}
emptyPawnPath.SetupFound(closedSet[final].knownCost, false);
return(emptyPawnPath);
}
public TrailblazerPather_AStar(PathfindData pathfindData) : base(pathfindData)
{
map = pathfindData.map;
openSet = new Priority_Queue.FastPriorityQueue <CellRefNode>(map.Area);
closedSet = new Dictionary <CellRef, CellNode>();
cellRefNodeCache = new Dictionary <CellRef, CellRefNode>();
startCell = pathfindData.start;
destCell = pathfindData.map.GetCellRef(pathfindData.dest.Cell);
CostRule_MoveTicks.GetMoveTicks(pathfindData, out moveTicksCardinal, out moveTicksDiagonal);
debugMat++;
debugVisualizer = pathfindData.map.GetComponent <TrailblazerDebugVisualizer>();
debugReplay = debugVisualizer.CreateNewReplay();
#if PROFILE
performanceTracker = new PerformanceTracker();
#endif
}
protected override int Heuristic(CellRef cell)
{
Region region = regionGrid.GetValidRegionAt_NoRebuild(cell);
if (destRegions.Contains(region))
{
return(DistanceBetween(cell, destCell));
}
IEnumerable <LinkNode> nodes = from link in region.links
from node in LinkNode.Both(link)
where rraClosedSet.ContainsKey(node)
select node;
if (nodes.EnumerableNullOrEmpty())
{
nodes = ReverseResumableAStar(cell).Yield();
}
return((from node in nodes
let totalCost = rraClosedSet[node] + RRAHeuristic(node, cell)
select totalCost).Min());
}
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);
}
public CellRefNode(CellRef cell)
{
this.cell = cell;
}
/// <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");
}
private int RRAHeuristic(LinkNode link, CellRef target)
{
return(DistanceBetween(link.GetCell(), target));
}
/// <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);
}
public bool CellIsInDestination(CellRef cell)
{
return(DestRect.Contains(cell) && !DisallowedCorners.Contains(cell));
}
