void Awake()
{
InitializeClusterMapAndLookupTable();
foreach (Cluster c in clusterArray)
{
Debug.Log(c.clusterID);
}
// populate nodes
GameObject nodesParent = GameObject.Find("Pathnodes");
foreach (Transform child in nodesParent.transform)
{
Pathnode node = child.GetComponent <Pathnode>();
nodesList.Add(node);
clusterArray[node.clusterID].AddNode(node);
}
// populate edges
foreach (Pathnode node in nodesList)
{
foreach (Pathnode neighbour in node.GetNeighbours())
{
edgesList.Add(new Edge(node, neighbour, (node.position - neighbour.position).magnitude));
}
}
FindShortestPath(new Vector3(-4.6f, 0f, -4.6f), new Vector3(4.4f, 0f, 4.4f));
//CreateClusterLookupTable();
Debug.Log("done");
}
// refreshes the target node
private void Flee2()
{
// looks for a node in the opposite direction in the neighbour nodes, if none are found, default to the first neighbour
if (isFleeing())
{
Pathnode nextTarget = lastVisitedNode.neighbourNodes[0];
foreach (Pathnode neighbour in lastVisitedNode.neighbourNodes)
{
Vector3 lookAtEnemyDirection = Vector3.Normalize(currentPursuer.transform.position - transform.position);
Quaternion lookAtEnemyRotation = Quaternion.LookRotation(lookAtEnemyDirection);
Vector3 lookAtNextNodeDirection = Vector3.Normalize(neighbour.position - transform.position);
Quaternion lookAtNextNodeRotation = Quaternion.LookRotation(lookAtNextNodeDirection);
if (Mathf.Abs(lookAtNextNodeRotation.eulerAngles.y - lookAtEnemyRotation.eulerAngles.y) > 90)
{
nextTarget = neighbour;
}
}
Debug.DrawLine(transform.position, nextTarget.position, Color.blue);
pathToFollow = Graph.FindShortestPath(transform.position, nextTarget.position);
followedNode = pathToFollow.firstNode();
// avoid backtracking
if (followedNode.Equals(lastVisitedNode))
{
followedNode = pathToFollow.nextNode();
}
}
}
// seek target
void FixedUpdate()
{
CalculatePath();
Vector3 accelVector = Vector3.zero;
Vector3 distanceVector = followedNode.position - transform.position;
Vector3 velocityDirection = Vector3.Normalize(distanceVector);
accelVector = velocityDirection * maxAccel;
velocity = accelVector * Time.deltaTime;
transform.LookAt(followedNode.position);
// when we hit a node on our path and there is still more
if (distanceVector.magnitude < 0.1f && pathToFollow.hasNextNode())
{
lastVisitedNode = followedNode;
followedNode = pathToFollow.nextNode();
}
// when we hit the end of our path, stop
if (distanceVector.magnitude < 0.05f)
{
lastVisitedNode = followedNode;
velocity = Vector3.zero;
}
transform.position += velocity;
}
// find next cluster, and visit a random node in that cluster (for searching)
void NextClusterNode()
{
int currentClusterID = Graph.GetNodeClosestTo(transform.position).clusterID;
clusterToSearch = Graph.clusterArray[(clusterToSearch.clusterID + 1) % Graph.clusterArray.Length];
Pathnode destination = clusterToSearch.getClusterNodes()[Random.Range(0, clusterToSearch.getClusterNodes().Count)];
pathToFollow = Graph.FindShortestPath(transform.position, destination.position);
}
// same as pathfinding seek
private void Flee()
{
Pathnode nextTarget = lastVisitedNode.neighbourNodes[0];
//just for debug purposes
foreach (Pathnode neighbour in lastVisitedNode.neighbourNodes)
{
Vector3 lookAtEnemyDirection = Vector3.Normalize(currentPursuer.transform.position - transform.position);
Quaternion lookAtEnemyRotation = Quaternion.LookRotation(lookAtEnemyDirection);
Vector3 lookAtNextNodeDirection = Vector3.Normalize(neighbour.position - transform.position);
Quaternion lookAtNextNodeRotation = Quaternion.LookRotation(lookAtNextNodeDirection);
if (Mathf.Abs(lookAtNextNodeRotation.eulerAngles.y - lookAtEnemyRotation.eulerAngles.y) > 90)
{
nextTarget = neighbour;
}
}
Debug.DrawLine(transform.position, nextTarget.position, Color.blue);
Vector3 vectorFromPursuer = currentPursuer.transform.position - transform.position;
Vector3 pursuerDistVector = Vector3.Normalize(vectorFromPursuer);
Vector3 destination = transform.position + -pursuerDistVector * 3f;
//Pathnode nextTarget = Graph.GetNodeClosestTo(destination);
Vector3 accelVector = Vector3.zero;
Vector3 distanceVector = followedNode.position - transform.position;
Vector3 velocityDirection = Vector3.Normalize(distanceVector);
accelVector = velocityDirection * maxAccel;
velocity = accelVector * Time.deltaTime;
transform.LookAt(followedNode.position);
if (distanceVector.magnitude < 0.1f && pathToFollow.hasNextNode())
{
lastVisitedNode = followedNode;
followedNode = pathToFollow.nextNode();
}
if (distanceVector.magnitude < 0.05f)
{
lastVisitedNode = followedNode;
velocity = Vector3.zero;
}
transform.position += velocity;
float distFromPursuer = vectorFromPursuer.magnitude;
if (distFromPursuer < 0.25f)
{
currentState = State.FROZEN;
}
}
// find path to target, while avoiding backtracking to the previous node
// backtracking is a result of the path generation algorithm
void CalculatePath()
{
pathToFollow = Graph.FindShortestPath(transform.position, target.position);
followedNode = pathToFollow.firstNode();
if (followedNode.Equals(lastVisitedNode))
{
followedNode = pathToFollow.nextNode();
}
}
public override bool Equals(System.Object node)
{
if ((node == null) || !this.GetType().Equals(node.GetType()))
{
return(false);
}
else
{
Pathnode n = (Pathnode)node;
return(position == n.position && heuristic == n.heuristic);
}
}
Edge getEdge(Pathnode nodeFrom, Pathnode nodeTo)
{
foreach (Edge edge in edgesList)
{
if (edge.getFromNode() == nodeFrom && edge.getToNode() == nodeTo)
{
Debug.Log("found a match");
return(edge);
}
}
return(null);
}
private void OnTriggerEnter(Collider other)
{
if (other.gameObject.CompareTag("Pathnode") && other.gameObject.name == CurrrentNode.name)
{
Pathnode node;
if (other.TryGetComponent <Pathnode>(out node))
{
CurrrentNode = node.nextnode;
MoveToPathNode();
}
}
}
// backtracks the path from the endNode and connects it to its previous nodes
static List <Edge> connectPath(Pathnode startNode, Pathnode endNode)
{
List <Edge> connectedPath = new List <Edge>();
Pathnode prevNode = endNode;
do
{
float dist = (prevNode.position - prevNode.previous.position).magnitude;
Edge edge = new Edge(prevNode.previous, prevNode, dist);
connectedPath.Insert(0, edge);
prevNode = prevNode.previous;
} while(prevNode != startNode);
return(connectedPath);
}
// Start is called before the first frame update
void Awake()
{
//InvokeRepeating("RandomizeTarget", 0.0f, 3.0f);
InvokeRepeating("CheckNearbyPursuers", 0.0f, 0.5f);
InvokeRepeating("Flee2", 0.0f, 0.5f);
//InvokeRepeating("AvoidBacktracking", 3.0f, 3.0f);
lastVisitedNode = Graph.GetNodeClosestTo(transform.position);
bounds = GameObject.Find("Plane").GetComponent <MeshCollider>().bounds;
followedNode = Graph.GetNodeClosestTo(transform.position);
clusterToSearch = Graph.clusterArray[Graph.GetNodeClosestTo(transform.position).clusterID];
if (isWandering())
{
NextClusterNode();
}
}
public static Pathnode GetNodeClosestTo(Vector3 pos)
{
if (nodesList == null || nodesList.Count <= 0)
{
return(null);
}
else
{
float shortestDistance = float.MaxValue;
Pathnode closestNode = null;
for (int i = 0; i < nodesList.Count; ++i)
{
Pathnode node = nodesList [i];
float dist = Vector3.Distance(pos, node.position);
if (dist < shortestDistance)
{
shortestDistance = dist;
closestNode = node;
}
}
return(closestNode);
}
}
/**
* Find a random neighbour and go visit that one
*/
void Wander()
{
Vector3 distanceVector = followedNode.position - transform.position;
float distance = distanceVector.magnitude;
Vector3 accelVector = (distanceVector / distance) * maxAccel;
velocity = accelVector * Time.deltaTime;
// when we hit a node on our path, go to the next one
if (distanceVector.magnitude < 0.1f && pathToFollow.hasNextNode())
{
lastVisitedNode = followedNode;
followedNode = pathToFollow.nextNode();
}
// when we finally arrive to the last point of our path, generate a new path
if (distanceVector.magnitude < 0.05f)
{
NextClusterNode();
}
transform.LookAt(followedNode.position);
transform.position += velocity;
}
// Start is called before the first frame update
void Awake()
{
clusterToSearch = Graph.clusterArray[Graph.GetNodeClosestTo(transform.position).clusterID];
followedNode = Graph.GetNodeClosestTo(transform.position);
NextClusterNode();
}
protected void CreatePath(Pathnode target)
{
float shortest = float.MaxValue;
if (target == null) return;
if (path == null || !path.Any())
{
path = new List<Pathnode>();
}
else
{
shortest = CalcLength(path, shortest);
}
Queue<Pathnode> queueThing = new Queue<Pathnode>();
List<Pathnode> done = new List<Pathnode>();
done.AddRange(human.restrictedPaths);
var closestNode = world.Pathnodes.OrderBy(n => (n.Location - human.Position).Length()).Where(n => !human.restrictedPaths.Contains(n)).FirstOrDefault();
if (closestNode != null)
{
done.Add(closestNode);
}
if (closestNode == target)
{
path = new List<Pathnode>() { target };
return;
}
bool found = false;
foreach (var link in closestNode.LinkedNodes)
{
if (human.restrictedPaths.Contains(link)) continue;
var route = new List<Pathnode>() { closestNode, link };
var result = new List<Pathnode>();
if (FindPath(link, target, route, done.ToList(), result))
{
found = true;
float length = CalcLength(result, shortest);
if (length < shortest)
{
path = result;
shortest = length;
}
}
}
if (!found)
{
shortest = float.MaxValue;
foreach (var link in closestNode.LinkedNodes)
{
var route = new List<Pathnode>() { closestNode, link };
var result = new List<Pathnode>();
done = new List<Pathnode>() { closestNode };
if (FindPath(link, target, route, done.ToList(), result))
{
found = true;
float length = CalcLength(result, shortest);
if (length < shortest)
{
path = result;
shortest = length;
}
}
}
}
}
public Edge(Pathnode fromNode, Pathnode toNode, float cost)
{
this.fromNode = fromNode;
this.toNode = toNode;
this.cost = cost;
}
public Edge()
{
fromNode = null;
toNode = null;
cost = 0;
}
private bool FindPath(Pathnode link, Pathnode target, List<Pathnode> path, List<Pathnode> avoid, List<Pathnode> result)
{
bool found = false;
float shortest = float.MaxValue;
avoid.Add(link);
if (link == target)
{
result.AddRange(path);
return true;
}
foreach (var l in link.LinkedNodes)
{
if (!avoid.Contains(l))
{
var potential = new List<Pathnode>();
if (FindPath(l, target, path.Concat(new[] { l }).ToList(), avoid.ToList(), potential))
{
found = true;
var newLength = CalcLength(potential, shortest);
if (newLength < shortest)
{
result.Clear();
result.AddRange(potential);
shortest = newLength;
}
}
}
}
return found;
}
/**
* Connects a path between the closest start and end node to two points
*/
public static Path FindShortestPath(Vector3 start, Vector3 end)
{
Pathnode nodeClosestToStart = GetNodeClosestTo(start);
Pathnode nodeClosestToEnd = GetNodeClosestTo(end);
GameObject startObject = new GameObject("startObject");
startObject.transform.position = start;
startObject.AddComponent <Pathnode>();
GameObject endObject = new GameObject("endObject");
endObject.transform.position = end;
endObject.AddComponent <Pathnode>();
// create a node at start and end
Pathnode startNode = startObject.GetComponent <Pathnode>();
Pathnode endNode = endObject.GetComponent <Pathnode>();
if (start == end)
{
return(null);
}
if (startNode.Equals(nodeClosestToStart))
{
startNode = nodeClosestToStart;
}
else
{
startNode.neighbourNodes = new List <Pathnode>();
startNode.neighbourNodes.Add(nodeClosestToStart);
nodeClosestToStart.neighbourNodes.Add(startObject.GetComponent <Pathnode>());
}
if (endNode.Equals(nodeClosestToEnd))
{
endNode = nodeClosestToEnd;
}
else
{
endNode.neighbourNodes = new List <Pathnode>();
endNode.neighbourNodes.Add(nodeClosestToEnd);
nodeClosestToEnd.neighbourNodes.Add(endObject.GetComponent <Pathnode>());
}
Pathnode currentNode = startNode;
List <Pathnode> openList = new List <Pathnode>();
List <Pathnode> closedList = new List <Pathnode>();
closedList.Add(currentNode);
int iter = 0;
do
{
iter++;
//Debug.Log("START OF ITERATION " + iter);
//Debug.Log("currentNode is " + currentNode.name);
float lowestCost = float.MaxValue;
float cost = 0;
Pathnode closestNode = null;
Pathnode toNode = null;
Pathnode fromNode = null;
//Debug.Log("looking for " + currentNode.name + "'s neighbours");
/*Debug.Log("Neighbours = " + string.Join("",
* new List<Pathnode>(currentNode.neighbourNodes)
* .ConvertAll(i => i.ToString())
* .ToArray()));*/
// add to open list
foreach (Pathnode neighbour in currentNode.neighbourNodes)
{
if (!openList.Contains(neighbour) && !closedList.Contains(neighbour))
{
neighbour.previous = currentNode;
openList.Add(neighbour);
//Debug.Log("added " + neighbour.name + " to open list");
}
}
/*Debug.Log("OpenList = " + string.Join("",
* new List<Pathnode>(openList)
* .ConvertAll(i => i.ToString())
* .ToArray()));
* Debug.Log("ClosedList = " + string.Join("",
* new List<Pathnode>(closedList)
* .ConvertAll(i => i.ToString())
* .ToArray()));*/
foreach (Pathnode openNode in openList)
{
if (openNode.Equals(currentNode)) // skip current node if already in open list
{
continue;
}
float edgeCost = float.MaxValue;
if (openNode.clusterID != openNode.previous.clusterID)
{
// lookup table for cluster values
edgeCost = lookupTable[openNode.clusterID, openNode.previous.clusterID];
}
else
{
// otherwise compute euclidean distance as the heuristic
edgeCost = (openNode.position - openNode.previous.position).magnitude;
}
cost = edgeCost + (endNode.position - openNode.position).magnitude;
//Debug.Log("edge cost from " + openNode.previous.name + " to " + openNode.name + " is " + cost);
// Find lowest cost path
if (cost < lowestCost && !closedList.Contains(openNode))
{
lowestCost = cost;
closestNode = openNode;
//Debug.Log("cost is lower than before, new closest node is now " + closestNode.name);
toNode = openNode;
fromNode = openNode.previous;
}
}
//Debug.Log("adding " + currentNode.name + " to closedlist");
//Debug.Log("closest node is " + closestNode.name);
closedList.Add(currentNode);
//Debug.Log("removing " + currentNode.name + " from the open list");
openList.Remove(currentNode);
//pathNodes.Add(currentNode);
//path.Add(currentNode); to replace
visitedPath.Add(new Edge(fromNode, toNode, lowestCost));
//Debug.Log("next node will be " + closestNode.name);
currentNode = closestNode;
//Debug.Log("END OF ITERATION " + iter);
} while(currentNode != endNode && iter < 1000);
// finally, connect the path
path = new Path(connectPath(startNode, endNode));
nodeClosestToStart.neighbourNodes.Remove(startNode);
nodeClosestToEnd.neighbourNodes.Remove(endNode);
Destroy(startObject);
Destroy(endObject);
return(path);
}
public void AddNode(Pathnode node)
{
nodesList.Add(node);
}
public void MoveToPathNode(Pathnode node)
{
CurrrentNode = node;
MoveTo(CurrrentNode.gameObject);
}
