public void AddNeighborUnique(NeighborConnection neighbor) { bool contains = false; foreach (NeighborConnection addedneighbor in neighbors) { if (addedneighbor.neighborID == neighbor.neighborID) { contains = true; break; } } Debug.Log("Checking uniqueness for " + neighbor.neighborName); if (!contains) { Debug.Log("Neighbor has not been added yet, adding"); //this.neighbors.Add(neighbor.neighborID, neighbor); this.neighbors.Add(neighbor); } else { Debug.Log("Neighbor has already been added. Doing nothing"); } }
///<summary> ///Ideally this reference to all waypoints is only done in the editor when the waypoint doesn't know who it's neighbors are ///</summary> public void FindNeighbors() { Debug.Log("Waypoint " + this.hashname + " has begun looking for nearby neighbors"); Collider[] potentialNearbyObjects = Physics.OverlapSphere(this.transform.position, neighborSearchRadius, Physics.DefaultRaycastLayers, QueryTriggerInteraction.Collide); List <Collider> nearbyObjects = new List <Collider>(); this.gameObject.layer = LayerMask.NameToLayer("Ignore Raycast"); foreach (Collider c in potentialNearbyObjects) { RaycastHit hit; if (Physics.Raycast(this.transform.position, c.transform.position - this.transform.position, out hit, neighborSearchRadius, LayerMask.GetMask("Default"), QueryTriggerInteraction.Collide)) { if (hit.collider == c) { Debug.Log("Raycast to collider " + c.name + " succesful; collider visible."); nearbyObjects.Add(c); } else { Debug.Log("Raycast to collider " + c.name + " failed; cannot make conneciton."); } } } this.gameObject.layer = LayerMask.NameToLayer("Default"); for (int i = 0; i < nearbyObjects.Count; i++) { Debug.Log("Potential neighbor " + nearbyObjects[i].name + nearbyObjects[i].GetHashCode() + " detected"); Waypoint neighbor; if (nearbyObjects[i].gameObject != this.gameObject && nearbyObjects[i].TryGetComponent <Waypoint>(out neighbor) && (!neighbor.CompareTag("Offduty Area") && !neighbor.CompareTag("Repair Area")) ) { Debug.Log("Neighbor confirmed. Adding neighbor " + neighbor.hashname + " ID " + neighbor.WaypointID); NeighborConnection connection = new NeighborConnection(neighbor); connection.disttoneighbor = Vector3.Distance(this.transform.position, neighbor.transform.position); AddNeighborUnique(connection); Debug.Log("Adding self as neighbor to the possibly new neighbor."); NeighborConnection connectionBack = new NeighborConnection(this); connectionBack.disttoneighbor = connection.disttoneighbor; neighbor.AddNeighborUnique(connectionBack); } else { Debug.Log("Neighbor rejected"); } } }
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); } }