public Path(Stage theStage, NavNode pathNode, PathType aPathType)
: base(theStage)
{
nextNode = 0;
pathType = aPathType;
stage = theStage;
done = false;
int spacing = stage.Spacing;
// make a simple path, show how to set the type of the NavNode outside of construction.
node = new List <NavNode>();
node.Add(pathNode);
}
// methods
public int DistanceBetween(NavNode n, int spacing = 1)
{
int X = (int)n.Translation.X / spacing;
int Z = (int)n.Translation.Z / spacing;
int x = (int)translation.X / spacing;
int z = (int)translation.Z / spacing;
int A = (int)Math.Pow(x - X, 2);
int B = (int)Math.Pow(z - Z, 2);
int C = (int)Math.Sqrt(A + B);
return(C);
}
public int Heuristic(NavNode goal)
{
// Scale optimzes to specific map
float Scale = 1.75f;
float Cost = heuristicCostEstimate(goal);
float dx = Math.Abs(translation.X - goal.Translation.X);
float dz = Math.Abs(translation.Z - goal.Translation.Z);
// Manhattan (up, down, left, right)
//return (int)(Scale * (dx + dz) + (Cost - 2 * Scale));
// Diagonal (up, down, left, right, diagonal)
return((int)(Scale * (dx + dz) + (Cost - 2 * Scale) * Math.Min(dx, dz)));
}
private void TreasureSeekUpdate(GameTime gameTime)
{
Treasure t = stage.GetClosestTreasure(agentObject.Translation);
if (t == null)
{
// No more treasure. Stop Moving
state = NPAgentState.DONE;
return;
}
if (treasureNav == null || treasureNav.Translation != t.Obj.Translation)
{
treasureNav = new NavNode(t.Obj.Translation, NavNode.NavNodeEnum.WAYPOINT);
agentObject.turnToFace(treasureNav.Translation);
}
ObjectAvoidance();
float distance = Vector3.Distance(
new Vector3(treasureNav.Translation.X, 0, treasureNav.Translation.Z),
new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z));
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})",
treasureNav.Translation.X / stage.Spacing, treasureNav.Translation.Y, treasureNav.Translation.Z / stage.Spacing, distance));
// Add in the bounding sphere radius to make distance 200 from edge of bounding box instead of 200 from center of object
if (distance <= (tagDistance + t.BoundingSphereRadius))
{
//Tag the treasure so it can't be found again.
stage.TagTreasure(t, this);
this.treasuresFound++;
//Found treasure, stop seeking and switch states.
treasureNav = null;
Treasure anyMore = stage.GetClosestTreasure(agentObject.Translation);
if (anyMore != null)
{
state = NPAgentState.PATH_SEEK;
// snap to nextGoal and orient toward the new nextGoal
agentObject.turnToFace(nextGoal.Translation);
}
else
{
state = NPAgentState.DONE;
}
}
}
/// <summary>
/// Useful in A* path finding
/// when inserting into an min priority queue open set ordered on distance
/// </summary>
/// <param name="n"> goal node </param>
/// <returns> usual comparison values: -1, 0, 1 </returns>
public int CompareTo(NavNode n)
{
if (distance < n.Distance)
{
return(-1);
}
else if (distance > n.Distance)
{
return(1);
}
else
{
return(0);
}
}
{ 495, 480 } }; // loop return
/// <summary>
/// Create a NPC.
/// AGXNASK distribution has npAgent move following a Path.
/// </summary>
/// <param name="theStage"> the world</param>
/// <param name="label"> name of </param>
/// <param name="pos"> initial position </param>
/// <param name="orientAxis"> initial rotation axis</param>
/// <param name="radians"> initial rotation</param>
/// <param name="meshFile"> Direct X *.x Model in Contents directory </param>
public NPAgent(Stage theStage, string label, Vector3 pos, Vector3 orientAxis,
float radians, string meshFile)
: base(theStage, label, pos, orientAxis, radians, meshFile)
{ // change names for on-screen display of current camera
first.Name = "npFirst";
follow.Name = "npFollow";
above.Name = "npAbove";
// path is built to work on specific terrain, make from int[x,z] array pathNode
path = new Path(stage, pathNode, Path.PathType.LOOP); // continuous search path
stage.Components.Add(path);
nextGoal = path.NextNode; // get first path goal
agentObject.turnToFace(nextGoal.Translation); // orient towards the first path goal
// set snapDistance to be a little larger than step * stepSize
snapDistance = (int)(1.5 * (agentObject.Step * agentObject.StepSize));
}
// returns the NavNode of the treasure that is closest to the agent
public NavNode ClosestNode(float radius)
{
NavNode node;
Object3D closest = ClosestObject(radius);
if (closest == null)
{
return(null);
}
node = new NavNode(new Vector3(closest.X,
stage.Terrain.surfaceHeight((int)closest.X / spacing,
(int)closest.Y / spacing), closest.Z),
NavNode.NavNodeEnum.WAYPOINT);
return(node);
}
/// <summary>
/// Create a NPC.
/// AGXNASK distribution has npAgent move following a Path.
/// </summary>
/// <param name="theStage"> the world</param>
/// <param name="label"> name of </param>
/// <param name="pos"> initial position </param>
/// <param name="orientAxis"> initial rotation axis</param>
/// <param name="radians"> initial rotation</param>
/// <param name="meshFile"> Direct X *.x Model in Contents directory </param>
public NPAgent(Stage theStage, string label, Vector3 pos, Vector3 orientAxis,
float radians, string meshFile)
: base(theStage, label, pos, orientAxis, radians, meshFile)
{
// change names for on-screen display of current camera
first.Name = "npFirst";
follow.Name = "npFollow";
above.Name = "npAbove";
IsCollidable = true; // agent test collision with Collidable set.
stage.Collidable.Add(agentObject); // agents's agentObject can be collided with by others.
// path is built to work on specific terrain, make from int[x,z] array pathNode
path = new Path(stage, AGProject1.CustomItems.FORWARD ? pathNodeForward : pathNodeBackward, Path.PathType.LOOP); // continuous search path
stage.Components.Add(path);
nextGoal = path.NextNode; // get first path goal
backupNextGoal = new NavNode(nextGoal.Translation);
agentObject.turnToFace(nextGoal.Translation); // orient towards the first path goal
// set snapDistance to be a little larger than step * stepSize
snapDistance = (int)(1.5 * (agentObject.Step * agentObject.StepSize));
}
private void PathSeekUpdate(GameTime gameTime)
{
ObjectAvoidance();
float distance = Vector3.Distance(
new Vector3(nextGoal.Translation.X, 0, nextGoal.Translation.Z),
new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z));
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));
if (distance <= snapDistance)
{
// snap to nextGoal and orient toward the new nextGoal
nextGoal = path.NextNode;
agentObject.turnToFace(nextGoal.Translation);
}
}
public bool Invalid(NavNode target)
{
if (target.Translation.X > stage.Range)
{
return(true);
}
else if (target.Translation.Z > stage.Range)
{
return(true);
}
if (target.Translation.X < 1)
{
return(true);
}
else if (target.Translation.Z < 1)
{
return(true);
}
return(false);
}
/// <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)
{
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
float distance = Vector3.Distance(
new Vector3(nextGoal.Translation.X, 0, nextGoal.Translation.Z),
new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z));
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));
if (distance <= snapDistance)
{
// snap to nextGoal and orient toward the new nextGoal
nextGoal = path.NextNode;
// agentObject.turnToFace(nextGoal.Translation);
}
base.Update(gameTime); // Agent's Update();
}
{ 445, 460 } }; // loop return
/// <summary>
/// Create a NPC.
/// AGXNASK distribution has npAgent move following a Path.
/// </summary>
/// <param name="theStage"> the world</param>
/// <param name="label"> name of </param>
/// <param name="pos"> initial position </param>
/// <param name="orientAxis"> initial rotation axis</param>
/// <param name="radians"> initial rotation</param>
/// <param name="meshFile"> Direct X *.x Model in Contents directory </param>
public NPAgent(Stage theStage, string label, Vector3 pos, Vector3 orientAxis,
float radians, string meshFile)
: base(theStage, label, pos, orientAxis, radians, meshFile)
{ // change names for on-screen display of current camera
IsCollidable = true; // players test collision with Collidable set.
stage.Collidable.Add(agentObject); // player's agentObject can be collided with by others.
InitSensors();
state = NPAgentState.PATH_SEEK;
first.Name = "npFirst";
follow.Name = "npFollow";
above.Name = "npAbove";
// Flip a coin. If 0, Go Normal Path. If 1, reverse order of list (Go backwards)
Random r = new Random();
int coinFlip = r.Next(2);
if (coinFlip == 0)
{
stage.setInfo(17, "Direction: Regular");
path = new Path(stage, pathNode, Path.PathType.LOOP); // continuous search path
}
else
{
stage.setInfo(17, "Direction: Backwards");
path = new Path(stage, pathNode, Path.PathType.BACKWARDS); // continuous search path backwards *ADDITION
}
// path is built to work on specific terrain, make from int[x,z] array pathNode
stage.Components.Add(path);
nextGoal = path.NextNode; // get first path goal
agentObject.turnToFace(nextGoal.Translation); // orient towards the first path goal
// set snapDistance to be a little larger than step * stepSize
snapDistance = (int)(1.5 * (agentObject.Step * agentObject.StepSize));
}
/// <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)
{
float distance, distance2;
// On treasure hunt path.
if (pathToggle == true)
{
// If the treasure has already been found go back to regular path.
if (treasures.getTreasure(treasureIndex).isFound() == true)
{
pathToggle = false;
}
// face target treasure
agentObject.turnToFace(new Vector3(treasures.getTreasure(treasureIndex).xPos *stage.Spacing, 0,
treasures.getTreasure(treasureIndex).zPos *stage.Spacing));
// get distance between npAgent and target treasure.
distance = Vector3.Distance(new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z),
new Vector3(treasures.getTreasure(treasureIndex).xPos *stage.Spacing, 0,
treasures.getTreasure(treasureIndex).zPos *stage.Spacing));
// check if within snap range
if (distance < 400)
{
// increment number of collected treasures
collectedTreasures++;
Console.WriteLine("Treasure Count: " + collectedTreasures);
treasures.getTreasure(treasureIndex).found = true;
// Stop moving if all four treasures have been updated.
// It would be better to use variables then a specific number for this, so this is something to fix.
if (collectedTreasures == 4)
{
instance[0].Step = 0;
}
pathToggle = false;
}
}
// Not on treasure hunt path.
if (pathToggle == false)
{
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));
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));
if (distance <= snapDistance)
{
// snap to nextGoal and orient toward the new nextGoal
nextGoal = path.NextNode;
// agentObject.turnToFace(nextGoal.Translation);
}
// Do a check to see if any treasure is withing 4000px deteciton range.
for (int i = 0; i < 4; i++)
{
if (treasures.getTreasure(i).isFound() == true)
{
continue;
}
distance = Vector3.Distance(new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z),
new Vector3(treasures.getTreasure(i).xPos *stage.Spacing, 0,
treasures.getTreasure(i).zPos *stage.Spacing));
// If within detection range, start doing automatic treasure hunt.
if (distance < 4000)
{
pathToggle = true;
treasureIndex = i;
}
}
}
KeyboardState keyboardState = Keyboard.GetState();
// If we press N, then we wish to change to treasure seeking path.
if (keyboardState.IsKeyDown(Keys.N) && !oldKeyboardState.IsKeyDown(Keys.N))
{
// set distance to some max value.
distance = float.MaxValue;
// Iterate through list of treasures
for (int i = 0; i < 4; i++)
{
// Check if we have already found a given treasure.
// If so, skip it.
if (treasures.getTreasure(i).isFound() == true)
{
continue;
}
// Get the distance between agent and treasure[i].
distance2 = Vector3.Distance(
new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z),
new Vector3(treasures.getTreasure(i).xPos, 0, treasures.getTreasure(i).zPos));
// this makes sure we get the closest treasure.
if (distance2 < distance)
{
treasureIndex = i;
distance = distance2;
}
}
// If we found a new treasure, then toggle treasure path.
if (distance != float.MaxValue)
{
pathToggle = true;
}
}
base.Update(gameTime); // Agent's Update();
}
/// <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)
{
// If the NPAgent should begin searching for a treasure
if (!AGProject1.CustomItems.NPSeekingTreasure)
{
// find closest non-activated treasure
// get distances to each treasure
float t1distance = Vector2.Distance(new Vector2(this.agentObject.Translation.X, this.agentObject.Translation.Z), new Vector2(AGProject1.CustomItems.TREASURE_ONE_LOCATION[0] * stage.Spacing, AGProject1.CustomItems.TREASURE_ONE_LOCATION[1] * stage.Spacing));
float t2distance = Vector2.Distance(new Vector2(this.agentObject.Translation.X, this.agentObject.Translation.Z), new Vector2(AGProject1.CustomItems.TREASURE_TWO_LOCATION[0] * stage.Spacing, AGProject1.CustomItems.TREASURE_TWO_LOCATION[1] * stage.Spacing));
float t3distance = Vector2.Distance(new Vector2(this.agentObject.Translation.X, this.agentObject.Translation.Z), new Vector2(AGProject1.CustomItems.TREASURE_THREE_LOCATION[0] * stage.Spacing, AGProject1.CustomItems.TREASURE_THREE_LOCATION[1] * stage.Spacing));
float t4distance = Vector2.Distance(new Vector2(this.agentObject.Translation.X, this.agentObject.Translation.Z), new Vector2(AGProject1.CustomItems.TREASURE_FOUR_LOCATION[0] * stage.Spacing, AGProject1.CustomItems.TREASURE_FOUR_LOCATION[1] * stage.Spacing));
// put distances into an array, then sort
Tuple <float, AGProject1.Treasure> tup1dist = new Tuple <float, AGProject1.Treasure>(t1distance, stage.treasure1);
Tuple <float, AGProject1.Treasure> tup2dist = new Tuple <float, AGProject1.Treasure>(t2distance, stage.treasure2);
Tuple <float, AGProject1.Treasure> tup3dist = new Tuple <float, AGProject1.Treasure>(t3distance, stage.treasure3);
Tuple <float, AGProject1.Treasure> tup4dist = new Tuple <float, AGProject1.Treasure>(t4distance, stage.treasure4);
Tuple <float, AGProject1.Treasure>[] distances = { tup1dist, tup2dist, tup3dist, tup4dist };
Array.Sort(distances, new AGProject1.TreasureTupleComparer());
Console.WriteLine("" + distances[0].Item1 + ":" + distances[0].Item2.Name + " " + distances[1].Item1 + ":" + distances[1].Item2.Name + " " + distances[2].Item1 + ":" + distances[2].Item2.Name + " " + distances[3].Item1 + ":" + distances[3].Item2.Name);
// set next target to treasure
if (!distances[0].Item2.Activated() && (AGProject1.CustomItems.TreasureMode || distances[0].Item1 <= AGProject1.CustomItems.SEARCH_DISTANCE))
{
nextGoal = new NavNode(new Vector3(distances[0].Item2.Object.Translation.X, 0, distances[0].Item2.Object.Translation.Z));
treasureGoal = distances[0].Item2;
AGProject1.CustomItems.NPSeekingTreasure = true;
}
else if (!distances[1].Item2.Activated() && (AGProject1.CustomItems.TreasureMode || distances[1].Item1 <= AGProject1.CustomItems.SEARCH_DISTANCE))
{
nextGoal = new NavNode(new Vector3(distances[1].Item2.Object.Translation.X, 0, distances[1].Item2.Object.Translation.Z));
treasureGoal = distances[1].Item2;
AGProject1.CustomItems.NPSeekingTreasure = true;
}
else if (!distances[2].Item2.Activated() && (AGProject1.CustomItems.TreasureMode || distances[2].Item1 <= AGProject1.CustomItems.SEARCH_DISTANCE))
{
nextGoal = new NavNode(new Vector3(distances[2].Item2.Object.Translation.X, 0, distances[2].Item2.Object.Translation.Z));
treasureGoal = distances[2].Item2;
AGProject1.CustomItems.NPSeekingTreasure = true;
}
else if (!distances[3].Item2.Activated() && (AGProject1.CustomItems.TreasureMode || distances[3].Item1 <= AGProject1.CustomItems.SEARCH_DISTANCE))
{
nextGoal = new NavNode(new Vector3(distances[3].Item2.Object.Translation.X, 0, distances[3].Item2.Object.Translation.Z));
treasureGoal = distances[3].Item2;
AGProject1.CustomItems.NPSeekingTreasure = true;
}
}
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
float distance = Vector3.Distance(
new Vector3(nextGoal.Translation.X, 0, nextGoal.Translation.Z),
new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z));
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));
if (distance <= snapDistance)
{
// If the NPAgent finished searching for a treasure
if (AGProject1.CustomItems.NPSeekingTreasure)
{
// Set the treasure to active.
treasureGoal.Activate();
AGProject1.CustomItems.NPCNumTreasuresLeft = AGProject1.CustomItems.NPCNumTreasuresLeft + 1;
AGProject1.CustomItems.NumTreasuresLeft = AGProject1.CustomItems.NumTreasuresLeft - 1;
AGProject1.CustomItems.NPSeekingTreasure = false;
AGProject1.CustomItems.TreasureMode = false;
// Resume moving to the last active waypoint.
nextGoal = new NavNode(backupNextGoal.Translation);
}
// else, the agent finished moving to the next node in its path.
else
{
// snap to nextGoal and orient toward the new nextGoal
nextGoal = path.NextNode;
backupNextGoal = new NavNode(nextGoal.Translation);
// agentObject.turnToFace(nextGoal.Translation);
}
}
base.Update(gameTime); // Agent's Update();
}
/// <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)
{
if (stage.NumUnTagedTereasure == 0)
{
agentObject.Step = 0;
}
stage.setInfo(13, "NPAgent treasure score: " + treasureScore.ToString());
KeyboardState keyboardState = Keyboard.GetState();
if (keyboardState.IsKeyDown(Keys.N) && !oldKeyboardState.IsKeyDown(Keys.N))
{
if (treasure == false)
{
treasure = true;
foreach (Treasures te in stage.AllTreasures)
{
if (!te.Flaged)
{
savedGoal = nextGoal;
if (stage.NumUnTagedTereasure != 0)
{
nextGoal = new NavNode(new Vector3(te.Position.X, te.Position.Y, te.Position.Z));
}
break;
}
}
}
else
{
treasure = false;
nextGoal = savedGoal;
}
}
else if (keyboardState.IsKeyDown(Keys.Z) && !oldKeyboardState.IsKeyDown(Keys.Z))
{
if (stage.Sensor1.Visible)
{
stage.Sensor1.Visible = false;
}
else
{
stage.Sensor1.Visible = true;
}
if (stage.Sensor2.Visible)
{
stage.Sensor2.Visible = false;
}
else
{
stage.Sensor2.Visible = true;
}
}
oldKeyboardState = keyboardState;
foreach (Treasures te in stage.AllTreasures)
{
if (!te.Flaged && Vector3.Distance(new Vector3(te.Position.X, 0, te.Position.Z), new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z)) < 4000)
{
savedGoal = nextGoal;
if (stage.NumUnTagedTereasure != 0)
{
nextGoal = new NavNode(new Vector3(te.Position.X, te.Position.Y, te.Position.Z));
break;
}
}
}
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
float distance = Vector3.Distance(
new Vector3(nextGoal.Translation.X, 0, nextGoal.Translation.Z),
new Vector3(agentObject.Translation.X, 0, agentObject.Translation.Z));
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));
if (distance <= snapDistance)
{
// snap to nextGoal and orient toward the new nextGoal
nextGoal = path.NextNode;
savedGoal = nextGoal;
// agentObject.turnToFace(nextGoal.Translation);
}
base.Update(gameTime); // Agent's Update();
}
/// <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();
}
/// <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)
{
float distance; //distance from goal
float tagDistance = 200f; //minimum distance from treasure to tag it
// if currently seeking treasure
if (this.treasurePath)
{
if (treasureCount < this.treasureList.getTreasureNode.Length)
{
if (this.expectedTreasureCount == treasureCount)
{
++expectedTreasureCount;
distance = float.MaxValue; //set distance to max
for (int i = 0; i < this.treasureList.getTreasureNode.Length; i++)
{
if (this.treasureList.getTreasureNode[i].isTagged)
{
continue;
}
//iterate through list and get distance of treasure
float newDistance = Vector2.Distance(new Vector2(agentObject.Translation.X, agentObject.Translation.Z), new Vector2(this.treasureList.getTreasureNode[i].x * this.stage.Spacing, this.treasureList.getTreasureNode[i].z * this.stage.Spacing));
//if smaller than smallest so far make that the treasure to seek
if (newDistance < distance)
{
this.treasureListNum = i;
distance = newDistance;
}
}
}
//Orient agent towards the treasure then get distance to treasure
agentObject.turnToFace(new Vector3(this.treasureList.getTreasureNode[this.treasureListNum].x * this.stage.Terrain.Spacing, 0, this.treasureList.getTreasureNode[this.treasureListNum].z * this.stage.Terrain.Spacing));
distance = Vector2.Distance(new Vector2(agentObject.Translation.X, agentObject.Translation.Z), new Vector2(this.treasureList.getTreasureNode[this.treasureListNum].x * this.stage.Spacing, this.treasureList.getTreasureNode[this.treasureListNum].z * this.stage.Spacing));
// if we are within distance from the treasure, increment count, tag treasure, and return to waypoint navigation
if (distance < tagDistance)
{
this.treasureCount += 1;
this.treasureList.getTreasureNode[this.treasureListNum].isTagged = true;
this.treasurePath = false;
}
} //endif treasurecount < node length
else
{
this.treasurePath = false;
}
}
else
{
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));
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));
if (distance <= snapDistance)
{
// snap to nextGoal and orient toward the new nextGoal
nextGoal = path.NextNode;
// agentObject.turnToFace(nextGoal.Translation);
}
}
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);
}
