|
|
| (7 intermediate revisions by the same user not shown) |
| Line 1: |
Line 1: |
| − | | + | #REDIRECT [[Wall Smoothing/Precise]] |
| − | <syntaxhighlight>
| |
| − | public double smoothHeading(double absoluteHeading, @Output Trig headingTrig, double x, double y, int latDir) {
| |
| − | double stickBearingSin = +headingTrig.cos * latDir;
| |
| − | double stickBearingCos = -headingTrig.sin * latDir;
| |
| − | | |
| − | double stickX = x + INNER_R * stickBearingSin + 4 * headingTrig.sin;
| |
| − | double stickY = y + INNER_R * stickBearingCos + 4 * headingTrig.cos;
| |
| − | | |
| − | double stickDistW = stickX - MIN_X;
| |
| − | double stickDistE = MAX_X - stickX;
| |
| − | double stickDistN = MAX_Y - stickY;
| |
| − | double stickDistS = stickY - MIN_Y;
| |
| − | | |
| − | double distW = x - MIN_X;
| |
| − | double distE = MAX_X - x;
| |
| − | double distN = MAX_Y - y;
| |
| − | double distS = y - MIN_Y;
| |
| − | | |
| − | double stickDistH;
| |
| − | WALL wallH = null;
| |
| − | if (stickDistW < stickDistE) {
| |
| − | stickDistH = stickDistW;
| |
| − | if (stickDistH < OUTER_R - 0.1) {
| |
| − | wallH = WALL.W;
| |
| − | }
| |
| − | } else {
| |
| − | stickDistH = stickDistE;
| |
| − | if (stickDistH < OUTER_R - 0.1) {
| |
| − | wallH = WALL.E;
| |
| − | }
| |
| − | }
| |
| − | | |
| − | double stickDistV;
| |
| − | WALL wallV = null;
| |
| − | if (stickDistS < stickDistN) {
| |
| − | stickDistV = stickDistS;
| |
| − | if (stickDistV < OUTER_R - 0.1) {
| |
| − | wallV = WALL.S;
| |
| − | }
| |
| − | } else {
| |
| − | stickDistV = stickDistN;
| |
| − | if (stickDistV < OUTER_R - 0.1) {
| |
| − | wallV = WALL.N;
| |
| − | }
| |
| − | }
| |
| − | | |
| − | WALL forwardHWall;
| |
| − | if (headingTrig.sin < 0) {
| |
| − | forwardHWall = WALL.W;
| |
| − | } else {
| |
| − | forwardHWall = WALL.E;
| |
| − | }
| |
| − | | |
| − | WALL forwardVWall;
| |
| − | if (headingTrig.cos < 0) {
| |
| − | forwardVWall = WALL.S;
| |
| − | } else {
| |
| − | forwardVWall = WALL.N;
| |
| − | }
| |
| − | | |
| − | WALL firstWall = null;
| |
| − | WALL secondWall = null;
| |
| − | double firstDist = Double.NaN;
| |
| − | double secondDist = Double.NaN;
| |
| − | | |
| − | if (forwardHWall == wallH && forwardVWall == wallV) {
| |
| − | if (latDir > 0) {
| |
| − | if (forwardHWall == WALL.W) {
| |
| − | if (forwardVWall == WALL.N) {
| |
| − | firstWall = WALL.W;
| |
| − | firstDist = distW;
| |
| − | secondWall = WALL.N;
| |
| − | secondDist = distN;
| |
| − | } else {
| |
| − | firstWall = WALL.S;
| |
| − | firstDist = distS;
| |
| − | secondWall = WALL.W;
| |
| − | secondDist = distW;
| |
| − | }
| |
| − | } else {
| |
| − | if (forwardVWall == WALL.N) {
| |
| − | firstWall = WALL.N;
| |
| − | firstDist = distN;
| |
| − | secondWall = WALL.E;
| |
| − | secondDist = distE;
| |
| − | } else {
| |
| − | firstWall = WALL.E;
| |
| − | firstDist = distE;
| |
| − | secondWall = WALL.S;
| |
| − | secondDist = distS;
| |
| − | }
| |
| − | }
| |
| − | } else {
| |
| − | if (forwardHWall == WALL.W) {
| |
| − | if (forwardVWall == WALL.N) {
| |
| − | firstWall = WALL.N;
| |
| − | firstDist = distN;
| |
| − | secondWall = WALL.W;
| |
| − | secondDist = distW;
| |
| − | } else {
| |
| − | firstWall = WALL.W;
| |
| − | firstDist = distW;
| |
| − | secondWall = WALL.S;
| |
| − | secondDist = distS;
| |
| − | }
| |
| − | } else {
| |
| − | if (forwardVWall == WALL.N) {
| |
| − | firstWall = WALL.E;
| |
| − | firstDist = distE;
| |
| − | secondWall = WALL.N;
| |
| − | secondDist = distN;
| |
| − | } else {
| |
| − | firstWall = WALL.S;
| |
| − | firstDist = distS;
| |
| − | secondWall = WALL.E;
| |
| − | secondDist = distE;
| |
| − | }
| |
| − | }
| |
| − | }
| |
| − | } else if (forwardHWall == wallH) {
| |
| − | firstWall = wallH;
| |
| − | firstDist = wallH == WALL.W ? distW : distE;
| |
| − | } else if (forwardVWall == wallV) {
| |
| − | firstWall = wallV;
| |
| − | firstDist = wallV == WALL.S ? distS : distN;
| |
| − | }
| |
| − | | |
| − | WALL smoothWall = firstWall;
| |
| − | double smoothDist = firstDist;
| |
| − | | |
| − | while (smoothWall != null) {
| |
| − | double wallSin = (OUTER_R - smoothDist) / OUTER_R;
| |
| − | if (wallSin > 1) {
| |
| − | wallSin = 1;
| |
| − | }
| |
| − | | |
| − | wallSin *= latDir;
| |
| − | | |
| − | double wallCos = Math.sqrt(1 - wallSin * wallSin);
| |
| − | | |
| − | double newWallSin = wallSin * DELTA_COS + wallCos * DELTA_SIN * latDir;
| |
| − | double newWallCos = wallCos * DELTA_COS - wallSin * DELTA_SIN * latDir;
| |
| − | | |
| − | if (newWallCos > 0) {
| |
| − | wallSin = newWallSin;
| |
| − | wallCos = newWallCos;
| |
| − | }
| |
| − | | |
| − | double newSin;
| |
| − | double newCos;
| |
| − | if (smoothWall == WALL.N) {
| |
| − | newSin = +wallSin;
| |
| − | newCos = +wallCos * +1;
| |
| − | } else if (smoothWall == WALL.E) {
| |
| − | newSin = +wallCos * +1;
| |
| − | newCos = -wallSin;
| |
| − | } else if (smoothWall == WALL.S) {
| |
| − | newSin = -wallSin;
| |
| − | newCos = -wallCos * +1;
| |
| − | } else {
| |
| − | newSin = -wallCos * +1;
| |
| − | newCos = +wallSin;
| |
| − | }
| |
| − | | |
| − | double newHeading = Math.atan2(newSin, newCos);
| |
| − | | |
| − | if (Math.signum(Utils.normalRelativeAngle(newHeading - absoluteHeading)) == latDir) {
| |
| − | headingTrig.sin = newSin;
| |
| − | headingTrig.cos = newCos;
| |
| − | | |
| − | absoluteHeading = newHeading;
| |
| − | | |
| − | if (debugSticks != null || debugHeadings != null) {
| |
| − | stickBearingSin = +newCos * latDir;
| |
| − | stickBearingCos = -newSin * latDir;
| |
| − | | |
| − | double imagineStickX = x + INNER_R * stickBearingSin + 4 * newSin;
| |
| − | double imagineStickY = y + INNER_R * stickBearingCos + 4 * newCos;
| |
| − | | |
| − | if (debugSticks != null) {
| |
| − | debugSticks.add(new Point(imagineStickX, imagineStickY));
| |
| − | }
| |
| − | if (debugHeadings != null) {
| |
| − | debugHeadings.add(newHeading);
| |
| − | }
| |
| − | }
| |
| − | }
| |
| − | | |
| − | if (smoothWall == firstWall) {
| |
| − | smoothWall = secondWall;
| |
| − | smoothDist = secondDist;
| |
| − | } else {
| |
| − | break;
| |
| − | }
| |
| − | }
| |
| − | return absoluteHeading;
| |
| − | }
| |
| − | </syntaxhighlight>
| |
| − | | |
| − | | |
| − | [[Category:Code Snippets]] | |
| − | [[Category:Movement]]
| |
