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 CellRefNode ProfilingDequeue(Priority_Queue.FastPriorityQueue <CellRefNode> queue)
{
ProfilerStart("Dequeue");
CellRefNode node = queue.Dequeue();
ProfilerEnd("Dequeue");
return(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");
}
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 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
}
public FastQueue()
{
_raws = new Priority_Queue.FastPriorityQueue <Node>(MAX_USERS_IN_QUEUE);
}
// 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));
}
