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