//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(NavMeshVertex Goal)
{
Vector2 CurrentHeuristic = new Vector2(VertexLocation.X, VertexLocation.Z);
Vector2 GoalHeuristic = new Vector2(Goal.VertexLocation.X, Goal.VertexLocation.Z);
return(Vector2.Distance(CurrentHeuristic, GoalHeuristic));
}
//Performs a line of sight check between two nodes, the function will return true if there is a straight line between these two
//vertices with no obstacles in the way and having no point of the line step outside of the nav mesh
public bool LineofSight(NavMeshVertex Target)
{
//First check if theres anything between the two nodes that would block walking in a straight line
Vector3 RayDirection = VertexLocation - Target.VertexLocation;
//Ray Ray = new Ray(VertexLocation, RayDirection);
//RayCastResult Result;
//Physics.WorldSimulator.Space.RayCast(Ray, out Result);
return(false);
}
public List <NavMeshVertex> VertexNeighbours = new List <NavMeshVertex>(); //The adjacent verices connected to
public void LinkVertices(NavMeshVertex OtherVertex)
{ //Assign the two vertices as neighbours to one another
if (!VertexNeighbours.Contains(OtherVertex))
{
VertexNeighbours.Add(OtherVertex);
}
if (!OtherVertex.VertexNeighbours.Contains(this))
{
OtherVertex.VertexNeighbours.Add(this);
}
}
public NavMeshVertex VertexForPosition(Vector3 VertexPosition)
{//Returns an already existing NavMeshVertex for the given position if it can be found
foreach (NavMeshVertex Vertex in MeshVertices)
{
if (Vertex.VertexLocation == VertexPosition)
{
return(Vertex);
}
}
//Create a new one in the list and return that if one didnt already exist for this location
NavMeshVertex NewVertex = new NavMeshVertex(VertexPosition);
MeshVertices.Add(NewVertex);
return(NewVertex);
}
//returns the NodeVertex which is closest to the given location
public NavMeshVertex GetVertexClosestTo(Vector3 Location)
{
NavMeshVertex ClosestVertex = NodeVertices[0];
float ClosestVertexDistance = Vector3.Distance(Location, ClosestVertex.VertexLocation);
for (int i = 1; i < NodeVertices.Count; i++)
{
float CompareVertexDistance = Vector3.Distance(Location, NodeVertices[i].VertexLocation);
if (CompareVertexDistance < ClosestVertexDistance)
{
ClosestVertex = NodeVertices[i];
ClosestVertexDistance = CompareVertexDistance;
}
}
return(ClosestVertex);
}
public float FScore = float.MaxValue; //Cost to travel from this vertex to the ending vertex
public void ResetPathfindingValues()
{
Parent = null;
GScore = float.MaxValue;
FScore = float.MaxValue;
}
//Default Constructor
public NavMesh(string FileName)
{
//Variables to temporarily store all the data loaded from file while the nav mesh is constructed
// Vector3[] Vertices; //The entire list of vertices which make up the entirety of the navigation mesh
//int[] Indices; //The indices of the vertices of the navigation mesh
//Model ModelData; //Model object constructed from the navmesh resource file, vertices and indices are extracted from this
////Load the nav mesh from file
//ModelData = Rendering.Window.Instance.Content.Load<Model>(FileName);
////Extract the meshes vertices and indices
//Data.ModelDataExtractor.GetVerticesAndIndicesFromModel(ModelData, out Vertices, out Indices);
////Use these to create a new static mesh object
//MeshData = new StaticMesh(Vertices, Indices, new AffineTransform(Vector3.Zero));
////Loop through each face which makes up the navigation mesh and create a new mesh node object for each of them
//for (int i = 0; i < Indices.Length / 3; i++)
//{
// //Grab the locations of the 3 vertices which make up this face of the navigation mesh
// Vector3[] MeshVertices = GetNextLocations(i * 3, Vertices, Indices);
// //Create a new nav mesh node object, store in it the vertices, and the center location of the nodes 3 vertices
// NavMeshNode NewNode = new NavMeshNode(MeshVertices);
// //As each mesh node is created, store it in the static NavMeshNodes class so they can easily be accessed by all classes
// MeshNodes.Add(NewNode);
//}
////Now that all of the mesh node objects have been define, we need to figure out and assign which nodes are neighbours to each other
//for (int i = 0; i < MeshNodes.Count; i++)
//{//Loop through each of the mesh nodes we have created
// NavMeshNode CurrentNode = MeshNodes[i];
// for (int j = i + 1; j < MeshNodes.Count; j++)
// {//Check each node against all nodes that are after it
// NavMeshNode CompareNode = MeshNodes[j];
// //If the current node shares any of its vertices with the compare node, then they are neighbours to one another
// if (CurrentNode.VertexLocations.Contains(CompareNode.VertexLocations[0]) ||
// CurrentNode.VertexLocations.Contains(CompareNode.VertexLocations[1]) ||
// CurrentNode.VertexLocations.Contains(CompareNode.VertexLocations[2]))
// {//Assign the two nodes as neighbours to one another (if they arent already)
// if (!CurrentNode.Neighbours.Contains(CompareNode))
// CurrentNode.Neighbours.Add(CompareNode);
// if (!CompareNode.Neighbours.Contains(CurrentNode))
// CompareNode.Neighbours.Add(CurrentNode);
// }
// }
//}
//Now set up a list for every unique vertex location in the entire nav mesh, with each of them pointing to their neighbours
foreach (NavMeshNode Node in MeshNodes)
{
//Get the NavMeshVertex for each of the 3 points of this node
NavMeshVertex Vertex1 = VertexForPosition(Node.VertexLocations[0]);
NavMeshVertex Vertex3 = VertexForPosition(Node.VertexLocations[2]);
NavMeshVertex Vertex2 = VertexForPosition(Node.VertexLocations[1]);
//Assign these all as neighbours to one another
Vertex1.LinkVertices(Vertex2);
Vertex1.LinkVertices(Vertex3);
Vertex2.LinkVertices(Vertex3);
//Store these vertices inside their parent mesh node
Node.NodeVertices.Add(Vertex1);
Node.NodeVertices.Add(Vertex2);
Node.NodeVertices.Add(Vertex3);
}
}
//Constructs a pathway searching through the vertices in the nav mesh
public static List <Vector3> ConstructVertexPathway(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);
//Define new NavMeshVertexs placed at the pathway start and end locations
NavMeshVertex StartVertex = new NavMeshVertex(PathStart);
NavMeshVertex EndVertex = new NavMeshVertex(PathEnd);
//Link the starting vertex to the vertices in the start node
StartVertex.AddNeighbours(StartNode.NodeVertices);
//The set of vertices already evaluated
List <NavMeshVertex> ClosedSet = new List <NavMeshVertex>();
//The set of currently discovered vertices that are not evaluated yet.
//Initially, only the start vertex is known.
List <NavMeshVertex> OpenSet = new List <NavMeshVertex>();
OpenSet.Add(StartVertex);
//Reset the pathfinding values of all the vertices in the nav mesh
foreach (NavMeshVertex Vertex in NavMesh.MeshVertices)
{
Vertex.ResetPathfindingValues();
}
//Precalculate the values for the starting node, as the cost to travel to itself is zero
StartVertex.GScore = 0;
StartVertex.FScore = StartVertex.HeuristicCost(EndVertex);
//Iterate 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 vertex, member of OpenSet with the lowest FScore value
NavMeshVertex CurrentVertex = OpenSet[0];
for (int i = 1; i < OpenSet.Count; i++)
{
if (OpenSet[i].FScore < CurrentVertex.FScore)
{
CurrentVertex = OpenSet[i];
}
}
//If the new current vertex is one of the vertices making up the end node, then the pathway is complete
if (EndNode.NodeVertices.Contains(CurrentVertex))
{
MessageLog.Print("Pathfinding.AStarSearch pathway found");
//Start from the end vertex and follow its parents back all the way to the start
List <Vector3> Pathway = new List <Vector3>();
NavMeshVertex CurrentStep = CurrentVertex;
while (CurrentStep != StartVertex)
{
Pathway.Add(CurrentStep.VertexLocation);
CurrentStep = CurrentStep.Parent;
}
Pathway.Reverse();
return(Pathway);
}
//Move the new current vertex over to the closed set as we are now going to compute all possible pathways over it
OpenSet.Remove(CurrentVertex);
ClosedSet.Add(CurrentVertex);
//Iterate over each neighbour of the current vertex to check if thats a cheaper way to travel to the target location
foreach (NavMeshVertex Neighbour in CurrentVertex.VertexNeighbours)
{
//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 vertex
float GScore = CurrentVertex.GScore + Vector3.Distance(CurrentVertex.VertexLocation, Neighbour.VertexLocation);
//Add newly discovered vertices 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 = CurrentVertex;
Neighbour.GScore = GScore;
Neighbour.FScore = Neighbour.GScore + Neighbour.HeuristicCost(EndVertex);
}
}
MessageLog.Print("Pathfinding.AStarSearch no pathway found");
return(null);
}
