//Computes the heuristic cost value between this node and the given node //This is simply the vector2 distance between the two points if the navmesh were flatten onto a 2d plane public float HeuristicCost(NavMeshNode Goal) { Vector3 NodeLocation = AverageVertexLocations(); Vector3 GoalLocation = Goal.AverageVertexLocations(); Vector2 CurrentHeuristic = new Vector2(NodeLocation.X, NodeLocation.Z); Vector2 GoalHeuristic = new Vector2(GoalLocation.X, GoalLocation.Z); return(Vector2.Distance(CurrentHeuristic, GoalHeuristic)); }
//Constructs a pathway searching only through the nodes in the nav mesh public static List <Vector3> ConstructNodePathway(NavMesh NavMesh, Vector3 PathStart, Vector3 PathEnd) { //Project the pathways starting and ending locations onto the nav mesh plane to find which nodes they are contained within NavMeshNode StartNode = NavMesh.FindNodeContainingPoint(PathStart); NavMeshNode EndNode = NavMesh.FindNodeContainingPoint(PathEnd); //The set of nodes already evaluated List <NavMeshNode> ClosedSet = new List <NavMeshNode>(); //The set of currently discovered nodes that are not evaluated yet. //Initially, only the start node is known. List <NavMeshNode> OpenSet = new List <NavMeshNode>(); OpenSet.Add(StartNode); //Reset the pathfinding values of all nodes in the nav mesh foreach (NavMeshNode Node in NavMesh.MeshNodes) { Node.ResetPathfindingValues(); } //Precalculate the values for the starting node, as the cost to travel to itself is zero StartNode.GScore = 0; StartNode.FScore = StartNode.HeuristicCost(EndNode); //Iterator over the open set until a pathway is found or all nodes have been evaluated resulting in no pathway while (OpenSet.Count > 0) { //Finding the new current node, member of OpenSet with the lowest FScore value NavMeshNode CurrentNode = OpenSet[0]; for (int i = 1; i < OpenSet.Count; i++) { if (OpenSet[i].FScore < CurrentNode.FScore) { CurrentNode = OpenSet[i]; } } //If the new current node is the end node, then the pathway is complete if (CurrentNode == EndNode) { MessageLog.Print("Pathfinding.AStarSearch pathway found"); //Start from the end node and follow its parents back all the way to the start List <Vector3> Pathway = new List <Vector3>(); NavMeshNode CurrentStep = EndNode; while (CurrentStep != null) { Pathway.Add(CurrentStep.AverageVertexLocations()); CurrentStep = CurrentStep.Parent; } Pathway.Reverse(); return(Pathway); } //Move the new current node over the closed set as we are now going to compute all possible pathways over it OpenSet.Remove(CurrentNode); ClosedSet.Add(CurrentNode); //Iterate over each neighbour of the current node to check if thats a cheaper way to travel to the target location foreach (NavMeshNode Neighbour in CurrentNode.Neighbours) { //Ignore any neighbours in the closed set which have been completely evaulated if (ClosedSet.Contains(Neighbour)) { continue; } //Calculate the distance to travel here from the starting node float GScore = CurrentNode.GScore + Vector3.Distance(CurrentNode.AverageVertexLocations(), Neighbour.AverageVertexLocations()); //Add newly discovered nodes into the open list so they can be evaluated later if (!OpenSet.Contains(Neighbour)) { OpenSet.Add(Neighbour); } //If not, ignore if its not a cheaper way to travel else if (GScore >= Neighbour.GScore) { continue; } //A cheaper GScore means this neighbour is the cheapest way to travel, update things accordingly Neighbour.Parent = CurrentNode; Neighbour.GScore = GScore; Neighbour.FScore = Neighbour.GScore + Neighbour.HeuristicCost(EndNode); } } MessageLog.Print("Pathfinding.AStarSearch no path found"); return(null); }