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