Wall Smoothing/Implementations/Fancy Stick

From Robowiki
Jump to navigation Jump to search
Wall Smoothing Sub-pages:
Wall SmoothingImplementations

There are a few other wall smoothing algorithms out there, but this one is a little different so I thought I'd share it. It does not use a standard "walking stick" (a theoretical "stick" that, when it hits a wall, tells your bot to turn more inward toward your orbiting point). That type of algorithm keeps your bot unnecessarily far from the walls around the corners (which is not necessarily a bad thing, but it's true). This algorithm uses a more sophisticated stick, that will let your bot stay much closer to the walls (almost as close as possible when running full speed).

To understand this new type of walking stick, consider the picture below. You are orbiting SittingDuck there, where the blue circle is the orbit. Obviously, following the orbit would crash you into the wall, so you need to do some wall smoothing. The green line is the closest you can get to the wall without crashing (<tt>getBattlefieldWidth() - 18.0</tt>), and the white circle represents your smallest turning radius at top speed (r = 114.5450131316624, ask me if you want to know how to come up with that number). The two red lines are your walking stick. The walking stick always extends to the center of the tightest circle you can turn, then straight to the wall.

Wall smoothing.gif

Let's call the new kind of walking stick your "feeler". To find the position of you feeler relative to the wall (to see if you need to turn sharply), consider the x position of the green line as 0, and your x position as simply x (where x <= 0). Then when you are traveling at an angle a (where 0.0 < a && a < 180.0), the x position of your feeler's hinge is

    x + r * sin(a + 90) =
    x + r * cos(a)

and the x position of the end of your feeler is

    x + r * cos(a) + r =
    x + r * (cos(a) + 1.0)

With this information we can write a method to determine if we need to do some smoothing. We project where you will be next tick if you follow the normal path, calculate where the feeler will be, and see if it went past the green line. If it did and you continue on the normal path for one more tick, you will not be able to turn sharp enough to miss the wall! Note that a must be in radians, and s is the speed at which you will be traveling to get to the next tick (just using 8.0 will turn you just a little too sharp, but works fine).

    import static java.lang.Math.*;
    double r = 114.5450131316624;
    double d = 2 * r; // turning diameter
    private boolean shouldSmooth(double a, double x, double s) {
        double nextX = x + s * sin(a);
        if (nextX < -d) { // shortcut, unnecessary code
            return false;
        if (0.0 <= nextX) { // shortcut, unnecessary code
            return true;
        return 0.0 < nextX + r * (cos(a) + 1.0);

Now it remains to calculate how sharply you need to turn the wheel when shouldSmooth says you should. For this, we set your feeler's position equal to zero and solve for a:

    x + r * (cos(a) + 1.0) = 0.0
        r * (cos(a) + 1.0) = -x
              cos(a) + 1.0 = -x / r
                    cos(a) = -x / r - 1.0
                         a = acos(-x / r - 1.0)

Note here that we are only trying to find the smoothing angle when you are orbiting clockwise and checking against the rightmost wall - otherwise we might have to add PI to the result, or something similar. So, in a java method:

    private double smooth(double a, double x, double s) {
        double nextX = x + s * Math.sin(a);
        if (0.0 <= nextX) { // NOT a shortcut, necessary code
            return Math.PI;
        return Math.acos(-nextX / TURNING_RADIUS - 1.0);

The if statement protects us from giving the acos method illegal values, and therefore from returning NaN. Also notice that we are using the normal a value to project the next point, instead of the smoothed angle. This means you will actually turn slightly sharper than necessary (very slightly). If anyone can solve the equation x + s * sin(a) + r * (cos(a) + 1) = 0.0 for a, please do tell!!

As we just mentioned, this is only good for smoothing when traveling clockwise toward the rightmost wall (and then only if it's at x = 0!). So, to use the methods, you must first translate your bot's situation into this "normal" situation, and translate its answer back. Without further ado, here is how (explanation following):

        double TOP = getBattlefieldHeight() - 18.0;
        double RIGHT = getBattlefieldWidth() - 18.0;
        double BOTTOM = 18.0;
        double LEFT = 18.0;
        double N = 2.0 * Math.PI;
        double E = Math.PI / 2.0;
        double S = Math.PI;
        double W = 3.0 * Math.PI / 2.0;
        double s = 8.0;
        double x = getX();
        double y = getY();
        double a = ...; // whatever angle you wish to travel, we can smooth it!
        boolean clockwise = ...; // your choice!
        if (clockwise) {
            if (S < a) { // left wall
                if (shouldSmooth(a - S, LEFT - x, s)) {
                    a = smooth(a - S, LEFT - x, s) + S;
            } else if (a < S) { // right wall
                if (shouldSmooth(a, x - RIGHT, s)) {
                    a = smooth(a, x - RIGHT, s);
            if (W < a || a < E) { // top wall
                if (shouldSmooth(a + E, y - TOP, s)) {
                    a = smooth(a + E, y - TOP, s) - E;
            } else if (E < a && a < W) { // bottom wall
                if (shouldSmooth(a - E, BOTTOM - y, s)) {
                    a = smooth(a - E, BOTTOM - y, s) + E;
        } else {
            if (S < a) { // left wall
                if (shouldSmooth(N - a, LEFT - x, s)) {
                    a = N - smooth(N - a, LEFT - x, s);
            } else if (a < S) { // right wall
                if (shouldSmooth(S - a, x - RIGHT, s)) {
                    a = S - smooth(S - a, x - RIGHT, s);
            if (W < a || a < E) { // top wall
                if (shouldSmooth(E - a, y - TOP, s)) {
                    a = E - smooth(E - a, y - TOP, s);
            } else if (E < a && a < W) { // bottom wall
                if (shouldSmooth(W - a, BOTTOM - y, s)) {
                    a = W - smooth(W - a, BOTTOM - y, s);

If you want to understand the translations above, this is going to be a "hands-on" process. First visualize what's happening in the smoothing function: hold your left hand in front of you, elbow up, palm facing left, and fingers pointing at 5 o'clock. If your bot is moving clockwise in this direction and it needs to smooth, the algorithm should push your hand toward 6 o'clock. Now consider the first case in the code, where your bot is approaching the left wall. Angle your right hand palm facing left, pointing at about 11 o-clock. If your bot is moving in this direction and needs to smooth, the "wall" will push your hand toward 12 o'clock. Now put your wrists together and do both hands together :). You can see that to translate to or from your bot's current situation to the smoothing method's, you must add or subtract 180 degrees (PI radians). Try it with the other 3 clockwise cases :).

A little trickier is the counter-clockwise cases. Consider the first of these, where your bot is again approaching the left wall. This time hold out your right hand, elbow up, palm right, 7 o'clock. The algorithm should push your hand toward 6 o-clock. Doing both this and the normal smoothing case at the same time, besides looking and feeling really funny, shows you that you need to mirror the angles over 6 o-clock to make the translation, which you accomplish by subtracting either angle from 360 degrees (2 * PI).

If you're still reading this and interested, I'll assume you can handle translating your x or y position without my help :). If you'd like an explanation, leave a note and I'll fill it in :).

Also note that this method handles corners well, e.g. if you are approaching the top left corner going clockwise, and first case does not turn you sharp enough to miss the top wall, the third case will still catch that and turn the additional amount necessary.