Wall Smoothing/Precise

Unlike any other wall smoothing method, all using some kind of imaginary stick — this approach simulates exact robocode physics without using iterations. (as the term Precise is generally used for methods simulating exact physics (it doesn't matter whether using iteration or not, if the result is the same), like in PreciseIntersection and PrecisePrediction, etc.)

The code is released under Public Domain.

/*
 * This is free and unencumbered software released into the public domain.
 * 
 * Anyone is free to copy, modify, publish, use, compile, sell, or
 * distribute this software, either in source code form or as a compiled
 * binary, for any purpose, commercial or non-commercial, and by any
 * means.
 * 
 * In jurisdictions that recognize copyright laws, the author or authors
 * of this software dedicate any and all copyright interest in the
 * software to the public domain. We make this dedication for the benefit
 * of the public at large and to the detriment of our heirs and
 * successors. We intend this dedication to be an overt act of
 * relinquishment in perpetuity of all present and future rights to this
 * software under copyright law.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
 * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 * 
 * For more information, please refer to <http://unlicense.org/>* 
 */

public final class PreciseWallSmooth {
  private static final double DELTA_SIN = Math.sin(Rules.getTurnRateRadians(Rules.MAX_VELOCITY) / 2);
  private static final double DELTA_COS = Math.sqrt(1 - DELTA_SIN * DELTA_SIN);
  private static final double INNER_R = Rules.MAX_VELOCITY / 2 / Math.tan(Rules.getTurnRateRadians(Rules.MAX_VELOCITY) / 2);
  private static final double OUTER_R = Rules.MAX_VELOCITY / 2 / DELTA_SIN;
  private static final double WALL_PADDING = 18.5;

  private final double MIN_X;
  private final double MIN_Y;
  private final double MAX_X;
  private final double MAX_Y;

  public PreciseWallSmooth(double fieldWidth, double fieldHeight) {
    MIN_X = WALL_PADDING;
    MIN_Y = WALL_PADDING;
    MAX_X = fieldWidth - WALL_PADDING;
    MAX_Y = fieldHeight - WALL_PADDING;
  }

  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 (smoothWall == firstWall) {
        smoothWall = secondWall;
        smoothDist = secondDist;
      } else {
        break;
      }
    }
    return absoluteHeading;
  }

  enum WALL {
    W, N, E, S,
  }

}