// just to check if we can reach our goal from spawning point
// edge case for spawning point
// private bool canReachDestination(){
// float distance = Vector3.Distance(transform.position, destination.pos);
// Vector3 direction = destination.pos - transform.position;
// RaycastHit hitInfo;
// // currently can walk through air
// if (Physics.SphereCast(transform.position, 3.0f, destination.pos, out hitInfo, distance)){
// // then we can move directly, we assume we won't collide with other agents
// // if we can reach our goal
// Debug.Log("CanReachWillReturn " + hitInfo.transform.tag != "Wall" && hitInfo.transform.tag != "Agent" && hitInfo.transform.tag != "LShape");
// Debug.Log(hitInfo.transform.tag);
// return hitInfo.transform.tag != "Wall" && hitInfo.transform.tag != "Agent" && hitInfo.transform.tag != "LShape";
// }
// return false;
// }
// This is will look at nearby vertices that are accessible from the agent's current position
// and return the nearest one
private PathVertex getNearbyVertex()
{
List <PathVertex> allVertices = VisibilityGraphGenerator.getVertices();
List <PathVertex> potentialVertices = new List <PathVertex>();
RaycastHit hitInfo;
for (int i = 0; i < allVertices.Count; i++)
{
Vector3 direction = allVertices[i].pos - transform.position;
float distance = Vector3.Distance(transform.position, allVertices[i].pos);
if (Physics.SphereCast(transform.position, 3.0f, direction, out hitInfo, distance))
{
if (hitInfo.transform.tag != "Wall" && hitInfo.transform.tag != "LShape")
{
potentialVertices.Add(allVertices[i]);
}
}
}
float min = float.MaxValue;
int index = 0;
for (int i = 0; i < potentialVertices.Count; i++)
{
float dist = Vector3.Distance(transform.position, potentialVertices[i].pos);
if (dist < min)
{
min = dist;
index = i;
}
}
return(potentialVertices[index]);
}
// Runs A* on our graph : f(n) = g(n) + h(n)
// g(n) : distance from the source
// h(n) : estimate of the actual cost to get to the destination, it will be calculated from Vector3.Distance()
// our destination node is added into our graph but it isnt displayed
// returns the order of nodes that we need to walk through to reach our destination
public List <PathVertex> pathFind()
{
System.DateTime before = System.DateTime.Now;
System.DateTime after;
PathVertex startVertex = new PathVertex(start);
startVertex.gCost = 0.0f; // by definition
startVertex.fCost = Vector3.Distance(startVertex.pos, destination);
startVertex.setFCost();
fringe.Add(startVertex);
List <PathVertex> pathToTake = new List <PathVertex>();
// here we set the g cost to ALL our nodes to be infinity and we calculate the fcost
List <PathVertex> allNodes = VisibilityGraphGenerator.getVertices();
foreach (PathVertex v in allNodes)
{
v.parentVertex = null;
v.gCost = float.MaxValue;
v.setFCost();
}
// PSEUDO CODE FROM CLASS (DEAR TA, YOU DON'T NEED TO READ THIS, IT'S JUST FOR LOGIC/DEBUGGING)
// we might need to see node more than once
// g(start) = 0
// f(start) = g(start) + h(start) = h(start)
// g(n =/= start) = infinity, thus f(n =/= start) = infinity also
// fringe = {start} we start from the frige
// we keep going, while (fringe is not empty set):
// c = find node in the fringe that has the smallest f function (initially its start)
// if c == destination => done
// else : look at all neighbours n of C:
// compute new distance = g(c) + cost(getting from c to n)
// if dist < g(n) :
// if n is not in the fringe and dist + h(n) < f (n) :
// add n to the fringe
// either way, we're gonna update g(n) = dist
// f(n) = g(n) + h(n)
// our heuristic will be the straight distance to our destination
// basically f(start) = g(start) + h(start) = h(start) since g(start) = 0
while (fringe.Count > 0)
{
PathVertex current = getVertexWithSmallestFCost(fringe); // returns the one with smallest f function
if (current.pos == destination)
{
// we're done
after = System.DateTime.Now;
pathPlaningTime = after.Subtract(before).Milliseconds;
return(getPath(current));
}
// if current != destination THEN :
fringe.Remove(current); // we remove bc we've already searched it -> prevent infinite loop
// exploredVertices.Add(current);
// now we look at all neighbours of C
foreach (PathVertex neighbor in VisibilityGraphGenerator.getNeighbors(current))
{
// we only want the neighbors that we haven't explored already
// if (exploredVertices.Contains(neighbor)){
// continue;
// }
float transitionDistance = Vector3.Distance(current.pos, neighbor.pos);
float alternativeBestDistance = current.gCost + transitionDistance; // g(neighbor)
// if we found new best distance for neighbor then we update our neighbor
if (alternativeBestDistance < neighbor.gCost)
{
neighbor.gCost = alternativeBestDistance;
neighbor.parentVertex = current;
neighbor.hCost = Vector3.Distance(current.pos, destination);
neighbor.setFCost();
// if neighbor isn't in fringe, this just returns false, we don't care
// our way of updating the neighbor whether it was in our fringe or not
fringe.Remove(neighbor);
fringe.Add(neighbor);
}
}
}
// outside of while -> empty fringe -> couldn't find path -> return null
// sometimes path is not found, why?
after = System.DateTime.Now;
pathPlaningTime = after.Subtract(before).Milliseconds;
return(null);
}
