public void Update(float dt)
{
Position = entity.Position;
Rotation = entity.Orientation;
//Check out distance from the node at the front of the pathway
float NodeDistance = Vector3.Distance(entity.Position, EntityPath[0].NodeLocation);
//Once we are close enough to the next node, then we remove it from the list and proceed to the next one
if (NodeDistance <= 0.05f)
{
//If we have reached the final node in the path, then we want to calculate a new path back were we just came from
if (EntityPath.Count == 1)
{
if (PreviousTargetNode == EndNode)
{
EntityPath = AStarSearch.FindPath(EndNode, StartNode, new Vector2(9, 9));
PreviousTargetNode = StartNode;
}
else
{
EntityPath = AStarSearch.FindPath(StartNode, EndNode, new Vector2(9, 9));
PreviousTargetNode = EndNode;
}
}
//Otherwise we remove this node from the list and move to the next one
EntityPath.Remove(EntityPath[0]);
}
//Continue on towards out current target
pathTime += Globals.space.TimeStepSettings.TimeStepDuration;
mover.TargetPosition = EntityPath[0].NodeLocation;
}
public static List <NavMeshNode> GetNeighbours(NavMeshNode TargetNode, Vector2 MeshSize, bool IncludeDiagonals)
{
Vector2 TargetNodeLocation = TargetNode.NodeIndex;
List <NavMeshNode> Neighbours = new List <NavMeshNode>();
//north neighbour
if (TargetNodeLocation.Y + 1 < MeshSize.Y)
{
Neighbours.Add(MeshDictionary[new Vector2(TargetNodeLocation.X, TargetNodeLocation.Y + 1)]);
}
//east
if (TargetNodeLocation.X + 1 < MeshSize.X)
{
Neighbours.Add(MeshDictionary[new Vector2(TargetNodeLocation.X + 1, TargetNodeLocation.Y)]);
}
//south
if (TargetNodeLocation.Y - 1 > 0)
{
Neighbours.Add(MeshDictionary[new Vector2(TargetNodeLocation.X, TargetNodeLocation.Y - 1)]);
}
//west
if (TargetNodeLocation.X - 1 > 0)
{
Neighbours.Add(MeshDictionary[new Vector2(TargetNodeLocation.X - 1, TargetNodeLocation.Y)]);
}
if (IncludeDiagonals)
{
//North-East
if (TargetNodeLocation.Y + 1 < MeshSize.Y && TargetNodeLocation.X + 1 < MeshSize.X)
{
Neighbours.Add(MeshDictionary[new Vector2(TargetNodeLocation.X + 1, TargetNodeLocation.Y + 1)]);
}
//South-East
if (TargetNodeLocation.X + 1 < MeshSize.X && TargetNodeLocation.Y - 1 > 0)
{
Neighbours.Add(MeshDictionary[new Vector2(TargetNodeLocation.X + 1, TargetNodeLocation.Y - 1)]);
}
//South-West
if (TargetNodeLocation.X - 1 > 0 && TargetNodeLocation.Y - 1 > 0)
{
Neighbours.Add(MeshDictionary[new Vector2(TargetNodeLocation.X - 1, TargetNodeLocation.Y - 1)]);
}
//North-West
if (TargetNodeLocation.X - 1 > 0 && TargetNodeLocation.Y + 1 < MeshSize.Y)
{
Neighbours.Add(MeshDictionary[new Vector2(TargetNodeLocation.X - 1, TargetNodeLocation.Y + 1)]);
}
}
return(Neighbours);
}
//Returns whatever nav mesh node is the closest to the target location
public static NavMeshNode GetNearbyMeshNode(Vector3 NodeLocation)
{
NavMeshNode NearbyNode = MeshNodes[0];
float NodeDistance = Vector3.Distance(NearbyNode.NodeLocation, NodeLocation);
for (int i = 1; i < MeshNodes.Count; i++)
{
NavMeshNode NodeCompare = MeshNodes[i];
float CompareDistance = Vector3.Distance(NodeCompare.NodeLocation, NodeLocation);
if (CompareDistance < NodeDistance)
{
NearbyNode = NodeCompare;
NodeDistance = CompareDistance;
}
}
return(NearbyNode);
}
public static NavMeshNode GetCurrentNode(List <NavMeshNode> OpenSet)
{
NavMeshNode CurrentNode = OpenSet[0];
float CurrentFScore = CurrentNode.FScore;
for (int i = 1; i < OpenSet.Count; i++)
{
NavMeshNode CompareNode = OpenSet[i];
float CompareFScore = CompareNode.FScore;
//We are trying to find which node has the lowest FScore value
if (CompareFScore < CurrentFScore)
{
CurrentNode = CompareNode;
CurrentFScore = CompareFScore;
}
}
return(CurrentNode);
}
public FSMEntity(Vector3 StartPosition, Vector3 EndPosition)
{
StartNode = NavMeshNodes.GetNearbyMeshNode(StartPosition);
EndNode = NavMeshNodes.GetNearbyMeshNode(EndPosition);
EntityPath = AStarSearch.FindPath(StartNode, EndNode, new Vector2(9, 9));
PreviousTargetNode = EndNode;
Position = StartPosition;
entity = new Entity(new Box(StartPosition, 1, 1, 1, 1).CollisionInformation, 1);
entity.Position = StartPosition;
Globals.space.Add(entity);
Globals.game.ModelDrawer.Add(entity);
mover = new EntityMover(entity);
rotator = new EntityRotator(entity);
Globals.space.Add(mover);
Globals.space.Add(rotator);
}
public static void AddNode(NavMeshNode Node, Vector2 Index)
{
MeshDictionary.Add(Index, Node);
}
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>());
}
protected override void Update(GameTime gameTime)
{
PreviousKeyboardInput = KeyboardInput;
KeyboardInput = Keyboard.GetState();
var dt = (float)gameTime.ElapsedGameTime.TotalSeconds;
PreviousMouseInput = MouseInput;
MouseInput = Mouse.GetState();
//Keep the mouse within the screen
if (!IsMouseVisible)
{
Mouse.SetPosition(200, 200);
}
//Allow quit with escape key
if (KeyboardInput.IsKeyDown(Keys.Escape))
{
Exit();
}
//Display camera location and facing direction with F1
if (WasKeyPressed(Keys.F1))
{
Vector3 CameraPosition = ServerSimulation.ServerCamera.Camera.Position;
Console.WriteLine("Cam: " + CameraPosition);
}
////raycast down to find terrain location
//if (WasKeyPressed(Keys.F2))
//{
// //Get the fox entity that we want to raycast from
// ServerEntity Fox = EntityManager.GetServerEntity("PrincessFox");
// Console.WriteLine("Fox is at " + Fox.entity.Position);
// Vector3 RaycastOrigin = Fox.entity.Position;
// Vector3 RaycastDirection = Vector3.Down;
// RayCastResult Results;
// bool RaycastHitSomething = Globals.space.RayCast(new Ray(RaycastOrigin, RaycastDirection), 10000, out Results);
// Vector3 HitLocation = Results.HitData.Location;
// Console.WriteLine("Ground raycast hit at " + HitLocation);
// Vector3[] TVerts = Globals.TerrainVerts;
// Console.WriteLine(TVerts.Length + " verts in the terrain");
// //Sort through the list of nav mesh node locations that we loaded from the terrain meshes vertex data
// for (int i = 0; i < TVerts.Length; i++)
// {
// //Make sure locations are unique before adding new
// if (!NavMeshManager.IsNodeLocationAvailable(TVerts[i]))
// continue;
// float VertDistance;
// if (i < TVerts.Length - 1)
// VertDistance = Vector3.Distance(TVerts[i], TVerts[i + 1]);
// else
// VertDistance = Vector3.Distance(TVerts[i], TVerts[i - 1]);
// //Check the location of each vertex to see if we should place a new nav mesh node here or not
// Vector3 NodeLocation = TVerts[i];
// NavMeshManager.AddNewNode(NodeLocation);
// }
// //Map all the nodes into the dictionary by their index in the navmesh
// List<NavMeshNode> NodeList = NavMeshManager.GetNodeList();
// //Dictionary<Vector2, NavMeshNode> NavMeshNodes = new Dictionary<Vector2, NavMeshNode>();
// //Add the first column of nav mesh nodes
// for (int i = 1; i < 10; i++)
// {
// Vector2 NavMeshIndex = new Vector2(i, 1);
// int MeshNodeIndex = (i - 1) * 2;
// NavMeshNode MeshNode = NodeList[MeshNodeIndex];
// MeshNode.NodeIndex = NavMeshIndex;
// NavMeshManager.AddNode(NavMeshIndex, MeshNode);
// }
// //Then add the second column of nodes
// int CurrentNodeIndex = 1;
// for (int i = 1; i < 10; i++)
// {
// Vector2 NavMeshIndex = new Vector2(i, 2);
// int MeshNodeIndex = CurrentNodeIndex;
// CurrentNodeIndex += 2;
// NavMeshNode MeshNode = NodeList[MeshNodeIndex];
// MeshNode.NodeIndex = NavMeshIndex;
// NavMeshManager.AddNode(NavMeshIndex, MeshNode);
// }
// //Now add the rest of the columns (columns 3-9)
// int NodeListIndex = 18;
// for (int CurrentColumn = 3; CurrentColumn < 10; CurrentColumn++)
// {
// for (int j = 1; j < 10; j++)
// {
// Vector2 NavMeshIndex = new Vector2(j, CurrentColumn);
// NavMeshNode MeshNode = NodeList[NodeListIndex];
// MeshNode.NodeIndex = NavMeshIndex;
// NodeListIndex++;
// NavMeshManager.AddNode(NavMeshIndex, MeshNode);
// }
// }
// //Get the path start and end nodes
// Vector2 PathStartIndex = new Vector2(3, 4);
// NavMeshNode PathStartNode = NavMeshManager.GetNode(PathStartIndex);
// //NavMeshNode PathStartNode = NavMeshNodes[PathStartIndex];
// Vector2 PathEndIndex = new Vector2(7, 8);
// //NavMeshNode PathEndNode = NavMeshNodes[PathEndIndex];
// NavMeshNode PathEndNode = NavMeshManager.GetNode(PathEndIndex);
// //Use A* Search to find a path between the start and end nodes
// List<NavMeshNode> NodePath = AStarSearch.FindPath(PathStartNode, PathEndNode, new Vector2(9, 9));
// //Assign this path to the princess fox entity, have it navigate along the path until it reaches its target
//}
//returns which nav mesh node in the list is closest to the given location
NavMeshNode GetClosestNode(List <NavMeshNode> NodeList, Vector3 NodeLocation)
{
//Start off with the first node in the list being the closest node
NavMeshNode ClosestNode = NodeList[0];
float NodeDistance = Vector3.Distance(ClosestNode.NodeLocation, NodeLocation);
//Compare all the others keep track of which is the closest of them all
for (int i = 1; i < NodeList.Count; i++)
{
NavMeshNode NodeCompare = NodeList[i];
float CompareDistance = Vector3.Distance(ClosestNode.NodeLocation, NodeCompare.NodeLocation);
if (CompareDistance < NodeDistance)
{
ClosestNode = NodeCompare;
NodeDistance = CompareDistance;
}
}
return(ClosestNode);
}
//move fox forward
if (WasKeyPressed(Keys.NumPad8))
{
ServerEntity Fox = EntityManager.GetServerEntity("PrincessFox");
}
//Tab toggle mouse lock
if (WasKeyPressed(Keys.Tab))
{
IsMouseVisible = !IsMouseVisible;
}
//Toggle wireframe with G
if (WasKeyPressed(Keys.G))
{
ModelDrawer.IsWireframe = !ModelDrawer.IsWireframe;
}
//Allow control console to do whatever it wants
UpdateControlConsole(dt);
//Update everything
ServerSimulation.Update(dt);
//Render everything
if (displayConstraints)
{
ConstraintDrawer.Update();
}
if (displayEntities)
{
ModelDrawer.Update();
}
base.Update(gameTime);
}
//Adds a new mesh node objects to the list
public static void AddNode(NavMeshNode Node)
{
MeshNodes.Add(Node);
}
//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
}