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