public void FindPath(Vector3 a_StartPos, Vector3 a_TargetPos) { Node StartNode = GridReference.NodeFromWorldPoint(a_StartPos); //Gets the node closest to the starting position Node TargetNode = GridReference.NodeFromWorldPoint(a_TargetPos); //Gets the node closest to the target position List <Node> OpenList = new List <Node>(); //List of nodes for the open list HashSet <Node> ClosedList = new HashSet <Node>(); //Hashset of nodes for the closed list OpenList.Add(StartNode); //Add the starting node to the open list to begin the program while (OpenList.Count > 0) //Whilst there is something in the open list { Node CurrentNode = 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 < CurrentNode.FCost || OpenList[i].FCost == CurrentNode.FCost && OpenList[i].ihCost < CurrentNode.ihCost) //If the f cost of that object is less than or equal to the f cost of the current node { CurrentNode = OpenList[i]; //Set the current node to that object } } OpenList.Remove(CurrentNode); //Remove that from the open list ClosedList.Add(CurrentNode); //And add it to the closed list if (CurrentNode == TargetNode) //If the current node is the same as the target node { PathToWalk = GetFinalPath(StartNode, TargetNode); //Calculate the final path } foreach (Node NeighborNode in GridReference.GetNeighboringNodes(CurrentNode)) //Loop through each neighbor of the current node { if (!NeighborNode.isWalkable || ClosedList.Contains(NeighborNode)) //If the neighbor is a wall or has already been checked { continue; //Skip it } if (NeighborNode.isMud) { CurrentNode.igCost += GridWorld.instance.mudCost; } if (NeighborNode.isGravel) { CurrentNode.igCost += GridWorld.instance.gravelCost; } int MoveCost = CurrentNode.igCost + GetManhattanDistance(CurrentNode, NeighborNode); //Get the F cost of that neighbor if (MoveCost < NeighborNode.igCost || !OpenList.Contains(NeighborNode)) //If the f cost is greater than the g cost or it is not in the open list { NeighborNode.igCost = MoveCost; //Set the g cost to the f cost NeighborNode.ihCost = GetManhattanDistance(NeighborNode, TargetNode); //Set the h cost NeighborNode.ParentNode = CurrentNode; //Set the parent of the node for retracing steps if (!OpenList.Contains(NeighborNode)) //If the neighbor is not in the openlist { OpenList.Add(NeighborNode); //Add it to the list } } } } }