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