/// <summary>
/// Dijkstra's Algorithm pathfinding along track junctions
/// </summary>
/// <param name="start">Starting track junction</param>
/// <param name="destination">Destination track junction</param>
/// <returns>A stack containing the junctions the cart must visit to reach the destination</returns>
public Stack <FreightTrackJunction> RouteFind(FreightTrackJunction start, FreightTrackJunction destination)
{
//Address the trivial case
if (start == null || destination == null || start == destination)
{
return(new Stack <FreightTrackJunction>());
}
//Use a local copy of the junction list to avoid potential nasty consequences should the network change
Stack <FreightTrackJunction> CartRoute = new Stack <FreightTrackJunction>();
lock (safety)
{
List <FreightTrackJunction> localjunctions = this.TrackJunctions;
if (!localjunctions.Contains(start) || !localjunctions.Contains(destination))
{
Vector3 vec = WorldScript.instance.mPlayerFrustrum.GetCoordsToUnity(start.mnX, start.mnY, start.mnZ);
CubeCoord coordstart = new CubeCoord((long)vec.x, (long)vec.y, (long)vec.z);
Vector3 vec2 = WorldScript.instance.mPlayerFrustrum.GetCoordsToUnity(start.mnX, start.mnY, start.mnZ);
CubeCoord coorddest = new CubeCoord((long)vec2.x, (long)vec2.y, (long)vec2.z);
Debug.LogWarning("FreightTrackNetwork RouteFind attempted to rount to/from inaccessible junction\n start id " + start.JunctionID + " with coords: [" + coordstart.ToString() + "] destination id " + destination.JunctionID + " with coords [" + coorddest.ToString() + "] trackjunctions count: " + localjunctions.Count);
return(CartRoute);
}
int junctioncount = localjunctions.Count;
//Initialize priority queue with only 0 distance for the start
SimplePriorityQueue <FreightTrackJunction> tentativedistances = new SimplePriorityQueue <FreightTrackJunction>();
for (int n = 0; n < junctioncount; n++)
{
tentativedistances.Enqueue(localjunctions[n], float.MaxValue);
}
tentativedistances.UpdatePriority(start, 0);
//Initialize a separate array of distances for pulling the tentative distances, initialize the start to 0
int[] distances = Enumerable.Repeat(int.MaxValue, junctioncount).ToArray();
int currentnode = start.JunctionIndex;
if (currentnode >= distances.Length)
{
Debug.LogWarning("FreightTrackNetwork RouteFind trying to navigate from junction outside of range");
this.ResetJunctionIndices();
return(new Stack <FreightTrackJunction>());
}
distances[currentnode] = 0;
//Initialize an array for storing the parent junction for the path finding to later trace back to the beginning, indexed to JunctionIndex
//Start node previous is marked -1 to flag completion of the route trace
int[] previousnode = new int[junctioncount];
previousnode[currentnode] = -1;
//Initialize variables used in the core algorithm
int neighborindex; //Index of neighbor junction as found in JunctionIndex
int initialdis; //Distance to neighbor from start (tentative)
int currentdis; //Distance to active junction
int tryroute; //Distance to route to neighbor through active junction
FreightTrackJunction freightnode = tentativedistances.Dequeue();
FreightTrackJunction neighbor;
//Debug.LogWarning("FreightTrackNetwork RouteFind DEBUG - prior to loop");
for (int m = 0; m < junctioncount; m++)
{
for (int n = 0; n < 4; n++)
{
neighbor = freightnode.ConnectedJunctions[n];
//Skip the neighbor if it is null or previously visited
if (neighbor == null || !tentativedistances.Contains(neighbor))
{
continue;
}
neighborindex = neighbor.JunctionIndex;
if (neighborindex >= distances.Length || currentnode >= distances.Length)
{
Debug.LogWarning("FreightTrackNetwork RouteFind checking neighbor junction outside of range");
this.ResetJunctionIndices();
return(new Stack <FreightTrackJunction>());
}
initialdis = distances[neighborindex];
currentdis = distances[currentnode];
tryroute = currentdis + neighbor.ConnectedSegments[n].Length;
//If the newly calculated distance to the neighbor is shorter than the previous replace it and update pathing accordingly
if (tryroute < initialdis)
{
distances[neighborindex] = tryroute;
tentativedistances.UpdatePriority(neighbor, tryroute);
previousnode[neighborindex] = currentnode;
}
}
//I feel like this check should come after dequeue... if the dequeue is the destination is there really any value in calculating for all its neighbors?
//GASP! Wikipedia might be wrong!!?
if (!tentativedistances.Contains(destination))
{
break;
}
freightnode = tentativedistances.Dequeue();
currentnode = freightnode.JunctionIndex;
}
//Debug.LogWarning("FreightTrackNetwork RouteFind DEBUG - after loop");
if (freightnode != destination)
{
Debug.LogWarning("FreightTrackNetwork RouteFind completed path finding with junction other than the destination?");
return(new Stack <FreightTrackJunction>());
}
if (currentnode >= previousnode.Length)
{
Debug.LogWarning("FreightTrackNetwork RouteFind trying to trace back previousnode out of range");
this.ResetJunctionIndices();
return(new Stack <FreightTrackJunction>());
}
//Push the destination onto the route stack and get the first previous junction
CartRoute.Push(freightnode);
int prevnode = previousnode[currentnode];
int safetycount = 0;
//Trace back to the start by each of the previous junctions anding with the starting junction
while (prevnode != -1)
{
freightnode = localjunctions[prevnode];
CartRoute.Push(freightnode);
prevnode = previousnode[prevnode];
if (prevnode >= previousnode.Length)
{
Debug.LogWarning("FreightTrackNetwork RouteFind trying to trace back previousnode out of range on path");
this.ResetJunctionIndices();
return(new Stack <FreightTrackJunction>());
}
safetycount++;
if (safetycount > 255)
{
Debug.LogWarning("FreightTrackNetwork RouteFind failed construction or path has over 255 visited junctions! Network junction count: " + this.TrackJunctions.Count);
CartRoute = null;
return(new Stack <FreightTrackJunction>());
}
}
//The above loop automatically pushes the start junction and since the cart is assumed to be there we'll just toss it out as part of a sanity check
if (CartRoute.Pop() != start)
{
Debug.LogWarning("FreightTrackNetwork RouteFind returned a path that didn't start with original start junction!");
return(new Stack <FreightTrackJunction>());
}
}
return(CartRoute);
}
public override string ToString()
{
return(cubeCoords.ToString());
}