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