protected Step astar(IntVec3 start)
{
if (stepCache == null)
{
stepCache = new StepFactory();
}
stepCache.Reset(dest, costPerMoveCardinal, costPerMoveDiagonal, Math.Max(Math.Abs(start.x - dest.Cell.x), Math.Abs(start.z - dest.Cell.z)) > maxDistanceBeforeCheating, mapSizeX, mapSizeZ);
openlist = openlistShared;
if (openlist == null)
{
openlist = new Priority_Queue.FastPriorityQueue <Step>(limitForSearch + 10);
openlistShared = openlist;
}
else
{
openlist.Clear();
}
List <int> destCells = CalculateAllowedDestCells(map, dest, peMode, traverseParms);
// Initialisierung der Open List, die Closed List ist noch leer
// (die Priorität bzw. der f Wert des Startknotens ist unerheblich)
Step firstStep = stepCache.getCachedOrNewStep(start, cellIndices.CellToIndex(start));
openlist.Enqueue(firstStep, 0);
// diese Schleife wird durchlaufen bis entweder
// - die optimale Lösung gefunden wurde oder
// - feststeht, dass keine Lösung existiert
while (openlist.Count != 0)
{
// Knoten mit dem geringsten f Wert aus der Open List entfernen
Step currentStep = openlist.Dequeue();
// Wurde das Ziel gefunden?
if (destCells.Contains(currentStep.currentIndx))
{
return(currentStep);
}
if (stepCache.Count >= limitForSearch)
{
return(null);
}
// Wenn das Ziel noch nicht gefunden wurde: Nachfolgeknoten
// des aktuellen Knotens auf die Open List setzen
expandNode(currentStep);
// der aktuelle Knoten ist nun abschließend untersucht
currentStep.closed = true;
//if ( drawPath ) map.debugDrawer.FlashCell(currentStep.current, 0.9f, "closed", 100);
}
// die Open List ist leer, es existiert kein Pfad zum Ziel
return(null);
}
public void Enqueue(RawMsg msg)
{
lock (this)
{
_raws.Enqueue(new Node(msg), 0);
}
}
public TrailblazerPather_HAStar(PathfindData pathfindData) : base(pathfindData)
{
regionGrid = pathfindData.map.regionGrid;
int regions = regionGrid.AllRegions.Count();
// Worst case scenario - a large region where every single edge cell links to a different neighboring region
maxLinks = Region.GridSize * 4 * regions;
rraOpenSet = new Priority_Queue.FastPriorityQueue <LinkNode>(maxLinks);
rraClosedSet = new Dictionary <LinkNode, int>();
destRegions.AddRange(from cell in pathfindData.DestRect.Cells
let region = regionGrid.GetValidRegionAt(cell)
where region != null
select region);
// Initialize the RRA* algorithm
LinkNode.ClearCaches();
IEnumerable <LinkNode> initialNodes = (from region in destRegions
from link in region.links
from node in LinkNode.Both(link)
select node).Distinct();
foreach (LinkNode node in initialNodes)
{
rraClosedSet[node] = RRAHeuristic(node, destCell);
rraOpenSet.Enqueue(node, rraClosedSet[node]);
}
}
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);
}
// minMargin is the number of squares you must be away from the target
// maxMargin is the number of squared you may be away from the target
// so if minMargin is 3 and if maxMargin is 5,
// your destination may then be anywhere between 3-5 squares
public Task <Result> CalculatePath(Vector3Int start, Vector3Int end, int minMargin)
{
Stopwatch totalSw = Stopwatch.StartNew();
Result result = new Result();
minMargin = minMargin * minMargin + minMargin * minMargin + minMargin * minMargin; // convert from tiles to squared distance
if (!GridMap.Instance.IsPosInMap(start))
{
// explicitly don't care if our own block is passable, it just have to exist.
// Will help if unit for some reason gets stuck inside a block
result.failReason = FailReason.InvalidStartPosition;
result.executionTime = totalSw.ElapsedMilliseconds;
return(Task.FromResult(result));
}
if (minMargin == 0 && !GridMap.Instance.IsPosFree(end)) // with a higher margin it's messier to check if end is valid
{
result.failReason = FailReason.InvalidEndPosition;
result.executionTime = totalSw.ElapsedMilliseconds;
return(Task.FromResult(result));
}
if (start == end)
{
result.foundPath = true;
result.executionTime = totalSw.ElapsedMilliseconds;
return(Task.FromResult(result));
}
const int maxEntries = 100000;
Priority_Queue.FastPriorityQueue <AstarNode> nodeQueue = new Priority_Queue.FastPriorityQueue <AstarNode>(maxEntries);
AstarNode currentNode = new AstarNode(start, 0, 0, null);
currentNode.GeneratePriority(end);
nodeQueue.Enqueue(currentNode, 0);
Dictionary <Vector3Int, int> mapWeights = new Dictionary <Vector3Int, int>();
int steps = 0;
const int maxSteps = 10000;
while (nodeQueue.Count > 0)
{
steps++;
if (steps > maxSteps)
{
result.failReason = FailReason.Timeout;
result.executionTime = totalSw.ElapsedMilliseconds;
return(Task.FromResult(result));
}
currentNode = nodeQueue.Dequeue();
Vector3Int currentPos = currentNode.GetPos();
if (currentNode.GetRemainingDistanceSquared() <= minMargin)
{
Unravel(result, currentNode);
result.executionTime = totalSw.ElapsedMilliseconds;
return(Task.FromResult(result));
}
else
{
foreach (var delta in DeltaPositions.DeltaPositions3D)
{
Vector3Int newPos = currentPos + delta;
bool validStep = IsStepValid(newPos, currentPos, delta);
if (validStep)
{
AstarNode nextNode = new AstarNode(newPos, currentNode, end);
int weight;
bool weightExists = mapWeights.TryGetValue(newPos, out weight);
if (!weightExists || nextNode.GetPriority() < weight)
{
weight = nextNode.GetPriority();
mapWeights[newPos] = weight;
if (nodeQueue.Count < maxEntries)
{
nodeQueue.Enqueue(nextNode, weight);
}
else
{
result.failReason = FailReason.OutOfMemory;
result.executionTime = totalSw.ElapsedMilliseconds;
return(Task.FromResult(result));
}
}
}
}
}
}
result.executionTime = totalSw.ElapsedMilliseconds;
result.failReason = FailReason.NoPossiblePath;
return(Task.FromResult(result));
}