public Node(Grid3D _grid, bool _isObstacle, Vector3 _pos, int _gridX, int _gridY, int _gridZ, int _penalty)
{
gridX = _gridX;
gridY = _gridY;
gridZ = _gridZ;
isObstacle = _isObstacle;
worldPos = _pos;
Grid = _grid;
movementPenaly = _penalty;
}
public static List <Node> AStarPathFinder(Grid3D grid, Node startNode, Node goalNode, ref List <Node> path, ref bool isSuccess)
{
Heap <Node> openSet = new Heap <Node>(grid.MaxSize); // use List when not using heap
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == goalNode)
{
path = RetracePath(startNode, goalNode);
isSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.isObstacle || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenaly;
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, goalNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
return(path);
}
private void Awake()
{
requestManager = GetComponent <RequestPathManager>();
grid = GetComponent <Grid3D>();
}