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