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)); }