public Vector3[] FindPath(Vector3 startPos, Vector3 targetPos)
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.GetNodeFromWorldPoint(startPos);
Node targetNode = grid.GetNodeFromWorldPoint(targetPos);
if (!targetNode.walkable)
{
targetNode = GetClosestValidNode(targetNode, targetPos);
}
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node> (grid.MaxSize); //Nodes to be evaluated (check if it is the closest to target)
HashSet <Node> closedSet = new HashSet <Node> (); //Nodes already evaluated (is used to add his neighbours to the openSet)
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
/*//Find the node with the lowest fCost (picks the one with the lowest hcost if there are several)
* for (int i = 1; i < openSet.Count; i++)
* {
* if (openSet [i].fCost < currentNode.fCost || openSet [i].fCost == currentNode.fCost && openSet [i].hCost < currentNode.hCost)
* {
* currentNode = openSet [i];
* }
* }
*
* //The node is eveluated, so we pass it to the closedSet
* openSet.Remove (currentNode);*/
closedSet.Add(currentNode);
//Checks if the node is the target
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
//Check the neighbours of the current node to add them to the open set
//where they will be evaluted in the next loop
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
// pass the node if it's not walkable or has already been evaluted
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
//update or set gCost and hCost of the node
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
//set the currentNode as "parent" (it's a variable) of the neighbour node
//(the "parenting" hierachy will help trace the path from the target to the start)
neighbour.parent = currentNode;
//Add to open set if it's not already in
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
openSet.UpdateItem(neighbour);
}
}
}
}
}
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
return(RetracePath(startNode, targetNode));
}
else
{
return(null);
}
}
IEnumerator FindPath(Vector3 startPos_, Vector3 endPos_)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
// Get the start and end nodes.
PF_Node startNode = grid.GetNodeFromWorldPoint(startPos_);
PF_Node endNode = grid.GetNodeFromWorldPoint(endPos_);
startNode.parentNode = startNode;
// Only search if start and end are both walkable nodes.
if (startNode.walkable && endNode.walkable)
{
// Create two lists of nodes.
// Open list will contain nodes to be evaluated.
// Closed list will contain nodes that have already been evaluated.
PF_Heap <PF_Node> openSet = new PF_Heap <PF_Node>(grid.MaxSize);
HashSet <PF_Node> closedSet = new HashSet <PF_Node>();
// Add the starting node to our open list.
openSet.Add(startNode);
// Loop through the openSet.
while (openSet.Count > 0)
{
// Begin with the first element in the list.
PF_Node currentNode = openSet.RemoveFirst();
// Add the starting node to the closed set.
closedSet.Add(currentNode);
// Path has been found.
if (currentNode == endNode)
{
sw.Stop();
//UnityEngine.Debug.Log("Path found: " + sw.ElapsedMilliseconds + "ms");
pathSuccess = true;
break;
}
foreach (PF_Node neighbour in grid.GetNeighbourNodes(currentNode))
{
// Check if the neighbour is not walkable, or already in the closed list.
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
// Get the cost to move to the neighbour.
int newNeighbourMoveCost = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
// If the new move cost is smaller than the current, or the open list does not contain this neighbour.
if (newNeighbourMoveCost < neighbour.gCost || !openSet.Contains(neighbour))
{
// Update the gCost and hCost.
neighbour.gCost = newNeighbourMoveCost;
neighbour.hCost = GetDistance(neighbour, endNode);
// Update the parent node.
neighbour.parentNode = currentNode;
// Add it to the open set if it is not currently there.
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
// Otherwise update it.
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
waypoints = CalculatePath(startNode, endNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}