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.)

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,
  }

}