private boolean isOpen(
MapLocation currentLoc,
Direction currentDir,
Direction nextDir,
Direction desDir,
boolean cw) {
if (desDir == Direction.OMNI) {
return true;
}
if (!isTraversable(currentLoc.add(currentDir))) return false;
if (cw) {
while (currentDir != nextDir) {
if (currentDir == desDir) return true;
currentDir = currentDir.rotateRight();
}
} else {
while (currentDir != nextDir) {
if (currentDir == desDir) return true;
currentDir = currentDir.rotateLeft();
}
}
return (nextDir == desDir);
}
private MapLocation traceNext(MapLocation currentLoc, Direction faceDir, boolean cw) {
// Put isTraversable first so that the second while loop will rotate the
// direction to walkable position
// Try code reuse in the future
int step = 0;
if (cw) {
while (isTraversable(currentLoc.add(faceDir)) && step < 8) {
faceDir = faceDir.rotateRight();
step++;
}
step = 0;
while (!isTraversable(currentLoc.add(faceDir)) && step < 8) {
faceDir = faceDir.rotateLeft();
step++;
}
} else {
while (isTraversable(currentLoc.add(faceDir)) && step < 8) {
faceDir = faceDir.rotateLeft();
step++;
}
step = 0;
while (!isTraversable(currentLoc.add(faceDir)) && step < 8) {
faceDir = faceDir.rotateRight();
step++;
}
}
return currentLoc.add(faceDir);
}
public Direction Bug(MapLocation s, MapLocation t, int tolerance) {
// arrive the destination
if (s.distanceSquaredTo(t) <= tolerance) {
reset();
return Direction.OMNI;
}
Direction nextDir;
Direction faceDir = controllers.myRC.getDirection();
// if target is not traversable, back-tracing the destination
while (!isTraversable(t)) {
nextDir = s.directionTo(t);
t = t.subtract(nextDir);
// beside the source
if (s.distanceSquaredTo(t) <= tolerance) {
reset();
return Direction.OMNI;
}
}
modifiedDes = t;
Direction desDir = s.directionTo(t);
MapLocation nextLoc;
if (isTracing) {
Direction startTracingDir =
isCW ? faceDir.rotateRight().rotateRight() : faceDir.rotateLeft().rotateLeft();
nextLoc = traceNext(s, startTracingDir, isCW);
nextDir = s.directionTo(nextLoc);
// The way is open
if (isOpen(s, faceDir, nextDir, desDir, isCW) && isTraversable(s.add(desDir))) {
isTracing = false;
return desDir;
}
return nextDir;
} else {
if (isTraversable(s.add(desDir))) {
return desDir;
} else {
isTracing = true;
isCW = betterTracingWay(s, t);
nextLoc = traceNext(s, desDir, isCW);
return s.directionTo(nextLoc);
}
}
}
private int computeCost(int cost, Direction faceDir, Direction nextDir) {
if (nextDir.isDiagonal()) {
return cost + moveDiagonally + ((faceDir == nextDir) ? 0 : 1);
} else {
return cost + moveOrthogonally + ((faceDir == nextDir) ? 0 : 1);
}
}
private Direction detour(Direction faceDir, boolean cw) {
if (cw) {
while (controllers.motor.canMove(faceDir)) {
faceDir = faceDir.rotateRight();
}
while (!controllers.motor.canMove(faceDir)) {
faceDir = faceDir.rotateLeft();
}
} else {
while (controllers.motor.canMove(faceDir)) {
faceDir = faceDir.rotateLeft();
}
while (!controllers.motor.canMove(faceDir)) {
faceDir = faceDir.rotateRight();
}
}
return faceDir;
}
public Direction getNextDir(int tolerance) {
if (controllers.myRC.getLocation().equals(previousRobLoc)) {
if (controllers.motor.canMove(previousDir)) {
// controllers.myRC.setIndicatorString(1, "precomputed");
return previousDir;
}
// else if (!controllers.motor.isActive() &&
// controllers.motor.canMove(previousDir.rotateRight())) {
// isTracing = false;
// previousDir = previousDir.rotateRight();
// controllers.myRC.setIndicatorString(1, "yield");
// return previousDir;
// }
else {
// controllers.myRC.setIndicatorString(1, "detour " + isCW);
if (!isTracing) {
previousDir = detour(controllers.myRC.getDirection(), isCW);
} else {
Direction faceDir = controllers.myRC.getDirection();
Direction startTracingDir =
isCW ? faceDir.rotateRight().rotateRight() : faceDir.rotateLeft().rotateLeft();
previousDir = detour(startTracingDir, isCW);
}
}
return previousDir;
} else {
// controllers.myRC.setIndicatorString(1, "bugging");
previousRobLoc = controllers.myRC.getLocation();
if (destination == null) {
reset();
previousDir = Direction.OMNI;
return Direction.OMNI;
} else {
previousDir = Bug(controllers.myRC.getLocation(), modifiedDes, tolerance);
return previousDir;
}
}
}
public MapLocation getNextJumpingLoc(int tolerance) {
MapLocation jumpLoc = controllers.myRC.getLocation();
MapLocation currentLoc = controllers.myRC.getLocation();
Direction desDir = currentLoc.directionTo(destination);
MapLocation nextLoc = controllers.myRC.getLocation();
Direction nextDir = controllers.myRC.getDirection();
Direction startTracingDir;
if (jumpingTracing) {
// controllers.myRC.setIndicatorString(2, "tracing");
startTracingDir =
isCW ? nextDir.rotateRight().rotateRight() : nextDir.rotateLeft().rotateLeft();
nextLoc = currentLoc.add(startTracingDir, 2);
do {
if (isTraversable(nextLoc)) jumpLoc = nextLoc;
nextLoc = nextLoc.add(startTracingDir);
} while (currentLoc.distanceSquaredTo(nextLoc) <= 16);
if (currentLoc.distanceSquaredTo(currentLoc) > 2) {
jumpingTracing = false;
// controllers.myRC.setIndicatorString(2, "Find good jumpLoc: " + jumpLoc);
return jumpLoc;
}
nextLoc = currentLoc;
nextDir = controllers.myRC.getDirection();
// if( controllers.myRC.senseTerrainTile(currentLoc.add(startTracingDir)) ==
// TerrainTile.OFF_MAP ){
// isCW = !isCW;
// nextDir = nextDir.opposite();
// }
// controllers.myRC.setIndicatorString(2, "Do the bugging");
Direction faceDir;
do {
jumpLoc = nextLoc;
faceDir = nextDir;
startTracingDir =
isCW ? nextDir.rotateRight().rotateRight() : nextDir.rotateLeft().rotateLeft();
nextLoc = traceNext(jumpLoc, startTracingDir, isCW);
nextDir = jumpLoc.directionTo(nextLoc);
desDir = jumpLoc.directionTo(destination);
if (isOpen(jumpLoc, faceDir, nextDir, desDir, isCW) && isTraversable(jumpLoc.add(desDir))) {
// controllers.myRC.setIndicatorString(2, "Open");
jumpingTracing = false;
nextLoc = jumpLoc;
if (jumpLoc.distanceSquaredTo(currentLoc) <= 2) return null;
return jumpLoc;
}
} while (currentLoc.distanceSquaredTo(nextLoc) <= 16);
if (jumpLoc.distanceSquaredTo(currentLoc) <= 2) return null;
return jumpLoc;
} else {
// controllers.myRC.setIndicatorString(2, "go");
jumpLoc = getNextJumpingLocTowardDes(tolerance);
if (jumpLoc == null) {
// controllers.myRC.setIndicatorString(2, "No way to go");
if (isTraversable(currentLoc.add(desDir)))
isCW = betterTracingWay(currentLoc.add(desDir), destination);
else isCW = betterTracingWay(currentLoc, destination);
jumpingTracing = true;
}
}
return jumpLoc;
}
