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 }