public static List <NavMeshNode> FindPath(NavMeshNode StartNode, NavMeshNode EndNode, Vector2 NavMeshSize)
{
//Initialize the Open and Closed sets
ClosedSet = new List <NavMeshNode>();
OpenSet = new List <NavMeshNode>();
OpenSet.Add(StartNode);
//Reset nodes CameFrom references in preperation for finding our new pathway
NavMeshDictionary.ResetPathfinding();
//Cost of travelling from the start node to the start node is 0
StartNode.GScore = 0;
//The FScore for the starting node should be completely heuristic
StartNode.FScore = Vector3.Distance(StartNode.NodeLocation, EndNode.NodeLocation);
//Iterate over the OpenSet until there are no more nodes left to evaluate
while (OpenSet.Count > 0)
{
//The current node each time will be the node in OpenSet with the lowest FScore
NavMeshNode CurrentNode = GetCurrentNode(OpenSet);
//If this is the end node then the path is ready
if (CurrentNode == EndNode)
{
//When the path is found, we start at the end node, and keep following back each nodes CameFrom reference, until we end up at the start node
List <NavMeshNode> FinishedPath = new List <NavMeshNode>();
FinishedPath.Add(EndNode);
NavMeshNode PathNode = EndNode;
while (PathNode != StartNode)
{
PathNode = PathNode.CameFrom;
FinishedPath.Add(PathNode);
}
FinishedPath.Add(StartNode);
FinishedPath.Reverse();
return(FinishedPath);
}
//Move the current node over to the closed set
OpenSet.Remove(CurrentNode);
ClosedSet.Add(CurrentNode);
//Find all of the current nodes neighbours
List <NavMeshNode> CurrentNeighbours = NavMeshDictionary.GetNeighbours(CurrentNode, new Vector2(9, 9), true);
for (int i = 0; i < CurrentNeighbours.Count; i++)
{
//Check through all of them
NavMeshNode CurrentNeighbour = CurrentNeighbours[i];
//Ignore neighbours in the closed set as they have already been evaluated
if (ClosedSet.Contains(CurrentNeighbour))
{
continue;
}
//Calculate the distance from start to this neighbour
float TentativeGScore = CurrentNode.GScore + Vector3.Distance(CurrentNode.NodeLocation, CurrentNeighbour.NodeLocation);
//Add all newly discovered nodes into the open set
if (!OpenSet.Contains(CurrentNeighbour))
{
OpenSet.Add(CurrentNeighbour);
}
//make sure this node isnt more expensive to travel from
else if (TentativeGScore >= CurrentNeighbour.GScore)
{
continue;
}
//If we get this far the current neighbour has been found to be the cheapest node to travel from
CurrentNeighbour.CameFrom = CurrentNode;
CurrentNeighbour.GScore = TentativeGScore;
CurrentNeighbour.FScore = CurrentNeighbour.GScore + Vector3.Distance(CurrentNeighbour.NodeLocation, EndNode.NodeLocation);
}
}
return(new List <NavMeshNode>());
}
//public ServerEntity WaypointTest;
public WorldSimulator(WorldRenderer game)
{
Game = game;
parallelLooper = new ParallelLooper();
if (Environment.ProcessorCount > 1)
{
for (int i = 0; i < Environment.ProcessorCount; i++)
{
parallelLooper.AddThread();
}
}
Globals.space = new Space(parallelLooper);
game.Camera.LockedUp = Vector3.Up;
game.Camera.ViewDirection = new Vector3(0, -1.5f, 1);
game.Camera.Position = new Vector3(-19.55f, 39.25f, -10.35f);
ServerCamera = new FreeCamera(100, game.Camera, game);
//Add some force of gravity to the simulation
Globals.space.ForceUpdater.Gravity = new Vector3(0, -9.81f, 0);
//Load the world terrain mesh data
Vector3[] TerrainVertices;
int[] TerrainIndices;
Model TerrainCollision = Globals.game.Content.Load <Model>("LowDetailTerrain");
ModelDataExtractor.GetVerticesAndIndicesFromModel(TerrainCollision, out TerrainVertices, out TerrainIndices);
StaticMesh TerrainMesh = new StaticMesh(TerrainVertices, TerrainIndices, new AffineTransform(new Vector3(0, 0, 0)));
Globals.TerrainVerts = TerrainVertices;
Globals.TerrainInds = TerrainIndices;
Globals.space.Add(TerrainMesh);
Globals.game.ModelDrawer.Add(TerrainMesh);
//Create a new mesh node object for each unique location in the navigation mesh
for (int i = 0; i < Globals.TerrainVerts.Length; i++)
{
//Dont create mesh nodes at locations already been used
if (!NavMeshNodes.IsLocationAvailable(Globals.TerrainVerts[i]))
{
continue;
}
//Add a new mesh node object to every other space in the terrain verts that we find to be available
NavMeshNodes.AddNode(new NavMeshNode(Globals.TerrainVerts[i]));
}
//Now sort all of the mesh nodes into a dictionary sorted by their index in the level
//Start with the first column of mesh nodes
for (int i = 1; i < 10; i++)
{
Vector2 MeshIndex = new Vector2(i, 1);
int MeshNodeIndex = (i - 1) * 2;
NavMeshNode MeshNode = NavMeshNodes.MeshNodes[MeshNodeIndex];
NavMeshDictionary.AddNode(MeshNode, MeshIndex);
}
//Then the second column of mesh nodes
int CurrentNodeIndex = 1;
for (int i = 1; i < 10; i++)
{
Vector2 MeshIndex = new Vector2(i, 2);
int NodeIndex = CurrentNodeIndex;
CurrentNodeIndex += 2;
NavMeshNode MeshNode = NavMeshNodes.MeshNodes[NodeIndex];
MeshNode.NodeIndex = MeshIndex;
NavMeshDictionary.AddNode(MeshNode, MeshIndex);
}
//Add the other remaining columns
int ListIndex = 18;
for (int Column = 3; Column < 10; Column++)
{
for (int j = 1; j < 10; j++)
{
Vector2 MeshIndex = new Vector2(j, Column);
NavMeshNode MeshNode = NavMeshNodes.MeshNodes[ListIndex];
MeshNode.NodeIndex = MeshIndex;
ListIndex++;
NavMeshDictionary.AddNode(MeshNode, MeshIndex);
}
}
//Add two game entities into the scene, the first entity will pathfind its way to the 2nd entity
PrincessFox = new GameEntity(new Vector3(-17.74f, 2.15f, 10.54f));
PrincessTarget = new GameEntity(new Vector3(-41.17f, 0.89f, 24.15f));
//Figure out which node each of these entities is closest two, these will be the ends of the pathway
NavMeshNode StartNode = NavMeshDictionary.MeshDictionary[new Vector2(3, 4)];
NavMeshNode EndNode = NavMeshDictionary.MeshDictionary[new Vector2(7, 8)];
//Find our pathway between the two entities
List <NavMeshNode> NodePath = AStarSearch.FindPath(StartNode, EndNode, new Vector2(9, 9));
//Now assign this path to the princess fox entity and have her navigate along to reach her target
}