void FindPath(Vector3 a_StartPos, Vector3 a_TargetPos)
{
Node StartNode = GridReference.NodeFromWorldPoint(a_StartPos);
Node TargetNode = GridReference.NodeFromWorldPoint(a_TargetPos);
List <Node> OpenList = new List <Node>();
HashSet <Node> ClosedList = new HashSet <Node>();
OpenList.Add(StartNode);
GridReference.FinalPath = null;
while (OpenList.Count > 0)
{
Node CurrentNode = OpenList[0];
for (int i = 1; i < OpenList.Count; i++)
{
if (OpenList[i].FCost < CurrentNode.FCost || OpenList[i].FCost == CurrentNode.FCost && OpenList[i].ihCost < CurrentNode.ihCost)
{
CurrentNode = OpenList[i];
}
}
OpenList.Remove(CurrentNode);
ClosedList.Add(CurrentNode);
if (CurrentNode == TargetNode)
{
GetFinalPath(StartNode, TargetNode);
//Debug.Log("REEEEE I'M BLOCKED");
//WorldGrid.GridObj.Blocked = true;
break;
}
foreach (Node NeighborNode in GridReference.GetNeighboringNodes(CurrentNode))
{
if (!NeighborNode.bIsWall || ClosedList.Contains(NeighborNode))
{
continue;
}
int MoveCost = CurrentNode.igCost + GetManhattenDistance(CurrentNode, NeighborNode);
if (MoveCost < NeighborNode.igCost || !OpenList.Contains(NeighborNode))
{
NeighborNode.igCost = MoveCost;
NeighborNode.ihCost = GetManhattenDistance(NeighborNode, TargetNode);
NeighborNode.ParentNode = CurrentNode;
if (!OpenList.Contains(NeighborNode))
{
OpenList.Add(NeighborNode);
}
}
}
}
}