//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);
}
public void ResetPathfindingValues()
{
Parent = null;
GScore = float.MaxValue;
FScore = float.MaxValue;
}
