我們只要對在 Tracking your opponents' movement中做的機器人加以補充 ,就能將因數避牆法添加到現有的或討厭的移動算法中。這種方法將預想的方向 和根據機器人和牆之間距離遠近確定的安全方向作為因數試圖找到最可能的方向 。
添加做常見數學計算的輔助方法
我們先要給機器人添加常見數學算法使用的一些輔助方法。
calculateBearingToXYRadians() 方法使用 java.lang.Math 中的 atan2() 方法來計算從 sourceX,sourceY 到 targetX,targetY 的絕對方位,然後再把這 個值轉化為相對於 sourceHeading 的相對方位。
我們還需要 normalizeAbsoluteAngleRadians() 方法和 normalizeRelativeAngleRadians() 方法。
清單 1. 數學輔助方法
private static final double DOUBLE_PI = (Math.PI * 2);
private static final double HALF_PI = (Math.PI / 2);
public double calculateBearingToXYRadians(double sourceX, double sourceY,
double sourceHeading, double targetX, double targetY) {
return normalizeRelativeAngleRadians(
Math.atan2((targetX - sourceX), (targetY - sourceY)) -
sourceHeading);
}
public double normalizeAbsoluteAngleRadians(double angle) {
if (angle < 0) {
return (DOUBLE_PI + (angle % DOUBLE_PI));
} else {
return (angle % DOUBLE_PI);
}
}
public static double normalizeRelativeAngleRadians(double angle) {
double trimmedAngle = (angle % DOUBLE_PI);
if (trimmedAngle > Math.PI) {
return -(Math.PI - (trimmedAngle % Math.PI));
} else if (trimmedAngle < -Math.PI) {
return (Math.PI + (trimmedAngle % Math.PI));
} else {
return trimmedAngle;
}
}
使 AdvancedRobot 擴展到有倒行功能
接著,為了以相反方向導航,我們需要用一些輔助方法把 AdvancedRobot 類 的功能擴展到允許倒行操作:
getRelativeHeading() 方法將應付正確計算相對於機器人當前的方向的相對 方向產生的額外開銷。
reverseDirection() 非常簡單。它負責 direction 實例變量的開關和使機 器人掉頭。請注意,由於減速需要時間,依據機器人的速度,在掉過頭來之前最 多會沿原來的方向再走 4 格。
setAhead() 和 setBack() 方法將覆蓋 AdvancedRobot 類中的同名方法。這 兩個方法會設置機器人對於目前方向的相對速度,必要的時候,還會調整 direction 實例變量。我們這麼做的目的是要確保相對操作都與機器人當前的移 動方向有關。
setTurnLeftRadiansOptimal() 和 setTurnRightRadiansOptimal() 方法使 機器人的方向轉過的角度超過 (Math.PI / 2) 。您會希望這個方法和 adjustHeadingForWalls 方法(我們將在後面討論)一起使用。
注:我沒有使用 getter 和 setter 方法,而是直接存取 direction 實例變 量。盡管通常這並非良好的編程習慣,但為了加快數據存取,在我的機器人代碼 中我一直都是直接存取的。
清單 2. 機器人輔助方法
public double getRelativeHeadingRadians() {
double relativeHeading = getHeadingRadians();
if (direction < 1) {
relativeHeading =
normalizeAbsoluteAngleRadians(relativeHeading + Math.PI);
}
return relativeHeading;
}
public void reverseDirection() {
double distance = (getDistanceRemaining() * direction);
direction *= -1;
setAhead(distance);
}
public void setAhead(double distance) {
double relativeDistance = (distance * direction);
super.setAhead(relativeDistance);
if (distance < 0) {
direction *= -1;
}
}
public void setBack(double distance) {
double relativeDistance = (distance * direction);
super.setBack(relativeDistance);
if (distance > 0) {
direction *= -1;
}
}
public void setTurnLeftRadiansOptimal(double angle) {
double turn = normalizeRelativeAngleRadians(angle);
if (Math.abs(turn) > HALF_PI) {
reverseDirection();
if (turn < 0) {
turn = (HALF_PI + (turn % HALF_PI));
} else if (turn > 0) {
turn = -(HALF_PI - (turn % HALF_PI));
}
}
setTurnLeftRadians(turn);
}
public void setTurnRightRadiansOptimal(double angle) {
double turn = normalizeRelativeAngleRadians(angle);
if (Math.abs(turn) > HALF_PI) {
reverseDirection();
if (turn < 0) {
turn = (HALF_PI + (turn % HALF_PI));
} else if (turn > 0) {
turn = -(HALF_PI - (turn % HALF_PI));
}
}
setTurnRightRadians(turn);
}
添加因數避牆法
我們需要添加的最後一個方法是 adjustHeadingForWalls() 方法。
這個方法的前面一半根據機器人和牆的靠近程度選擇 安全的 x 和 y 的位置 (機器人當前的 x 或 y 位置,或者如果機器人靠近牆,則就是中心點)。方法 的後面一半則計算距離“安全點”的方位,並把這個方位和依機器人離牆遠近得 到的預想方向都作為因數考慮在內。
可以使用 WALL_AVOID_INTERVAL 和 WALL_AVOID_FACTORS 常量來調整機器人 對牆的擔憂程度。
清單 3. 避牆法方法
private static final double WALL_AVOID_INTERVAL = 10;
private static final double WALL_AVOID_FACTORS = 20;
private static final double WALL_AVOID_DISTANCE =
(WALL_AVOID_INTERVAL * WALL_AVOID_FACTORS);
private double adjustHeadingForWalls(double heading) {
double fieldHeight = getBattleFieldHeight();
double fieldWidth = getBattleFieldWidth();
double centerX = (fieldWidth / 2);
double centerY = (fieldHeight / 2);
double currentHeading = getRelativeHeadingRadians();
double x = getX();
double y = getY();
boolean nearWall = false;
double desiredX;
double desiredY;
// If we are too close to a wall, calculate a course toward
// the center of the battlefield.
if ((y < WALL_AVOID_DISTANCE) ||
((fieldHeight - y) < WALL_AVOID_DISTANCE)) {
desiredY = centerY;
nearWall = true;
} else {
desiredY = y;
}
if ((x < WALL_AVOID_DISTANCE) ||
((fieldWidth - x) < WALL_AVOID_DISTANCE)) {
desiredX = centerX;
nearWall = true;
} else {
desiredX = x;
}
// Determine the safe heading and factor it in with the desired
// heading if the bot is near a wall
if (nearWall) {
double desiredBearing =
calculateBearingToXYRadians(x,
y,
currentHeading,
desiredX,
desiredY);
double distanceToWall = Math.min(
Math.min(x, (fieldWidth - x)),
Math.min(y, (fieldHeight - y)));
int wallFactor =
(int)Math.min((distanceToWall / WALL_AVOID_INTERVAL),
WALL_AVOID_FACTORS);
return ((((WALL_AVOID_FACTORS - wallFactor) * desiredBearing) +
(wallFactor * heading)) / WALL_AVOID_FACTORS);
} else {
return heading;
}
}
匯總
其余的工作很容易。我們可以使用目前的導航算法,將得出的結果送入 adjustHeadingForWalls() 方法來避開牆。
為了保持簡單,示例機器人要求方向改變為零,從而試著沿直線移動。
清單 4. 避牆法方法
public void run() {
while(true) {
setTurnRightRadiansOptimal(adjustHeadingForWalls(0));
setAhead(100);
execute();
}
}
關於它就是這樣了。簡單,但有效。