/// <summary> /// A* search /// </summary> /// <param name="allowReverse"></param> /// <param name="carsToIgnore"></param> /// <param name="consistLength"></param> protected async System.Threading.Tasks.Task Astar(bool allowReverse, HashSet <string> carsToIgnore, double consistLength, List <TrackTransition> bannedTransitions) { await Await.BackgroundSyncContext(); cameFrom = new Dictionary <RailTrack, RailTrack>(); costSoFar = new Dictionary <RailTrack, double>(); //var queue = new PriorityQueue<RailTrack>(); var queue = new GenericPriorityQueue <RailTrackNode, double>(10000); queue.Enqueue(new RailTrackNode(start), 0.0); cameFrom.Add(start, start); costSoFar.Add(start, 0.0); RailTrack current = null; while (queue.Count > 0) { current = queue.Dequeue().track; RailTrack prev = null; cameFrom.TryGetValue(current, out prev); string debug = $"ID: {current.logicTrack.ID.FullID} Prev: {prev?.logicTrack.ID.FullID}"; List <RailTrack> neighbors = new List <RailTrack>(); if (current.outIsConnected) { neighbors.AddRange(current.GetAllOutBranches().Select(b => b.track)); } if (current.inIsConnected) { neighbors.AddRange(current.GetAllInBranches().Select(b => b.track)); } string branches = DumpNodes(neighbors, current); debug += "\n" + $"all branches: {branches}"; #if DEBUG2 Terminal.Log(debug); #endif foreach (var neighbor in neighbors) { if (bannedTransitions != null && bannedTransitions.All(t => t.track == current && t.nextTrack == neighbor)) { Terminal.Log($"{current.logicTrack.ID.FullID}->{neighbor.logicTrack.ID.FullID} banned"); continue; } //if non start/end track is not free omit it if (neighbor != start && neighbor != goal && !neighbor.logicTrack.IsFree(carsToIgnore)) { Terminal.Log($"{neighbor.logicTrack.ID.FullID} not free"); continue; } //if we could go through junction directly (without reversing) bool isDirect = current.CanGoToDirectly(prev, neighbor); if (!allowReverse && !isDirect) { Terminal.Log($"{neighbor.logicTrack.ID.FullID} reverse needed"); continue; } // compute exact cost //double newCost = costSoFar[current] + neighbor.logicTrack.length; double newCost = costSoFar[current] + neighbor.logicTrack.length / neighbor.GetAverageSpeed(); if (!isDirect) { // if we can't fit consist on this track to reverse, drop this neighbor if (prev != null && !current.IsDirectLengthEnough(prev, consistLength)) { Terminal.Log($"{neighbor.logicTrack.ID.FullID} not long enough to reverse"); continue; } //add penalty when we must reverse //newCost += 2.0 * consistLength + 30.0; } // If there's no cost assigned to the neighbor yet, or if the new // cost is lower than the assigned one, add newCost for this neighbor if (!costSoFar.ContainsKey(neighbor) || newCost < costSoFar[neighbor]) { // If we're replacing the previous cost, remove it if (costSoFar.ContainsKey(neighbor)) { costSoFar.Remove(neighbor); cameFrom.Remove(neighbor); } //Terminal.Log($"neighbor {neighbor.logicTrack.ID.FullID} update {newCost}"); costSoFar.Add(neighbor, newCost); cameFrom.Add(neighbor, current); double priority = newCost + Heuristic(neighbor, goal) / 20.0f; //convert distance to time (t = s / v) queue.Enqueue(new RailTrackNode(neighbor), priority); } } } await Await.UnitySyncContext(); }