/*
* Find the path to a target through the array of nodes
*
* Resources used:
* https://en.wikipedia.org/wiki/A*_search_algorithm
* https://www.youtube.com/watch?v=ySN5Wnu88nE
* https://www.geeksforgeeks.org/a-search-algorithm/
*/
private void FindPath(Vector3 startPos, Vector3 targetPos)
{
PathNode startNode = nodeGridReference.NodeFromWorldPoint(startPos); // find the node closest to the starting position
PathNode targetNode = nodeGridReference.NodeFromWorldPoint(targetPos); // find the node closest to the target position
List <PathNode> openList = new List <PathNode>(); // a list of nodes still to search
HashSet <PathNode> closedList = new HashSet <PathNode>(); // hashset of nodes of searched nodes
openList.Add(startNode);
while (openList.Count > 0)
{
PathNode currentNode = openList[0];
for (int i = 1; i < openList.Count; i++)
{
if (openList[i].totalCost < currentNode.totalCost || openList[i].totalCost == currentNode.totalCost && openList[i].distanceCost < currentNode.distanceCost)
{
currentNode = openList[i];
}
}
openList.Remove(currentNode);
closedList.Add(currentNode);
if (currentNode == targetNode)
{
GetFinalPath(startNode, targetNode);
break;
}
foreach (PathNode neighbourNode in nodeGridReference.GetNeighbouringNodes(currentNode))
{
if (!neighbourNode.isWall || closedList.Contains(neighbourNode))
{
continue;
}
int moveCost = currentNode.movementCost + GetDistance(currentNode, neighbourNode);
if (!openList.Contains(neighbourNode) || moveCost < neighbourNode.totalCost)
{
neighbourNode.movementCost = moveCost;
neighbourNode.distanceCost = GetDistance(neighbourNode, targetNode);
neighbourNode.parentNode = currentNode;
if (!openList.Contains(neighbourNode))
{
openList.Add(neighbourNode);
}
}
}
}
}
void FindPath(Vector3 startPos, Vector3 targetPos)
{
NodeAStar startNode = grid.NodeFromWorldPoint(startPos);
NodeAStar targetNode = grid.NodeFromWorldPoint(targetPos);
Heap <NodeAStar> openSet = new Heap <NodeAStar>(grid.MaxSize);
HashSet <NodeAStar> closedSet = new HashSet <NodeAStar>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
NodeAStar currentNode = openSet.PopFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
RetracePath(startNode, targetNode);
return;
}
foreach (NodeAStar neighbourNode in grid.GetNeighbouringNodes(currentNode))
{
if (!neighbourNode.walkable || closedSet.Contains(neighbourNode))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbourNode);
if (newMovementCostToNeighbour < neighbourNode.gCost || !openSet.Contains(neighbourNode))
{
neighbourNode.gCost = newMovementCostToNeighbour;
neighbourNode.hCost = GetDistance(neighbourNode, targetNode);
neighbourNode.parentNode = currentNode;
if (!openSet.Contains(neighbourNode))
{
openSet.Add(neighbourNode);
}
}
}
}
}
