public SearchNode bugSearch(MapLocation start, MapLocation target) throws GameActionException {
MapLocation s = start, t = target;
int closest = s.distanceSquaredTo(t);
int closestL = closest, closestR = closest;
SearchNode current = new SearchNode(start, 1, null, true),
currentL = new SearchNode(start, 1, null, true),
currentR = new SearchNode(start, 1, null, true);
boolean isTracingL = false, isTracingR = false;
Direction curDir = current.loc.directionTo(t);
Direction curDirL = curDir, curDirR = curDir;
if (debug) System.out.println("Source: " + s + "; Target: " + t);
while (!(current.loc.x == t.x && current.loc.y == t.y)
&& !(currentL.loc.x == t.x && currentL.loc.y == t.y)
&& !(currentR.loc.x == t.x && currentR.loc.y == t.y)) {
if (debug)
System.out.println(
"Current: " + current.loc + ";Right " + currentR.loc + ";Left " + currentL.loc);
if (!isTracingL || !isTracingR) {
// we're done tracing
if (isTracingL) {
// the right bug finished first
isTracingL = false;
current = currentR;
closest = closestR;
} else if (isTracingR) {
// the left bug finished first
isTracingR = false;
current = currentL;
closest = closestL;
}
curDir = directionTo(current.loc, t);
if (curDir != null) {
current = current.update(curDir);
} else {
current.isPivot = true;
closest = current.loc.distanceSquaredTo(t);
closestL = closest;
closestR = closest;
isTracingL = true;
isTracingR = true;
curDir = current.loc.directionTo(t);
curDirL = curDir;
curDirR = curDir;
while (!isGood(current.loc.add(curDirL))) {
curDirL = curDirL.rotateRight();
}
while (!isGood(current.loc.add(curDirR))) {
curDirR = curDirR.rotateLeft();
}
currentL = current.update(curDirL);
currentR = current.update(curDirR);
if (currentL.loc.distanceSquaredTo(t) < closestL)
closestL = currentL.loc.distanceSquaredTo(t);
if (currentR.loc.distanceSquaredTo(t) < closestR)
closestR = currentR.loc.distanceSquaredTo(t);
}
} else { // we're tracing
if (currentL.loc.distanceSquaredTo(t) < currentR.loc.distanceSquaredTo(t)) {
if (debug) System.out.println("LEFT TRACE");
// the left trace is closer
curDirL = directionTo(currentL.loc, t);
if (curDirL != null && currentL.loc.add(curDirL).distanceSquaredTo(t) < closestL) {
if (debug) System.out.print("FINISH LEFT TRACE. GOING " + curDirL);
isTracingL = false;
currentL.isPivot = true;
if (debug) System.out.println("LEFT PIVOT");
currentL = currentL.update(curDirL);
if (currentL.loc.distanceSquaredTo(t) < closestL)
closestL = currentL.loc.distanceSquaredTo(t);
} else {
curDirL = currentL.loc.directionTo(currentL.prevLoc.loc).rotateRight().rotateRight();
int i = 2;
while (!isGood(currentL.loc.add(curDirL))) {
curDirL = curDirL.rotateRight();
i++;
}
if (i < 4
|| curDirL != Direction.EAST
&& curDirL != Direction.WEST
&& curDirL != Direction.NORTH
&& curDirL != Direction.SOUTH) {
currentL.isPivot = true;
if (debug) System.out.println("LEFT PIVOT");
}
if (curDirL != directionTo(currentL.prevLoc.loc, currentL.loc)) {
currentL.isPivot = true;
if (debug) System.out.println("LEFT PIVOT");
}
currentL = currentL.update(curDirL);
if (currentL.loc.distanceSquaredTo(t) < closestL)
closestL = currentL.loc.distanceSquaredTo(t);
}
} else {
// the right trace is closer
if (debug) System.out.println("RIGHT TRACE");
curDirR = directionTo(currentR.loc, t);
if (curDirR != null && currentR.loc.add(curDirR).distanceSquaredTo(t) < closestR) {
if (debug) System.out.println("FINISH RIGHT TRACE. GOING " + curDirR);
isTracingR = false;
currentR.isPivot = true;
if (debug) System.out.println("RIGHT PIVOT");
currentR = currentR.update(curDirR);
if (currentR.loc.distanceSquaredTo(t) < closestR)
closestR = currentR.loc.distanceSquaredTo(t);
} else {
curDirR = currentR.loc.directionTo(currentR.prevLoc.loc).rotateLeft().rotateLeft();
int i = 2;
while (!isGood(currentR.loc.add(curDirR))) {
curDirR = curDirR.rotateLeft();
i++;
}
if (i < 4
|| curDirR != Direction.EAST
&& curDirR != Direction.WEST
&& curDirR != Direction.NORTH
&& curDirR != Direction.SOUTH) {
currentR.isPivot = true;
if (debug) System.out.println("RIGHT PIVOT");
}
if (curDirR != directionTo(currentR.prevLoc.loc, currentR.loc)) {
currentR.isPivot = true;
if (debug) System.out.println("RIGHT PIVOT");
}
currentR = currentR.update(curDirR);
if (currentR.loc.distanceSquaredTo(t) < closestR)
closestR = currentR.loc.distanceSquaredTo(t);
}
}
}
}
current.isPivot = true;
currentL.isPivot = true;
currentR.isPivot = true;
if (current.loc.equals(t)) {
return current;
}
if (currentL.loc.equals(t)) {
return currentL;
}
if (currentR.loc.equals(t)) {
return currentR;
}
throw new GameActionException(null, "Unable to find a path from " + s + " to " + t);
}
