public void traverseMapDStep()
{
if (atTarget == false)
{
tScan();
steps++;
if (givenPath.Count == 0)
{
D_Star search = new D_Star(map.getMap(), positionX, positionY, targetX, targetY);
search.updateStart(positionX, positionY);
search.replan(map.getMap());
givenPath = search.getPath();
}
if (steps >= stepLimit)
{
atTarget = true;
}
if ((positionX == targetX) && (positionY == targetY))
{
atTarget = true;
}
if (givenPath.Count == 0)
{
atTarget = true;
}
if (atTarget == false)
{
PathNode nextNode = givenPath.Last();
givenPath.Remove(nextNode);
if ((nextNode.x == positionX) && (nextNode.y == positionY))
{
if (givenPath.Count > 0)
{
nextNode = givenPath.Last();
}
if ((nextNode.x == previousX) && (nextNode.y == previousY))
{
if (givenPath.Count > 0)
{
nextNode = givenPath.Last();
}
}
}
else
{
takenPath.Add(nextNode); // Add Start To taken Path
}
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);
}
}
}
sensorManager.updateFrontView(positionX, positionY, previousX, previousY);
generateRoverImage();
generatePathImage();
}
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();
}