public void LoadNewMapNodes(List<Vector3> nodePositions)
{
//erase old blocks
m_Nodes.Clear();
foreach (Vector3 position in nodePositions)
{
//create new node
Node newNode = new Node(m_NextId++);
//load content
newNode.LoadContent(Content, "Models//navNode");
//set position
newNode.SetPosition(position);
//add node
m_Nodes.Add(newNode);
}
// Connect all the nodes to each other
foreach (Node node in m_Nodes)
{
// Connect possible left node
foreach (Node connNode in m_Nodes)
{
if (connNode.Position.X == node.Position.X + 6
&& connNode.Position.Z == node.Position.Z)
{
node.adjNodes.Add(connNode);
}
else if (connNode.Position.X == node.Position.X
&& connNode.Position.Z == node.Position.Z + 6)
{
node.adjNodes.Add(connNode);
}
else if (connNode.Position.X == node.Position.X - 6
&& connNode.Position.Z == node.Position.Z)
{
node.adjNodes.Add(connNode);
}
else if (connNode.Position.X == node.Position.X
&& connNode.Position.Z == node.Position.Z - 6)
{
node.adjNodes.Add(connNode);
}
}
}
}
public Agent(Map world, int id, bool outputGenes)
: base()
{
//heading is initially 0
m_Heading = 0.0f;
//side is 90 degrees CW from heading
m_Side = m_Heading - (float)Math.PI / 2.0f;
//velocity, acceleration, speed are initially zero
m_Velocity = Vector3.Zero;
m_Acceleration = Vector3.Zero;
m_Speed = 0.0f;
m_TurnAmount = 0.0f;
//init max speed
m_MaxSpeed = MAX_SPEED;
m_AgentAdjacencyList = new List<Tuple<Agent, float, float>>();
//agent is initially inactive
m_IsActive = false;
//sensory radius for adjacent agents and pie slice
m_SensoryRadius = 400;
//do no initially output info
m_OutputAdjacentAgents = false;
m_OutputInfo = false;
m_OutputRayInfo = false;
//init agent's id
this.m_ID = id;
m_SteeringBehaviors = new SteeringBehaviors(this);
m_World = world;
m_Brain = new NeuralNetwork();
m_OutputGenes = outputGenes;
if (m_OutputGenes == true)
{
m_GeneticFile = new System.IO.StreamWriter("Agent" + Id + ".gene");
}
m_MemoryOutputs = new List<double>();
m_MemoryOutputs.Add(0);
m_MemoryOutputs.Add(0);
m_MemoryOutputs.Add(0);
m_MemoryOutputs.Add(0);
m_Fitness = 0;
//init rangefinder list
InitializeRangefinderWalls();
InitializeRangefinderBlocks();
//init pie slices
InitializePieSliceSensors();
//init node adj list
InitializeAdjacencyListNodes();
//init state is navigate
m_CurrentState = new State_Navigate();
m_NavDelay = DateTime.Now;
//begin invulnerability timer
m_Invuln = true;
m_InvulnTimer = DateTime.Now.AddSeconds(12);
// Initialize the Escape node to null to set it later
m_EscapeNode = null;
m_StartingLocation = Vector3.Up;
m_MovementSpeed = MAX_SPEED;
m_TurnSpeed = MAX_TURN_SPEED;
}
public List<Node> createNavPath(Dictionary<Node, Node> lastNode, Node crnt_node)
{
/*
* Construct the navigation graph for the agent to follow.
*/
List<Node> result = new List<Node>();
if (lastNode.ContainsKey(crnt_node))
{
result = createNavPath(lastNode, lastNode[crnt_node]);
result.Add(crnt_node);
return (result);
}
else
{
result.Add(crnt_node);
return (result);
}
}
public List<Node> FindPath(Node startNode, Node endNode)
{
/*
* Determine a navigation path from startNode to endNode using A*.
*/
//System.IO.TextWriter tw = new System.IO.StreamWriter("output.txt");
startNode.g_dist = 0;
startNode.h_dist = Vector3.Distance(startNode.Position, endNode.Position);
startNode.f_dist = startNode.g_dist + startNode.h_dist;
Dictionary<Node, Node> lastNode = new Dictionary<Node, Node>();
List<Node> blockedSet = new List<Node>();
List<Node> potentialSet = new List<Node>();
potentialSet.Add(startNode);
while (potentialSet.Count > 0)
{
Node nodeA = null;
double f_min = Double.MaxValue;
/*
* Choose the shortest path so far to continue from.
*/
//tw.WriteLine("Cycling through...");
foreach (Node tempNode in potentialSet)
{
//tw.WriteLine("Node " + tempNode.id + ", f_dist = " + tempNode.f_dist);
if (tempNode.f_dist < f_min)
{
f_min = tempNode.f_dist;
nodeA = tempNode;
}
}
//tw.WriteLine();
// End when the next node chosen is the endNode
if (nodeA.Equals(endNode))
return (createNavPath(lastNode, endNode));
/*
* Since A is chosen to move on from,
* Block it so it is not chosen again.
*/
potentialSet.Remove(nodeA);
blockedSet.Add(nodeA);
//tw.WriteLine("Checking nodes adjacent to Node " + nodeA.id);
//tw.WriteLine("====================================");
foreach (Node nodeB in nodeA.adjNodes)
{
/*
* Add the next nearest node onto the path
*/
if (!blockedSet.Contains(nodeB))
{
bool addNode;
//tw.WriteLine("Node " + nodeB.id + ": " +
// "{pos - (" + nodeB.Position.X + ", " + nodeB.Position.Y + ", " + nodeB.Position.Z + ")}, " +
// "{g = " + nodeB.g_dist + ", h = " + nodeB.h_dist + ", f = " + nodeB.f_dist + "}");
double g_temp = nodeA.g_dist + Vector3.Distance(nodeA.Position, nodeB.Position);
if (!potentialSet.Contains(nodeB))
{
// Add the node to the potential nodes
// if it is not already there.
potentialSet.Add(nodeB);
addNode = true;
}
else if (g_temp < nodeB.g_dist)
addNode = true;
else
addNode = false;
if (addNode)
{
// Add the node if either it was not in the
// potential node list yet or it was the next
// shortest node to traverse towards the end.
lastNode[nodeB] = nodeA;
nodeB.g_dist = g_temp;
nodeB.h_dist = Vector3.Distance(nodeB.Position, endNode.Position);
nodeB.f_dist = nodeB.g_dist + nodeB.h_dist;
}
}
}
//tw.WriteLine();
//tw.WriteLine();
}
//tw.Close();
return (null);
}
public Node MoveAlongPath(List<Node> path, Node nextNode)
{
/*
* Moves the agent along the generated path
*/
Vector3 headingVector = Vector3.Forward;
Matrix rotMatrix = Matrix.CreateRotationY(Heading);
headingVector = Vector3.Transform(headingVector, rotMatrix);
//get line connecting the current position to the next node
Vector3 nextNodeVector = nextNode.Position - Position;
//find the angle necessary to face the node
double relativeAngle = Math.Atan2(headingVector.Z, headingVector.X) - Math.Atan2(nextNodeVector.Z, nextNodeVector.X);
if (Vector3.Distance(this.Position, nextNode.Position) < 1)
{
nextNode.IsActive = false;
if (!path.ElementAt(path.Count - 1).Equals(nextNode))
{
nextNode = path.ElementAt(path.IndexOf(nextNode) + 1);
nextNode.IsActive = true;
}
return (nextNode);
}
else
{
if (relativeAngle < -0.1 || relativeAngle > 0.1)
{
turn((float)MathHelper.WrapAngle((float)relativeAngle), m_TurnSpeed);
}
//else if (relativeAngle > 0.05)
//{
// turn(-1, Game1.MAX_TURN_SPEED);
//}
else if (relativeAngle > -0.1 && relativeAngle < 0.1)
{
Matrix orientationMatrix = Matrix.CreateRotationY(Heading);
Vector3 predictedPosition = Position;
Vector3 movement = new Vector3(0, 0, -1);
Vector3 speed = Vector3.Normalize(Vector3.Transform(movement, orientationMatrix)) * m_MaxSpeed;
predictedPosition = Position + speed;
BoundingSphere predictedSphere = m_BoundingSphere;
predictedSphere.Center.X = predictedPosition.X;
predictedSphere.Center.Z = predictedPosition.Z;
if (!WallCollision(predictedSphere, m_World.Walls) && !BlockCollision(predictedSphere, m_World.Blocks))
{
m_Position = predictedPosition;
m_BoundingSphere = predictedSphere;
//Console.WriteLine("Updated BoundingSpheres Agent " + m_ID + "\n" + m_Position + "\n" + m_BoundingSphere.Center);
}
}
return (nextNode);
}
}