/* * 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); } } } } }