private float totaldistance(Waypoint self, WaypointQueueable from) { float distbetween = Vector3.Distance(self.transform.position, from.self.transform.position); //The distance to a node is equal to the total distance of the node it came from plus the distance from that node to the current node return(from.disttostart + distbetween); }
private WaypointQueueable enqueue(FastPriorityQueue <WaypointQueueable> q, Waypoint self, WaypointQueueable from = null) { float dist = 0; if (from != null) { dist = totaldistance(self, from); //the weight to the neighbor self from the waypoint from.self } WaypointQueueable newwayp = new WaypointQueueable() { self = self, from = from, disttostart = dist }; //Debug.Log("Enqueue " + self.hashname + " with distance " + dist); q.Enqueue(newwayp, dist); return(newwayp); }
public Stack <Waypoint> Path(Waypoint start, Waypoint end) { FastPriorityQueue <WaypointQueueable> queue = new FastPriorityQueue <WaypointQueueable>(128); //Debug.Log("Starting path algorithm from " + start.hashname + " to " + end.hashname); //Debug.Log("Enqueue start of " + start.hashname); enqueue(queue, start); List <int> alreadyAddedWaypoints = new List <int>(); alreadyAddedWaypoints.Add(start.WaypointID); WaypointQueueable endqueueable = null; bool endFound = false; while (queue.Count > 0 && !endFound) { if (queue.Count > 127) { Debug.LogError("Catastrophic pathing falure: Queue length exceeded maximum. Aborting loop!"); break; } WaypointQueueable headqueueable = dequeue(queue); Waypoint head = headqueueable.self; //Debug.Log("Dequeue " + head.hashname); //Debug.Log("Number of neighbors: " + head.neighbors.Count); for (int i = 0; i < head.neighbors.Count; i++) { NeighborConnection neighbor = head.neighbors[i];//This cant be foreach because you can't (break;) foreach *grumble grumble* //Debug.Log("Checking neighbor " + neighbor.neighbor.hashname); WaypointQueueable neighborqueueable = null; foreach (WaypointQueueable waypq in queue) { if (waypq.self.WaypointID == neighbor.neighbor.WaypointID) { //Debug.Log("Neighbor already found in queue"); neighborqueueable = waypq; break;//techincally I think this doesnt do anything } } bool addedBefore = false; foreach (int addedID in alreadyAddedWaypoints) // alreadyAddedWaypoints.Contains was not functioning for some reason so I just did it manually { if (addedID == neighbor.neighborID) { addedBefore = true; } } if (neighborqueueable == null && !addedBefore) { //Debug.Log("New neighbor, enqueueing " + neighbor.neighbor.name); neighborqueueable = enqueue(queue, neighbor.neighbor, headqueueable); alreadyAddedWaypoints.Add(neighbor.neighborID); } else if (neighborqueueable != null) { //Debug.Log("Already exists; Checking if should update weight"); float newdist = totaldistance(neighbor.neighbor, headqueueable); if (newdist < neighborqueueable.disttostart) { //Debug.Log("Updating weight"); queue.UpdatePriority(neighborqueueable, newdist); } } else { //Debug.Log("This waypoint has already been added at some point; will not add"); } //If the end point is the next point, a path is found if (neighbor.neighbor.WaypointID == end.WaypointID) { //Debug.Log("End detected"); endqueueable = neighborqueueable; //Should always be the first time this node is found so this should always be set to something endFound = true; break; } } } if (endqueueable == null) { Debug.Log("No path found between " + start.hashname + " and " + end.hashname); Stack <Waypoint> stack = new Stack <Waypoint>(); stack.Push(start); queue.Clear(); alreadyAddedWaypoints.Clear(); return(stack); } else { Stack <Waypoint> stack = new Stack <Waypoint>(); stack.Push(end); WaypointQueueable next = endqueueable.from; while (next != null) //Will stop before pushing start { stack.Push(next.self); next = next.from; } stack.Push(start); //Debug.Log("Path found. Returning path " + stack.ToArray()); queue.Clear(); alreadyAddedWaypoints.Clear(); return(stack); } }