public List <Node_Wrapper> PathHere()
{
List <Node_Wrapper> output;
if (parentNode != null)
{
output = parentNode.PathHere();
}
else
{
output = new List <Node_Wrapper>();
}
output.Add(this);
return(output);
}
// Murderer finds the most effective path from himself to the defined object.
// The function overwrites murderer's current path with the new one it finds.
private void FindPathTo(Transform destination)
{
if (nodeList.Count < 1)
{
pathIsImpossible = true;
return;
}
// Find the nodes closest to start and goal
Node_Wrapper startNode = null;
Node_Wrapper goalNode = null;
foreach (Node_Wrapper node in nodeList)
{
// Set startnode if this node is closer than the closest so far.
if (IsTransformVisibleFrom(node.position, transform) || (transform.position - node.position).sqrMagnitude < 0.5F)
{
if (startNode != null)
{
if ((transform.position - node.position).sqrMagnitude < (transform.position - startNode.position).sqrMagnitude)
{
startNode = node;
}
}
else
{
startNode = node;
}
}
// Set goalnode if this node is closer than the closest so far.
if (IsTransformVisibleFrom(node.position, destination, new string[] { "Door" }) || (destination.position - node.position).sqrMagnitude < 0.5F)
{
if (goalNode != null)
{
if ((destination.position - node.position).sqrMagnitude < (destination.position - goalNode.position).sqrMagnitude)
{
goalNode = node;
}
}
else
{
goalNode = node;
}
}
}
if (startNode == null || goalNode == null)
{
currentPath.Clear();
pathIsImpossible = true;
return;
}
// A* algorithm
int failSafe = 0;
List <Node_Wrapper> openList = new List <Node_Wrapper>();
List <Node_Wrapper> closedList = new List <Node_Wrapper>();
openList.Add(startNode);
while (openList.Count > 0 && failSafe < 1000F)
{
failSafe++;
// This is to find element with lowest score.
Node_Wrapper bestNode = null;
float bestPathScore = float.MaxValue;
foreach (Node_Wrapper node in openList)
{
float nodeScore = node.distanceTraveled + ((node.position - goalNode.position).sqrMagnitude / 2);
if (nodeScore < bestPathScore)
{
bestNode = node;
bestPathScore = nodeScore;
}
}
// Find all neighbours of that node, that haven't been tried out yet.
foreach (Node_Wrapper node in bestNode.neighbours)
{
if (!openList.Contains(node) && !closedList.Contains(node))
{
node.parentNode = bestNode;
node.distanceTraveled = bestNode.distanceTraveled + (bestNode.position - node.position).sqrMagnitude;
openList.Add(node);
}
else if (node.distanceTraveled > bestNode.distanceTraveled + (bestNode.position - node.position).sqrMagnitude)
{
node.parentNode = bestNode;
node.distanceTraveled = bestNode.distanceTraveled + (bestNode.position - node.position).sqrMagnitude;
}
}
if (bestNode.position != goalNode.position)
{
closedList.Add(bestNode);
openList.Remove(bestNode);
}
else
{
currentPath = bestNode.PathHere(); // JIPPI!
pathStage = 0;
arrivedAtPathEnd = false;
pathIsImpossible = false;
break;
}
}
// Is the path impossible??
if (openList.Count <= 0 || failSafe >= 1000F)
{
pathIsImpossible = true;
}
// Cleans up!
foreach (Node_Wrapper node in nodeList)
{
node.parentNode = null;
node.distanceTraveled = 0F;
}
}