/// <summary>
/// Simple path following. If within "snap distance" of a the nextGoal (a NavNode)
/// move to the NavNode, get a new nextGoal, turnToFace() that goal. Otherwise
/// continue making steps towards the nextGoal.
/// </summary>
public override void Update(GameTime gameTime)
{
KeyboardState keyboardState = Keyboard.GetState();
float distance = 0;
if (keyboardState.IsKeyDown(Keys.N) && !oldKeyboardState.IsKeyDown(Keys.N) && !pathFinding)
{
// toggles path finding on, disables treasure hunting
pathFinding = true;
System.Diagnostics.Debug.WriteLine("N: on");
// nextGoal = path.CurrentNode;
agentObject.defaultSpeed();
}
else if (keyboardState.IsKeyDown(Keys.N) && !oldKeyboardState.IsKeyDown(Keys.N) && pathFinding)
{
// toggles path finding off, enables treasure hunting
System.Diagnostics.Debug.WriteLine("N: off");
pathFinding = false;
if (this.TreasureList.Count > 0 && onPath)
{
onPath = false;
resume = new NavNode(agentObject.Translation);
Goal = aPath.ClosestNode(0); // searches for closest treasure and creates Goal Node
treasurePath = aPath.createPath(Goal); // creates A* Path for Goal node
nextGoal = treasurePath.NextNode;
}
}
agentObject.turnToFace(nextGoal.Translation); // adjust to face nextGoal every move
//agentObject.turnTowards(nextGoal.Translation);
// See if at or close to nextGoal, distance measured in 2D xz plane
distance = Vector3.Distance(
new Vector3(nextGoal.Translation.X, 0, nextGoal.Translation.Z),
new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z));
if (!pathFinding && !(this.TreasureList.Count > 0))
{
agentObject.Step = 0;
}
else
{
if (pathFinding && onPath)
{
if (distance <= snapDistance)
{
// snap to nextGoal and orient toward the new nextGoal
nextGoal = path.NextNode;
//agentObject.turnTowards(nextGoal.Translation);
}
if (this.TreasureList.Count > 0)
{
Goal = aPath.ClosestNode(4000); // searches for treasure within 4000 pixels
if (Goal != null)
{
pathFinding = false;
onPath = false;
resume = new NavNode(agentObject.Translation);
treasurePath = aPath.createPath(Goal); // creates A* Path for Goal node
nextGoal = treasurePath.NextNode;
}
}
}
else // if there are still more treasures NPAgent follows A* Path
// if it gets close enough to tag the treaure it turns back
// quick fix for occasionly getting stuck on walls
{
if (!restart && Goal.DistanceBetween(agentObject.Translation, spacing) < collectDistance)
{
Goal = resume; // creates an A* Path to the previous pathfinding node
treasurePath = aPath.createPath(Goal);
restart = true;
}
else if (restart && Goal.DistanceBetween(agentObject.Translation, spacing) < collectDistance)
{
restart = false;
pathFinding = true;
onPath = true;
nextGoal = resume; // if the NPAgent returns to previous path node it returns pathfinding mode
}
else if (distance <= snapDistance)
{
// snap to nextGoal and orient toward the new nextGoal
nextGoal = treasurePath.NextNode;
// agentObject.turnToFace(nextGoal.Translation);
}
}
}
// Display information
stage.setInfo(15, stage.agentLocation(this));
stage.setInfo(16,
string.Format(" nextGoal ({0:f0}, {1:f0}, {2:f0}) distance to next goal = {3,5:f2})",
nextGoal.Translation.X / stage.Spacing, nextGoal.Translation.Y, nextGoal.Translation.Z / stage.Spacing, distance));
oldKeyboardState = keyboardState;
base.Update(gameTime); // Agent's Update();
}
// Wikipedia pseudo code implementation
public Path createPath(NavNode target)
{
//stage.Terrain.Multiplier = 0;
Path aPath = null;
Vector3 pos = new Vector3((int)agentObject.Translation.X / spacing,
(int)agentObject.Translation.Y, (int)agentObject.Translation.Z / spacing);
NavNode start = new NavNode(pos);
pos = new Vector3((int)target.Translation.X / spacing,
(int)target.Translation.Y, (int)target.Translation.Z / spacing);
NavNode goal = new NavNode(pos);
NavNode current = start;
int tentative_gScore;
// The set of acceptable moves: up, down, left, right, diagonals
int[,] add = { { 0, 1 }, { 0, -1 }, { 1, 0 }, { -1, 0 },
{ 1, 1 }, { -1, -1 }, { 1, -1 }, { -1, 1 } };
// The set of nodes already evaluated
List <NavNode> closedSet = new List <NavNode>();
// The set of currently discovered nodes that are not evaluated yet.
// Initially, only the start node is known.
List <NavNode> openSet = new List <NavNode>();
openSet.Add(start);
// The set of neighbors on the current node
List <NavNode> neighborSet = new List <NavNode>();
// For each node, which node it can most efficiently be reached from.
// If a node can be reached from many nodes, cameFrom will eventually contain the
// most efficient previous step.
NavNode[,] cameFrom = new NavNode[stage.Range + 1, stage.Range + 1]; //an empty map
// For each node, the cost of getting from the start node to that node.
int[,] gScore = new int[stage.Range + 1, stage.Range + 1]; // map with default value of Infinity
for (int i = 0; i < stage.Range + 1; i++)
{
for (int j = 0; j < stage.Range + 1; j++)
{
gScore[i, j] = int.MaxValue;
}
}
// The cost of going from start to start is zero.
gScore[(int)start.Translation.X, (int)start.Translation.Z] = 0;
// For each node, the total cost of getting from the start node to the goal
// by passing by that node. That value is partly known, partly heuristic.
int[,] fScore = new int[stage.Range + 1, stage.Range + 1]; // map with default value of Infinity
for (int i = 0; i < stage.Range + 1; i++)
{
for (int j = 0; j < stage.Range + 1; j++)
{
fScore[i, j] = int.MaxValue;
}
}
// For the first node, that value is completely heuristic.
fScore[(int)start.Translation.X, (int)start.Translation.Z] = start.Heuristic(goal);
while (openSet.Count > 0)
{
current = openSet[0];
for (int i = 1; i < openSet.Count; i++)
{
if (fScore[(int)current.Translation.X, (int)current.Translation.Z]
> fScore[(int)openSet[i].Translation.X, (int)openSet[i].Translation.Z])
{
current = openSet[i];
}
} // current:= the node in openSet having the lowest fScore[] value
if (current.Translation == goal.Translation)
{
aPath = reconstructPath(cameFrom, current);
return(aPath);
}
openSet.Remove(current);
closedSet.Add(current);
//Finds neighbors of current node
neighborSet.Clear();
for (int k = 0; k < 8; k++)
{
pos = new Vector3((int)current.Translation.X + add[k, 0], (int)goal.Translation.Y,
(int)current.Translation.Z + add[k, 1]);
Object3D obj3d = agentObject.CollidedWith(pos * spacing);
NavNode neighbor = new NavNode(pos);
if (Invalid(neighbor))
{
continue;
}
if (Exists(closedSet, neighbor))
{
continue;
}
neighborSet.Add(neighbor);
if (obj3d != null)
{
// If the neighbor is an obstacle then skip
if (stage.SameType(obj3d.Name, "wall") || stage.SameType(obj3d.Name, "temple"))
{
neighborSet.Remove(neighbor);
if (Exists(closedSet, neighbor))
{
continue;
}
closedSet.Add(neighbor);
// removes neighbors of a wall to decrease chance of collision
//for (int j = 0; j < 8; j++)
//{
// pos = new Vector3((int)current.Translation.X + add[j, 0], (int)goal.Translation.Y,
// (int)current.Translation.Z + add[j, 1]);
// obj3d = agentObject.CollidedWith(pos * spacing);
// neighbor = new NavNode(pos);
// if (Exists(closedSet, neighbor))
// continue;
// closedSet.Add(neighbor);
//}
}
// finishes earlier
//else if (stage.SameType(obj3d.Name, "treasure")) {
// if (neighbor.Translation == goal.Translation) {
// aPath = reconstructPath(cameFrom, current);
// return aPath;
// }
//}
}
}
foreach (NavNode neighbor in neighborSet)
{
if (Exists(openSet, neighbor))
{
continue;
}
openSet.Add(neighbor); // Discover a new node
// The distance from start to a neighbor
// the "dist_between" function may vary as per the solution requirements.
tentative_gScore = gScore[(int)current.Translation.X, (int)current.Translation.Z]
+ current.DistanceBetween(neighbor);
if (tentative_gScore >= gScore[(int)neighbor.Translation.X, (int)neighbor.Translation.Z])
{
continue; // This is not a better path.
}
// This path is the best until now. Record it!
cameFrom[(int)neighbor.Translation.X, (int)neighbor.Translation.Z] = current;
gScore[(int)neighbor.Translation.X, (int)neighbor.Translation.Z] = tentative_gScore;
fScore[(int)neighbor.Translation.X, (int)neighbor.Translation.Z]
= gScore[(int)neighbor.Translation.X, (int)neighbor.Translation.Z]
+ neighbor.Heuristic(goal);
}
}
return(null);
}