/// <summary>
/// Computes if track without revresing is long enough for given length
/// </summary>
/// <param name="current"></param>
/// <param name="from"></param>
/// <param name="length"></param>
/// <returns></returns>
public static bool IsDirectLengthEnough(this RailTrack current, RailTrack from, double length)
{
if (current.logicTrack.length > length)
{
return(true);
}
bool isFromInJuction = current.inIsConnected && current.GetAllInBranches().Any(b => b.track == from);
bool isFromOutJuction = current.outIsConnected && current.GetAllOutBranches().Any(b => b.track == from);
if (current.inIsConnected && !isFromInJuction)
{
foreach (var branch in current.GetAllInBranches())
{
if (CanGoToDirectly(current, from, branch.track) && IsDirectLengthEnough(branch.track, current, length - current.logicTrack.length))
{
return(true);
}
}
}
if (current.outIsConnected && !isFromOutJuction)
{
foreach (var branch in current.GetAllOutBranches())
{
if (CanGoToDirectly(current, from, branch.track) && IsDirectLengthEnough(branch.track, current, length - current.logicTrack.length))
{
return(true);
}
}
}
return(false);
}
public static bool IsTrackInBranch(this RailTrack current, RailTrack next)
{
if (!current.inIsConnected)
{
return(false);
}
return(current.GetAllInBranches().Any(b => b.track == next));
}
public static bool CanGoToDirectly(this RailTrack current, RailTrack from, RailTrack to, out Junction reversingJunction)
{
reversingJunction = null;
bool isInJuction = current.inIsConnected && current.GetAllInBranches().Any(b => b.track == to);
bool isOutJuction = current.outIsConnected && current.GetAllOutBranches().Any(b => b.track == to);
if (current.inIsConnected)
{
//Terminal.Log($"IN: {from?.logicTrack.ID.FullID} -> {current.logicTrack.ID.FullID} -> {to?.logicTrack.ID.FullID}");
if (isInJuction && CanGoThroughJunctionDirectly(current, current.inJunction, from, to))
{
return(true);
}
}
if (current.outIsConnected)
{
//Terminal.Log($"OUT: {from?.logicTrack.ID.FullID} -> {current.logicTrack.ID.FullID} -> {to?.logicTrack.ID.FullID}");
if (isOutJuction && CanGoThroughJunctionDirectly(current, current.outJunction, from, to))
{
return(true);
}
}
if (isInJuction)
{
reversingJunction = current.inJunction;
}
if (isOutJuction)
{
reversingJunction = current.outJunction;
}
return(false);
}
/// <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();
}