private float orientToWayPoint(OrderedPair direction) {
float yaw_retn = 0; ;
Vector3 point = new Vector3(direction.X*scene.Spacing,0,direction.Z*scene.Spacing);
Vector3 forward = Orientation.Forward;
forward.Y = 0;
Vector3 position = Orientation.Translation;
position.Y = 0;
Vector3 right = Orientation.Right;
right.Y = 0;
Vector3 left = Orientation.Left;
left.Y = 0;
Vector3 u = Vector3.Normalize(forward);
Vector3 v = Vector3.Normalize(point - position);
Vector3 r = Vector3.Normalize(right);
Vector3 l = Vector3.Normalize(left);
float angle = (float)Math.Acos(Vector3.Dot(u,v));
float angleR = (float)Math.Acos(Vector3.Dot(r,v));
float angleL = (float)Math.Acos(Vector3.Dot(l,v));
if(angleL < angleR) {
yaw_retn = angle;
}else if(angleR < angleL) {
yaw_retn = -angle;
}else if(angleR == angleL) {
yaw_retn = angle;
}
if(double.IsNaN(yaw)) {
yaw_retn = (float)Math.PI;
}
return yaw_retn;
}
public void setUpPathReturn() {
returnedToOrigin = false;
queue = inverse_tressure_path.Pop();
previous = queue.Dequeue();
foreach(OrderedPair O in queue) {
scene.WayPoints[(int)O.Z, (int)O.X].changeCubeColor(Color.Cornsilk);
}
}
public OrderedPair generateTresurePositionAsOrderedPair() {
OrderedPair top = null;
Model3D T = targetTreasures.Peek();
if(treasureCount() != 0) {
top = new OrderedPair( T.X, scene.Terrain.surfaceHeight((int)T.X,(int)T.Z), T.Z );
}
return top;
}
public Node(OrderedPair op)
{
data = op;
edges = new LinkedList <Edge>();
f = 0f;
g = 0f;
h = 0f;
}
public void testData(Node myloc_node, Node tresloc_node, OrderedPair myloc, OrderedPair tresloc)
{
if (myloc_node == null)
{
System.Console.WriteLine("Could not find myloc node @ (" + myloc.X + ", " + myloc.Z + ")");
}
if (tresloc_node == null)
{
System.Console.WriteLine("Could not find tresloc node @ (" + tresloc.X + ", " + tresloc.Z + ")");
}
}
public void explorerPathBuilder() {
OrderedPair myLoc = null;
destinationExploring = wayPointsToExplore[currentWayPoint]; //GET NEXT WAYPOINT
myLoc = generateMyPositionAsOrderedPair();
if(myLoc != null || destinationExploring != null) {
explore_path = scene.Map.findShortestPath(myLoc, destinationExploring);
queue = explore_path;
previous = queue.Dequeue();
foreach(OrderedPair O in queue) {
scene.WayPoints[(int)O.Z, (int)O.X].changeCubeColor(Color.Green);
}
} else {
System.Console.WriteLine("Could Not Build Path Good Bye");
Environment.Exit(0);
}
}
public void aStarPathBuilder() {
treasuresChanged = false;
currentTreasure = targetTreasures.Peek();
OrderedPair tresLoc = generateTresurePositionAsOrderedPair(); //PEEKS AT TOP OF QUEUE
OrderedPair myLoc = generateMyPositionAsOrderedPair();
if (tresLoc != null && myLoc != null) {
tressure_path = scene.Map.findShortestPath(myLoc, tresLoc);
}
if (tressure_path.Count != 0) {
queue = tressure_path;
loadReturnPath();
previous = queue.Dequeue();
previousMovementMethod = currentMovementMethod;
foreach(OrderedPair O in queue) {
scene.WayPoints[(int)O.Z, (int)O.X].changeCubeColor(Color.Chocolate);
}
} else {
System.Console.WriteLine("INDEX_ERROR: Could not build Path");
}
}
private void standardPathToFollow() {
if (!queueIsEmpty()) {
if( bearing != null &&
xlocation > bearing.X - percentERROR && xlocation < bearing.X + percentERROR &&
zlocation > bearing.Z - percentERROR && zlocation < bearing.Z + percentERROR) {
bearing = null;
previous = queue.Dequeue();
scene.WayPoints[(int)previous.Z, (int)previous.X].changeCubeColor(setColor());
translateToWayPoint();
}
if(bearing == null) {
if(!queueIsEmpty()) {
if(!scene.PAUSE) {
bearing = queue.Peek();
}
}
}
}
if(bearing != null) {
orientToPath();
setStep();
}
}
public void moveToTreassure() {
float Xo = targetTreasures.Peek().Orientation.Translation.X;
float Zo = targetTreasures.Peek().Orientation.Translation.Z;
float Xi = Orientation.Translation.X;
float Zi = Orientation.Translation.Z;
float dist = distanceToTressure(Xo,Xi,Zo,Zi);
if( dist < DIST_ERROR ) {
translateToTressure();
countT--;
bearing = null; //YOU WILL GET NEW BEARING WHEN QUE IS FILLED
targetTreasures.Dequeue(); //GET RID OF CAPTURED TREASURE
if(treasureCount() != 0) { //IF MORE TREASURE FOUND THAN GET NEXT ONE
currentTreasure = targetTreasures.Peek();
} else {
currentTreasure = null;
}
treasuresChanged = true;
} else {
if(!queueIsEmpty()) {
standardPathToFollow();
}
}
}
private string displayOrderedPair(OrderedPair o) {
return "(" + o.X + ", " + o.Z + ")";
}
public NPAvatar(Scene scene, string label, Vector3 pos, Vector3 orientAxis,float radians, string meshFile)
: base(scene, label, pos, orientAxis, radians, meshFile) {
first.Name = "npFirst";
follow.Name = "npFollow";
above.Name = "npAbove";
stepSize = 40;
random = new Random();
move = true;
cellsVisited = new Boolean[512,512];
for (int row = 0; row < scene.Range; row++) {
for(int col = 0; col < scene.Range; col++) {
cellsVisited[row, col] = false;
}
}
Vector3 oldTranslation = Orientation.Translation;
Orientation *= Matrix.CreateTranslation(-1 * Orientation.Translation);
Orientation *= Matrix.CreateRotationY(90 * MathHelper.Pi / 180);
Orientation *= Matrix.CreateTranslation(oldTranslation);
targetTreasures = null;
bearing = null;
targetTreasures = new Queue<Model3D>();
inverse_tressure_path = new Stack<Queue<OrderedPair>>();
scene.PAUSE = false;
currentTreasure = null;
previousTreasure = null;
destinationExploring = null;
currentWayPoint = 1;
direction = true;
wayPointsToExplore = new OrderedPair [] {
new OrderedPair(504,0,504),
new OrderedPair(352,0,496),
new OrderedPair(16,0,496),
new OrderedPair(16,0,16),
new OrderedPair(32,0,16),
new OrderedPair(32,0,488),
new OrderedPair(48,0,488),
new OrderedPair(48,0,16),
new OrderedPair(64,0,16),
new OrderedPair(64,0,488),
new OrderedPair(80,0,488),
new OrderedPair(80,0,16),
new OrderedPair(96,0,16),
new OrderedPair(96,0,488),
new OrderedPair(112,0,488),
new OrderedPair(112,0,16),
new OrderedPair(128,0,16),
new OrderedPair(128,0,488),
new OrderedPair(144,0,488),
new OrderedPair(144,0,16),
new OrderedPair(160,0,16),
new OrderedPair(160,0,488),
new OrderedPair(176,0,488),
new OrderedPair(176,0,16),
new OrderedPair(192,0,16),
new OrderedPair(192,0,488),
new OrderedPair(208,0,488),
new OrderedPair(208,0,16),
new OrderedPair(224,0,16),
new OrderedPair(225,0,488),
};
/* STATE VARIABLES */
previousMovementMethod = MovementMethod.NONE;
currentMovementMethod = MovementMethod.NONE;
path_builder = PathBuildMethod.NONE;
wayPointReached = false;
treasuresChanged = false;
returnedToOrigin = true;
}
public Queue <OrderedPair> findShortestPath(OrderedPair myloc, OrderedPair tresloc)
{
myloc.X = (float)Math.Floor(myloc.X);
if (myloc.X % 2 != 0)
{
myloc.X += 1;
}
myloc.Z = (float)Math.Floor(myloc.Z);
if (myloc.Z % 2 != 0)
{
myloc.Z += 1;
}
Node myloc_node = findOrderedPair(myloc.X, myloc.Z);
Node tresloc_node = findOrderedPair(tresloc.X, tresloc.Z);
testData(myloc_node, tresloc_node, myloc, tresloc);
Queue <OrderedPair> retrn = null;
List <Node> closed_set = null;
List <Node> open_set = null;
if (myloc_node != null && tresloc_node != null)
{
bool done = false;
closed_set = new List <Node>();
open_set = new List <Node>();
myloc_node.g = 0.0f;
myloc_node.h = findDistanceBetweenNodes(myloc_node, tresloc_node);
myloc_node.f = myloc_node.g + myloc_node.h;
open_set.Add(myloc_node);
retrn = new Queue <OrderedPair>();
while (open_set.Count != 0 && !done)
{
Node min_node = findMinimumNodeInList(open_set);
if (min_node == tresloc_node)
{
closed_set.Add(min_node);
done = true;
}
if (!done)
{
open_set.Remove(min_node);
closed_set.Add(min_node);
float tentative_g_score = 0;
bool tentative_is_better = false;
foreach (Edge E in min_node.edges)
{
if (closed_set.Contains(E.adjacent))
{
continue;
}
tentative_g_score = min_node.g + findDistanceBetweenNodes(min_node, E.adjacent);
if (!open_set.Contains(E.adjacent))
{
open_set.Add(E.adjacent);
tentative_is_better = true;
}
else if (tentative_g_score < E.adjacent.g)
{
tentative_is_better = true;
}
else
{
tentative_is_better = false;
}
if (tentative_is_better)
{
E.adjacent.g = tentative_g_score;
E.adjacent.h = findDistanceBetweenNodes(E.adjacent, tresloc_node);
E.adjacent.f = E.adjacent.g + E.adjacent.h;
}
}
}
}
}
if (closed_set != null)
{
loadQueue(retrn, new List <Node>(), closed_set, tresloc_node, myloc_node);
}
resetGraph();
return(retrn);
}