// Start is called before the first frame update
public static Stack <WorldTile> FindPath(Vector3 start, Vector3 end)
{
var nodes = PathGrid.nodes;
Node startNode = nodes[start];
Node endNode = nodes[end];
List <Node> openSet = new List <Node>();
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node node = openSet[0];
for (int i = 1; i < openSet.Count; i++)
{
if (openSet[i].fCost < node.fCost || openSet[i].fCost == node.fCost)
{
if (openSet[i].hCost < node.hCost)
{
node = openSet[i];
}
}
}
openSet.Remove(node);
closedSet.Add(node);
if (node == endNode)
{
return(RetracePath(startNode, endNode));
}
var neighbors = PathGrid.GetNeighbours(node);
foreach (Node neighbour in neighbors)
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newCostToNeighbour = node.gCost + GetDistance(node, neighbour);
if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, endNode);
neighbour.parent = node;
node.child = neighbour;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
return(new Stack <WorldTile>());
}
public void FindPath(PathRequest request, Action <PathResult> callback)
{
Debug.Log("Find Path");
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(request.pathStart);
Node targetNode = grid.NodeFromWorldPoint(request.pathEnd);
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
}
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
pathSuccess = waypoints.Length > 0;
}
callback(new PathResult(waypoints, pathSuccess, request.callback));
}
IEnumerator FindPath(Vector2 startPos, Vector2 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector2[] waypoints = new Vector2[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
startNode.parent = startNode;
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
if (displayProcessingTime)
{
print("Path found: " + sw.ElapsedMilliseconds + " ms");
}
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
private IEnumerator PathSearch()
{
while (!grid.GridGenerated)
{
yield return(new WaitForSeconds(0.5f));
}
PathNode targetNode = grid.GetNearestWalkableNode(request.pathEnd);
PathNode startNode = grid.GetNearestWalkableNode(request.pathStart);
waypoints = new Vector3[0];
pathFound = false;
if (startNode.walkable && targetNode.walkable)
{
Heap <PathNode> openSet = new Heap <PathNode>(grid.MaxSize);
HashSet <PathNode> closedSet = new HashSet <PathNode>();
openSet.Add(startNode);
float timer = Time.realtimeSinceStartup;
float maxTime = .01f;
while (openSet.Count > 0)
{
if (Time.realtimeSinceStartup > timer + maxTime)
{
yield return(null);
timer = Time.realtimeSinceStartup;
}
PathNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathFound = true;
break;
}
foreach (PathNode neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMoveCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + currentNode.movementPenalty;
if (newMoveCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMoveCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
if (pathFound)
{
waypoints = RetracePath(startNode, targetNode);
pathFound = waypoints.Length > 0;
}
}
public void FindPath(PathGrid g, Vector3 startPos, Vector3 endPos)
{
//Translate start and end position into nodes on this
//agents map
PathNode start = g.GetNode(startPos);
PathNode end = g.GetNode(endPos);
//Two lists initialised to represent possible and impossible nodes for the path
List <PathNode> openNodes = new List <PathNode>();
List <PathNode> closedNodes = new List <PathNode>();
//Initialise by adding the first node
openNodes.Add(start);
//While the open list has not exhausted all of the nodes (and a solution has not been found)
while (openNodes.Count > 0)
{
//Initialise the current node to the first in the open list
PathNode current = openNodes[0];
//Cycle through the open list, if the f cost is less than the current,
//or it is equal but the heuristic cost is lower, make this node the current
for (int i = 1; i < openNodes.Count; i++)
{
if (openNodes[i].getFCost() < current.getFCost() ||
openNodes[i].getFCost() == current.getFCost() && openNodes[i].hCost < current.hCost)
{
current = openNodes[i];
}
}
//Remove the node with the lowest predicted cost from the open list and add
//it to the closed list
openNodes.Remove(current);
closedNodes.Add(current);
//Break the while loop if the target node has been reached
if (current == end)
{
InvertPath(start, end, g);
return;
}
//Cycle through the current nodes neighbours, ignoring the ones
//that are blocked
foreach (PathNode n in g.GetNeighbours(current))
{
if (!n.canWalk || closedNodes.Contains(n))
{
continue;
}
//Establish the neighbouring node with the lowest predicted cost
//Add it to the path list
int movementCost = current.gCost + GetNodeDistance(current, n);
if (movementCost < n.gCost || !openNodes.Contains(n))
{
n.gCost = movementCost;
n.hCost = GetNodeDistance(n, end);
n.parent = current;
if (!openNodes.Contains(n))
{
openNodes.Add(n);
}
}
}
}
}