void FindPath(Vector3 a_StartPos, Vector3 a_TargetPos)
{
ANode StartANode = AGridReference.ANodeFromWorldPoint(a_StartPos); //Gets the node closest to the starting position
ANode TargetANode = AGridReference.ANodeFromWorldPoint(a_TargetPos); //Gets the node closest to the target position
List <ANode> OpenList = new List <ANode>(); //List of nodes for the open list
HashSet <ANode> ClosedList = new HashSet <ANode>(); //Hashset of nodes for the closed list
OpenList.Add(StartANode); //Add the starting node to the open list to begin the program
while (OpenList.Count > 0) //Whilst there is something in the open list
{
ANode CurrentANode = OpenList[0]; //Create a node and set it to the first item in the open list
for (int i = 1; i < OpenList.Count; i++) //Loop through the open list starting from the second object
{
if (OpenList[i].FCost < CurrentANode.FCost || OpenList[i].FCost == CurrentANode.FCost && OpenList[i].ihCost < CurrentANode.ihCost) //If the f cost of that object is less than or equal to the f cost of the current node
{
CurrentANode = OpenList[i]; //Set the current node to that object
}
}
OpenList.Remove(CurrentANode); //Remove that from the open list
ClosedList.Add(CurrentANode); //And add it to the closed list
if (CurrentANode == TargetANode) //If the current node is the same as the target node
{
GetFinalPath(StartANode, TargetANode); //Calculate the final path
}
foreach (ANode NeighborANode in AGridReference.GetNeighboringANodes(CurrentANode)) //Loop through each neighbor of the current node
{
if (!NeighborANode.bIsWall || ClosedList.Contains(NeighborANode)) //If the neighbor is a wall or has already been checked
{
continue; //Skip it
}
int MoveCost = CurrentANode.igCost + GetManhattanDistance(CurrentANode, NeighborANode); //Get the F cost of that neighbor
if (MoveCost < NeighborANode.igCost || !OpenList.Contains(NeighborANode)) //If the f cost is greater than the g cost or it is not in the open list
{
NeighborANode.igCost = MoveCost; //Set the g cost to the f cost
NeighborANode.ihCost = GetManhattanDistance(NeighborANode, TargetANode); //Set the h cost
NeighborANode.ParentANode = CurrentANode; //Set the parent of the node for retracing steps
if (!OpenList.Contains(NeighborANode)) //If the neighbor is not in the openlist
{
OpenList.Add(NeighborANode); //Add it to the list
}
}
}
}
}
