public RegionLinkDijkstra(Map map, CellRect destination, IEnumerable <Region> startingRegions, IntVec3 target, TraverseParms parms, NewPathFinder.PawnPathCostSettings pathCosts)
{
this.map = map;
this.regionGrid = RegionPathCostHeuristic.regionGridGet(map.regionGrid);
this.traverseParms = parms;
this.targetCell = target;
this.rootCell = destination.CenterCell;
this.pathCostSettings = pathCosts;
avoidGrid = pathCosts.avoidGrid;
area = pathCosts.area;
nodes_popped = 0;
//if (DebugViewSettings.drawPaths && !NewPathFinder.disableDebugFlash)
//{
// debugPathfinder = new NewPathFinder(map);
//}
//Init the starting region links from the start cell
foreach (var region in startingRegions)
{
var minPathCost = RegionMedianPathCost(region);
foreach (RegionLink current in region.links)
{
var dist = RegionLinkDistance(rootCell, current, minPathCost);
if (distances.ContainsKey(current))
{
if (dist < distances[current])
{
linkTargetCells[current] = GetLinkTargetCell(rootCell, current);
}
dist = Math.Min(distances[current], dist);
}
else
{
linkTargetCells[current] = GetLinkTargetCell(rootCell, current);
}
distances[current] = dist;
}
foreach (var pair in PreciseRegionLinkDistances(region, destination))
{
var current = pair.First;
var dist = Math.Max(pair.Second, distances[current]);
distances[current] = dist;
queue.Push(new RegionLinkQueueEntry(region, current, dist, dist));
}
}
}
//The standard A* search algorithm has been modified to implement the bidirectional pathmax algorithm
//("Inconsistent heuristics in theory and practice" Felner et al.) http://web.cs.du.edu/~sturtevant/papers/incnew.pdf
internal PawnPath FindPathInner(IntVec3 start, LocalTargetInfo dest, TraverseParms traverseParms, PathEndMode peMode, HeuristicMode mode = HeuristicMode.Better)
{
//The initialization is largely unchanged from Core, aside from coding style in some spots
#region initialization
if (DebugSettings.pathThroughWalls)
{
traverseParms.mode = TraverseMode.PassAnything;
}
Pawn pawn = traverseParms.pawn;
bool canPassAnything = traverseParms.mode == TraverseMode.PassAnything;
if (!ValidateFindPathParameters(pawn, start, dest, traverseParms, peMode, canPassAnything))
{
return(PawnPath.NotFound);
}
PfProfilerBeginSample(string.Concat("FindPath for ", pawn, " from ", start, " to ", dest, (!dest.HasThing) ? string.Empty : (" at " + dest.Cell)));
destinationX = dest.Cell.x;
destinationZ = dest.Cell.z;
var cellIndices = this.map.cellIndices;
curIndex = cellIndices.CellToIndex(start);
destinationIndex = cellIndices.CellToIndex(dest.Cell);
if (!dest.HasThing || peMode == PathEndMode.OnCell)
{
destinationRect = CellRect.SingleCell(dest.Cell);
}
else
{
destinationRect = dest.Thing.OccupiedRect();
}
if (peMode == PathEndMode.Touch)
{
destinationRect = destinationRect.ExpandedBy(1);
}
destinationRect = destinationRect.ClipInsideMap(map);
var regions = destinationRect.Cells.Select(c => this.map.regionGrid.GetValidRegionAt_NoRebuild(c)).Where(r => r != null);
//Pretty sure this shouldn't be able to happen...
if (mode == HeuristicMode.Better && !canPassAnything && !regions.Any())
{
mode = HeuristicMode.Vanilla;
Log.Warning("Pathfinding destination not in region, must fall back to vanilla!");
}
destinationIsOneCell = (destinationRect.Width == 1 && destinationRect.Height == 1);
this.pathGridDirect = this.map.pathGrid.pathGrid;
this.edificeGrid = this.map.edificeGrid.InnerArray;
statusOpenValue += 2;
statusClosedValue += 2;
if (statusClosedValue >= 65435)
{
ResetStatuses();
}
if (pawn?.RaceProps.Animal == true)
{
heuristicStrength = 30;
}
else
{
float lengthHorizontal = (start - dest.Cell).LengthHorizontal;
heuristicStrength = (int)Math.Round(HeuristicStrengthHuman_DistanceCurve.Evaluate(lengthHorizontal));
}
closedCellCount = 0;
openList.Clear();
debug_pathFailMessaged = false;
debug_totalOpenListCount = 0;
debug_openCellsPopped = 0;
PawnPathCostSettings pawnPathCosts = GetPawnPathCostSettings(traverseParms.pawn);
moveTicksCardinal = pawnPathCosts.moveTicksCardinal;
moveTicksDiagonal = pawnPathCosts.moveTicksDiagonal;
//Where the magic happens
RegionPathCostHeuristic regionCost = new RegionPathCostHeuristic(map, start, destinationRect, regions, traverseParms, pawnPathCosts);
if (mode == HeuristicMode.Better)
{
if (canPassAnything)
{
//Roughly preserves the Vanilla behavior of increasing path accuracy for shorter paths and slower pawns, though not as smoothly. Only applies to sappers.
heuristicStrength = Math.Max(1, (int)Math.Round(heuristicStrength / (float)moveTicksCardinal));
}
else
{
var totalCostEst = (debug_totalHeuristicCostEstimate = regionCost.GetPathCostToRegion(curIndex)) + (moveTicksCardinal * 50); //Add constant cost so it tries harder on short paths
regionHeuristicWeightReal[1].x = totalCostEst / 2;
regionHeuristicWeightReal[2].x = totalCostEst;
}
regionHeuristicWeight = weightEnabled ? regionHeuristicWeightReal : regionHeuristicWeightNone;
}
else
{
regionHeuristicWeight = regionHeuristicWeightNone;
}
calcGrid[curIndex].knownCost = 0;
calcGrid[curIndex].heuristicCost = 0;
calcGrid[curIndex].parentIndex = curIndex;
calcGrid[curIndex].status = statusOpenValue;
openList.Push(new CostNode(curIndex, 0));
bool shouldCollideWithPawns = false;
if (pawn != null)
{
shouldCollideWithPawns = PawnUtility.ShouldCollideWithPawns(pawn);
}
#endregion
while (true)
{
PfProfilerBeginSample("Open cell pop");
if (openList.Count <= 0)
{
break;
}
debug_openCellsPopped++;
var thisNode = openList.Pop();
curIndex = thisNode.gridIndex;
PfProfilerEndSample();
PfProfilerBeginSample("Open cell");
if (calcGrid[curIndex].status == statusClosedValue)
{
PfProfilerEndSample();
}
else
{
#if DEBUG
calcGrid[curIndex].timesPopped++;
#endif
curIntVec3 = cellIndices.IndexToCell(curIndex);
if (DebugViewSettings.drawPaths && !disableDebugFlash && debug_openCellsPopped < 20000)
{
//draw backpointer
var arrow = GetBackPointerArrow(cellIndices.IndexToCell(calcGrid[curIndex].parentIndex), curIntVec3);
string leading = "";
string trailing = "";
#if DEBUG
switch (calcGrid[curIndex].timesPopped)
{
case 1:
trailing = "\n\n"; // $"\n\n\n{thisNode.totalCostEstimate}({calcGrid[curIndex].knownCost + calcGrid[curIndex].originalHeuristicCost})";
break;
case 2:
trailing = "\n"; break;
case 3: break;
case 4:
leading = "\n"; break;
default:
leading = "\n\n"; break;
}
#endif
DebugFlash(curIntVec3, calcGrid[curIndex].knownCost / 1500f, leading + calcGrid[curIndex].knownCost + " " + arrow + " " + debug_openCellsPopped + trailing);
}
if (curIndex == destinationIndex || (!destinationIsOneCell && destinationRect.Contains(curIntVec3)))
{
PfProfilerEndSample();
PfProfilerBeginSample("Finalize Path");
var ret = FinalizedPath(curIndex);
PfProfilerEndSample();
return(ret);
}
//With reopening closed nodes, this limit can be reached a lot more easily. I've left it as is because it gets users to report bad paths.
if (closedCellCount > 160000)
{
Log.Warning(string.Concat(pawn, " pathing from ", start, " to ", dest, " hit search limit of ", 160000, " cells."));
PfProfilerEndSample();
return(PawnPath.NotFound);
}
PfProfilerEndSample();
PfProfilerBeginSample("Neighbor consideration");
for (int i = 0; i < 8; i++)
{
neighIndexes[i] = -1;
neighX = (ushort)(curIntVec3.x + Directions[i]);
neighZ = (ushort)(curIntVec3.z + Directions[i + 8]);
if (neighX >= mapSizeX || neighZ >= mapSizeZ)
{
continue;
}
switch (i)
{
case 4: //Northeast
if (!pathGridDirect.WalkableExtraFast(curIndex - mapSizeX) || !pathGridDirect.WalkableExtraFast(curIndex + 1))
{
continue;
}
break;
case 5: //Southeast
if (!pathGridDirect.WalkableExtraFast(curIndex + mapSizeX) || !pathGridDirect.WalkableExtraFast(curIndex + 1))
{
continue;
}
break;
case 6: //Southwest
if (!pathGridDirect.WalkableExtraFast(curIndex + mapSizeX) || !pathGridDirect.WalkableExtraFast(curIndex - 1))
{
continue;
}
break;
case 7: //Northwest
if (!pathGridDirect.WalkableExtraFast(curIndex - mapSizeX) || !pathGridDirect.WalkableExtraFast(curIndex - 1))
{
continue;
}
break;
}
neighIndex = cellIndices.CellToIndex(neighX, neighZ);
if ((calcGrid[neighIndex].status != statusClosedValue) && (calcGrid[neighIndex].status != statusOpenValue))
{
if (10000 <= (calcGrid[neighIndex].perceivedPathCost = GetTotalPerceivedPathCost(traverseParms, canPassAnything, shouldCollideWithPawns, pawn, pawnPathCosts)))
{
continue;
}
#if DEBUG
calcGrid[neighIndex].timesPopped = 0;
#endif
#region heuristic
PfProfilerBeginSample("Heuristic");
switch (mode)
{
case HeuristicMode.Vanilla:
h = heuristicStrength * (Math.Abs(neighX - destinationX) + Math.Abs(neighZ - destinationZ));
break;
case HeuristicMode.AdmissableOctile:
{
var dx = Math.Abs(neighX - destinationX);
var dy = Math.Abs(neighZ - destinationZ);
h = moveTicksCardinal * (dx + dy) + (moveTicksDiagonal - 2 * moveTicksCardinal) * Math.Min(dx, dy);
}
break;
case HeuristicMode.Better:
if (canPassAnything)
{
var dx = Math.Abs(neighX - destinationX);
var dy = Math.Abs(neighZ - destinationZ);
h = heuristicStrength * (moveTicksCardinal * (dx + dy) + (moveTicksDiagonal - 2 * moveTicksCardinal) * Math.Min(dx, dy));
}
else
{
h = regionCost.GetPathCostToRegion(neighIndex);
}
break;
}
calcGrid[neighIndex].heuristicCost = h;
#if PATHMAX
calcGrid[neighIndex].originalHeuristicCost = h;
#endif
PfProfilerEndSample();
#endregion
}
if (calcGrid[neighIndex].perceivedPathCost < 10000)
{
neighIndexes[i] = neighIndex;
}
if (mode == HeuristicMode.Better && (calcGrid[neighIndex].status == statusOpenValue &&
Math.Max(i > 3 ? (int)(calcGrid[curIndex].perceivedPathCost * diagonalPerceivedCostWeight) + moveTicksDiagonal : calcGrid[curIndex].perceivedPathCost + moveTicksCardinal, 1) + calcGrid[neighIndex].knownCost < calcGrid[curIndex].knownCost))
{
calcGrid[curIndex].parentIndex = neighIndex;
calcGrid[curIndex].knownCost = Math.Max(i > 3 ? (int)(calcGrid[curIndex].perceivedPathCost * diagonalPerceivedCostWeight) + moveTicksDiagonal : calcGrid[curIndex].perceivedPathCost + moveTicksCardinal, 1) + calcGrid[neighIndex].knownCost;
}
}
#region BPMX Best H
#if PATHMAX
PfProfilerBeginSample("BPMX Best H");
int bestH = calcGrid[curIndex].heuristicCost;
if (mode == HeuristicMode.Better && pathmaxEnabled)
{
for (int i = 0; i < 8; i++)
{
neighIndex = neighIndexes[i];
if (neighIndex < 0)
{
continue;
}
bestH = Math.Max(bestH, calcGrid[neighIndex].heuristicCost - (calcGrid[curIndex].perceivedPathCost + (i > 3 ? moveTicksDiagonal : moveTicksCardinal)));
}
}
//Pathmax Rule 3: set the current node heuristic to the best value of all connected nodes
calcGrid[curIndex].heuristicCost = bestH;
PfProfilerEndSample();
#endif
#endregion
#region Updating open list
for (int i = 0; i < 8; i++)
{
neighIndex = neighIndexes[i];
if (neighIndex < 0)
{
continue;
}
if (calcGrid[neighIndex].status == statusClosedValue && (canPassAnything || mode != HeuristicMode.Better))
{
continue;
}
//When path costs are significantly higher than move costs (e.g. snowy ice, or outside of allowed areas),
//small differences in the weighted heuristic overwhelm the added cost of diagonal movement, so nodes
//can often be visited in unnecessary zig-zags, causing lots of nodes to be reopened later, and weird looking
//paths if they are not revisited. Weighting the diagonal path cost slightly counteracts this behavior, and
//should result in natural looking paths when it does cause suboptimal behavior
var thisDirEdgeCost = (i > 3 ? (int)(calcGrid[neighIndex].perceivedPathCost * diagonalPerceivedCostWeight) + moveTicksDiagonal : calcGrid[neighIndex].perceivedPathCost + moveTicksCardinal);
//var thisDirEdgeCost = calcGrid[neighIndex].perceivedPathCost + (i > 3 ? moveTicksDiagonal : moveTicksCardinal);
//Some mods can result in negative path costs. That works well enough with Vanilla, since it won't revisit closed nodes, but when we do, it's an infinite loop.
thisDirEdgeCost = (ushort)Math.Max(thisDirEdgeCost, 1);
neighCostThroughCur = thisDirEdgeCost + calcGrid[curIndex].knownCost;
#if PATHMAX
//Pathmax Rule 1
int nodeH = (mode == HeuristicMode.Better && pathmaxEnabled) ? Math.Max(calcGrid[neighIndex].heuristicCost, bestH - thisDirEdgeCost) : calcGrid[neighIndex].heuristicCost;
#endif
if (calcGrid[neighIndex].status == statusClosedValue || calcGrid[neighIndex].status == statusOpenValue)
{
#if PATHMAX
bool needsUpdate = false;
#endif
int minReopenGain = 0;
if (calcGrid[neighIndex].status == statusOpenValue)
{
#if PATHMAX
needsUpdate = nodeH > calcGrid[neighIndex].heuristicCost;
#endif
}
else
{ //Don't reopen closed nodes if the path cost difference isn't large enough to justify it; otherwise there can be cascades of revisiting the same nodes over and over for tiny path improvements each time
//Increasing the threshold as more cells get reopened further helps prevent cascades
minReopenGain = moveTicksCardinal + closedCellsReopened / 5;
if (pawnPathCosts.area?[neighIndex] == false)
{
minReopenGain *= 10;
}
}
#if PATHMAX
calcGrid[neighIndex].heuristicCost = nodeH;
#endif
if (!(neighCostThroughCur + minReopenGain < calcGrid[neighIndex].knownCost))
{
#if PATHMAX
if (needsUpdate) //if the heuristic cost was increased for an open node, we need to adjust its spot in the queue
{
var neighCell = cellIndices.IndexToCell(neighIndex);
var edgeCost = Math.Max(calcGrid[neighIndex].parentX != neighCell.x && calcGrid[neighIndex].parentZ != neighCell.z ? (int)(calcGrid[neighIndex].perceivedPathCost * diagonalPercievedCostWeight) + moveTicksDiagonal : calcGrid[neighIndex].perceivedPathCost + moveTicksCardinal, 1);
openList.PushOrUpdate(new CostNode(neighIndex, calcGrid[neighIndex].knownCost - edgeCost
+ (int)Math.Ceiling((edgeCost + nodeH) * regionHeuristicWeight.Evaluate(calcGrid[neighIndex].knownCost))));
}
#endif
continue;
}
if (calcGrid[neighIndex].status == statusClosedValue)
{
closedCellsReopened++;
}
}
//else
//{
// DebugFlash(cellIndices.IndexToCell(neighIndex), 0.2f, $"\n\n{neighCostThroughCur} | {nodeH}\n{calcGrid[curIndex].knownCost + (int)Math.Ceiling((nodeH + thisDirEdgeCost) * regionHeuristicWeight.Evaluate(calcGrid[curIndex].knownCost))}");
//}
calcGrid[neighIndex].parentIndex = curIndex;
calcGrid[neighIndex].knownCost = neighCostThroughCur;
calcGrid[neighIndex].status = statusOpenValue;
#if PATHMAX
calcGrid[neighIndex].heuristicCost = nodeH;
#endif
PfProfilerBeginSample("Push Open");
openList.PushOrUpdate(new CostNode(neighIndex, calcGrid[curIndex].knownCost
+ (int)Math.Ceiling((calcGrid[neighIndex].heuristicCost + thisDirEdgeCost) * regionHeuristicWeight.Evaluate(calcGrid[curIndex].knownCost))));
debug_totalOpenListCount++;
PfProfilerEndSample();
}
#endregion
PfProfilerEndSample();
closedCellCount++;
calcGrid[curIndex].status = statusClosedValue;
}
}
if (!debug_pathFailMessaged)
{
string text = pawn?.CurJob?.ToString() ?? "null";
string text2 = pawn?.Faction?.ToString() ?? "null";
Log.Warning(string.Concat(pawn, " pathing from ", start, " to ", dest, " ran out of cells to process.\nJob:", text, "\nFaction: ", text2));
debug_pathFailMessaged = true;
}
PfProfilerEndSample();
return(PawnPath.NotFound);
}