private List <Node> Pathfinding(Vector3 position, Vector3 targetPosition)
{
Node startNode = nodeGrid.NodeFromWorldPoint(position);
Node targetNode = nodeGrid.NodeFromWorldPoint(targetPosition);
List <Node> openList = new List <Node>();
HashSet <Node> closedList = new HashSet <Node>();
openList.Add(startNode);
while (openList.Count > 0)
{
Node currentNode = openList[0];
for (int i = 1; i < openList.Count; i++)
{
if (openList[i].GetFCost() < currentNode.GetFCost() || openList[i].GetFCost() == currentNode.GetFCost() && openList[i].GetHCost() < currentNode.GetHCost())
{
currentNode = openList[i];
}
}
openList.Remove(currentNode);
closedList.Add(currentNode);
if (currentNode == targetNode)
{
return(GetFinalPath(startNode, targetNode));
}
foreach (Node neighborNode in nodeGrid.GetNeighboringNodes(currentNode))
{
if (!neighborNode.GetIsWall() || closedList.Contains(neighborNode))
{
continue;
}
int moveCost = currentNode.GetGCost() + GetManhattenDistance(currentNode, neighborNode);
if (moveCost < neighborNode.GetGCost() || !openList.Contains(neighborNode))
{
neighborNode.SetGCost(moveCost);
neighborNode.SetHCost(GetManhattenDistance(neighborNode, targetNode));
neighborNode.SetParentNode(currentNode);
if (!openList.Contains(neighborNode))
{
openList.Add(neighborNode);
}
}
}
}
return(null);
}
