public bool isAreaSafe(PathNode node)
{
if (realMap[node.x,node.y] <= 5.0)
{
return true;
}
return false;
}
public A_Star(int[,] gridT, int startXT, int startYT, int targetXT, int targetYT)
: base(gridT, startXT, startYT, targetXT, targetYT)
{
grid = gridT;
startX = startXT;
startY = startYT;
targetX = targetXT;
targetY = targetYT;
if (((startX == targetX) && (startY == targetY)) == false)
{
bool found = false; // used to determine if path is found
// creates the start node
Node current = new Node();
current.x = startX;
current.y = startY;
//creates the target node
targetNode = new Node();
targetNode.x = targetX;
targetNode.y = targetY;
open.Add(current);
do
{
// This call places the lowest f at the bottom
open.Sort(
delegate(Node x, Node y)
{
return x.f() - y.f();
});
// Get the node with the lowest TotalCost
current = open[0];
if (current.x == targetNode.x)
{
if (current.y == targetNode.y)
{
found = true;
break;
}
}
checkAdjacent(current.x, current.y, open, closed, current);
//remove a from the open list and move into the 'closed' list
open.Remove(current);
closed.Add(current);
} while (open.Count > 0); // Keeps going until the open list is empty
// if a path was found the path is worked out then returned
if (found == true)
{
foreach (Node n in closed)
{
PathNode pathNode = new PathNode();
pathNode.x = current.x;
pathNode.y = current.y;
pathNodes.Add(pathNode);
if( current.parent == null)
{
break;
}
current = current.parent;
}
}
}
}
public void traverseMapDstar(int startXt, int startYt, int endXt, int endYt)
{
steps = 0;
startX = startXt;
startY = startYt;
targetX = endXt;
targetY = endYt;
positionX = startX;
positionY = startY;
atTarget = false;
D_Star search = new D_Star(map.getMap(), positionX, positionY, targetX, targetY);
//Add In start Node
PathNode startNode = new PathNode();
startNode.x = startX;
startNode.y = startY;
takenPath.Add(startNode);
do
{
search.updateStart(positionX, positionY);
search.replan(map.getMap());
givenPath = search.getPath();
do
{
sensorManager.updateFrontView(positionX, positionY, previousX, previousY);
if (steps >= stepLimit)
{
atTarget = true;
break;
}
steps++;
if ((positionX == targetX) && (positionY == targetY))
{
atTarget = true;
break;
}
if (givenPath.Count == 0)
{
atTarget = true;
break;
}
PathNode nextNode = givenPath.Last();
givenPath.Remove(nextNode);
if (sensorManager.isAreaSafe(nextNode) == true)
{
map.setNode(nextNode.x, nextNode.y, 40);
previousX = positionX;
previousY = positionY;
positionX = nextNode.x;
positionY = nextNode.y;
takenPath.Add(nextNode);
sensorManager.updateOwnLocation(positionX, positionY);
}
else
{
givenPath.Clear();
map.setNode(nextNode.x, nextNode.y, 99999);
sensorManager.updateOwnLocation(nextNode.x, nextNode.y);
break;
}
} while (true);
} while (atTarget == false);
sensorManager.updateFrontView(positionX, positionY, previousX, previousY);
generatePathImage();
generateRoverImage();
}
public void computeShortestPath()
{
open.Clear();
closed.Clear();
if (((startX == targetX) && (startY == targetY)) == false)
{
bool found = false; // used to determine if path is found
//puts the start item in list
open.Add(current);
do
{
// This call places the lowest f at the bottom
open.Sort(
delegate(Node x, Node y)
{
return x.f() - y.f();
});
// Get the node with the lowest TotalCost
current = open[0];
if (current.x == target.x)
{
if (current.y == target.y)
{
found = true;
break;
}
}
checkAdjacent(current.x, current.y, open, closed, current);
//remove a from the open list and move into the 'closed' list
open.Remove(current);
closed.Add(current);
} while (open.Count > 0); // Keeps going until the open list is empty
// if a path was found the path is worked out then returned
if (found == true)
{
foreach (Node n in closed)
{
PathNode pathNode = new PathNode();
pathNode.x = current.x;
pathNode.y = current.y;
pathNodes.Add(pathNode);
if (current.parent == null)
{
break;
}
current = current.parent;
}
}
}
}