常見數(shù)學(xué)計(jì)算的輔助方法

1、添加做常見數(shù)學(xué)計(jì)算的輔助方法我們先要給機(jī)器人添加常見數(shù)學(xué)算法使用的一些輔助方法。我們還需要 normalizeAbsoluteAngleRadians() 方法和 normalizeRelativeAngleRadians() 方法。 清單 1. 數(shù)學(xué)輔助方法private static final double DOUBLE_PI = (Math.PI * 2);private static final double HALF_PI = (Math.PI / 2);public double calculateBearingToXYRadians(double sourceX, double

2、 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

3、(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 tr

4、immedAngle; 回頁首使 AdvancedRobot 擴(kuò)展到有倒行功能接著，為了以相反方向?qū)Ш?，我們需要用一些輔助方法把 AdvancedRobot 類的功能擴(kuò)展到允許倒行操作： · getRelativeHeading() 方法將應(yīng)付正確計(jì)算相對(duì)于機(jī)器人當(dāng)前的方向的相對(duì)方向產(chǎn)生的額外開銷。 · reverseDirection() 非常簡(jiǎn)單。它負(fù)責(zé) direction 實(shí)例變量的開關(guān)和使機(jī)器人掉頭。 請(qǐng)注意，由于減速需要時(shí)間，依據(jù)機(jī)器人的速度，在掉過頭來之前最多會(huì)沿原來的方向再走 4 格。 · setAhead() 和 setBack() 方法將覆蓋 A

5、dvancedRobot 類中的同名方法。這兩個(gè)方法會(huì)設(shè)置機(jī)器人對(duì)于目前方向的相對(duì)速度，必要的時(shí)候，還會(huì)調(diào)整 direction 實(shí)例變量。我們這么做的目的是要確保相對(duì)操作都與機(jī)器人當(dāng)前的移動(dòng)方向有關(guān)。 · setTurnLeftRadiansOptimal() 和 setTurnRightRadiansOptimal() 方法使機(jī)器人的方向轉(zhuǎn)過的角度超過 (Math.PI / 2) 。您會(huì)希望這個(gè)方法和 adjustHeadingForWalls 方法（我們將在后面討論）一起使用。 注：我沒有使用 getter 和 setter 方法，而是直接存取 direction 實(shí)例變量。盡

6、管通常這并非良好的編程習(xí)慣，但為了加快數(shù)據(jù)存取，在我的機(jī)器人代碼中我一直都是直接存取的。 清單 2. 機(jī)器人輔助方法public double getRelativeHeadingRadians() double relativeHeading = getHeadingRadians(); if (direction < 1) relativeHeading = normalizeAbsoluteAngleRadians(relativeHeading + Math.PI); return relativeHeading;public void reverseDirection() do

7、uble 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 relati

8、veDistance = (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 +

9、(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

10、 % HALF_PI); else if (turn > 0) turn = -(HALF_PI - (turn % HALF_PI); setTurnRightRadians(turn);回頁首添加因數(shù)避墻法我們需要添加的最后一個(gè)方法是 adjustHeadingForWalls() 方法。 這個(gè)方法的前面一半根據(jù)機(jī)器人和墻的靠近程度選擇 安全的 x 和 y 的位置（機(jī)器人當(dāng)前的 x 或 y 位置，或者如果機(jī)器人靠近墻，則就是中心點(diǎn)）。方法的后面一半則計(jì)算距離“安全點(diǎn)”的方位，并把這個(gè)方位和依機(jī)器人離墻遠(yuǎn)近得到的預(yù)想方向都作為因數(shù)考慮在內(nèi)。 可以使用 WALL_AVOID_INTERV

11、AL 和 WALL_AVOID_FACTORS 常量來調(diào)整機(jī)器人對(duì)墻的擔(dān)憂程度。 清單 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 headin

12、g) 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; doub

13、le 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_

14、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 distanceToWal

15、l = 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; 回頁首匯總其余的工作很容易。我們可以使用目前的導(dǎo)