MathUtil.java

package com.hypixel.hytale.math.util;

import com.hypixel.hytale.math.vector.Vector3d;
import com.hypixel.hytale.math.vector.Vector3f;
import com.hypixel.hytale.math.vector.Vector3i;
import java.util.concurrent.ThreadLocalRandom;
import javax.annotation.Nonnull;

public class MathUtil {
   public static final double EPSILON_DOUBLE = Math.ulp(1.0);
   public static final float EPSILON_FLOAT = Math.ulp(1.0F);
   public static float PITCH_EDGE_PADDING = 0.01F;

   public static int abs(int i) {
      int mask = i >> 31;
      return i + mask ^ mask;
   }

   public static int floor(double d) {
      int i = (int)d;
      if ((double)i <= d) {
         return i;
      } else {
         return d < -2.147483648E9 ? -2147483648 : i - 1;
      }
   }

   public static int ceil(double d) {
      int i = (int)d;
      if (d > 0.0 && d != (double)i) {
         return d > 2.147483647E9 ? 2147483647 : i + 1;
      } else {
         return i;
      }
   }

   public static int randomInt(int min, int max) {
      return ThreadLocalRandom.current().nextInt(min, max);
   }

   public static double randomDouble(double min, double max) {
      return min + Math.random() * (max - min);
   }

   public static float randomFloat(float min, float max) {
      return min + (float)Math.random() * (max - min);
   }

   public static double round(double d, int p) {
      double pow = Math.pow(10.0, (double)p);
      return (double)Math.round(d * pow) / pow;
   }

   public static boolean within(double val, double min, double max) {
      return val >= min && val <= max;
   }

   public static double minValue(double v, double a, double c) {
      if (a < v) {
         v = a;
      }

      if (c < v) {
         v = c;
      }

      return v;
   }

   public static int minValue(int v, int a, int c) {
      if (a < v) {
         v = a;
      }

      if (c < v) {
         v = c;
      }

      return v;
   }

   public static double maxValue(double v, double a, double b, double c) {
      if (a > v) {
         v = a;
      }

      if (b > v) {
         v = b;
      }

      if (c > v) {
         v = c;
      }

      return v;
   }

   public static double maxValue(double v, double a, double b) {
      if (a > v) {
         v = a;
      }

      if (b > v) {
         v = b;
      }

      return v;
   }

   public static byte maxValue(byte v, byte a, byte b) {
      if (a > v) {
         v = a;
      }

      if (b > v) {
         v = b;
      }

      return v;
   }

   public static byte maxValue(byte v, byte a, byte b, byte c) {
      if (a > v) {
         v = a;
      }

      if (b > v) {
         v = b;
      }

      if (c > v) {
         v = c;
      }

      return v;
   }

   public static int maxValue(int v, int a, int b) {
      if (a > v) {
         v = a;
      }

      if (b > v) {
         v = b;
      }

      return v;
   }

   public static double lengthSquared(double x, double y) {
      return x * x + y * y;
   }

   public static double length(double x, double y) {
      return Math.sqrt(lengthSquared(x, y));
   }

   public static double lengthSquared(double x, double y, double z) {
      return x * x + y * y + z * z;
   }

   public static double length(double x, double y, double z) {
      return Math.sqrt(lengthSquared(x, y, z));
   }

   public static double maxValue(double v, double a) {
      return a > v ? a : v;
   }

   public static double clipToZero(double v) {
      return clipToZero(v, EPSILON_DOUBLE);
   }

   public static double clipToZero(double v, double epsilon) {
      return v >= -epsilon && v <= epsilon ? 0.0 : v;
   }

   public static float clipToZero(float v) {
      return clipToZero(v, EPSILON_FLOAT);
   }

   public static float clipToZero(float v, float epsilon) {
      return v >= -epsilon && v <= epsilon ? 0.0F : v;
   }

   public static boolean closeToZero(double v) {
      return closeToZero(v, EPSILON_DOUBLE);
   }

   public static boolean closeToZero(double v, double epsilon) {
      return v >= -epsilon && v <= epsilon;
   }

   public static boolean closeToZero(float v) {
      return closeToZero(v, EPSILON_FLOAT);
   }

   public static boolean closeToZero(float v, float epsilon) {
      return v >= -epsilon && v <= epsilon;
   }

   public static double clamp(double v, double min, double max) {
      if (v > max) {
         return v < min ? min : max;
      } else {
         return v < min ? min : v;
      }
   }

   public static float clamp(float v, float min, float max) {
      if (v > max) {
         return v < min ? min : max;
      } else {
         return v < min ? min : v;
      }
   }

   public static int clamp(int v, int min, int max) {
      if (v > max) {
         return v < min ? min : max;
      } else {
         return v < min ? min : v;
      }
   }

   public static long clamp(long v, long min, long max) {
      if (v > max) {
         return v < min ? min : max;
      } else {
         return v < min ? min : v;
      }
   }

   public static int getPercentageOf(int index, int max) {
      return (int)((double)index / ((double)max - 1.0) * 100.0);
   }

   public static double percent(int v, int total) {
      return total == 0 ? 0.0 : (double)v * 100.0 / (double)total;
   }

   public static int fastRound(float f) {
      return fastFloor(f + 0.5F);
   }

   public static long fastRound(double d) {
      return fastFloor(d + 0.5);
   }

   public static int fastFloor(float f) {
      int i = (int)f;
      if ((float)i <= f) {
         return i;
      } else {
         return f < -2.1474836E9F ? -2147483648 : i - 1;
      }
   }

   public static long fastFloor(double d) {
      long i = (long)d;
      if ((double)i <= d) {
         return i;
      } else {
         return d < -9.223372036854776E18 ? -9223372036854775808L : i - 1L;
      }
   }

   public static int fastCeil(float f) {
      int i = (int)f;
      if (f > 0.0F && f != (float)i) {
         return f > 2.1474836E9F ? 2147483647 : i + 1;
      } else {
         return i;
      }
   }

   public static long fastCeil(double d) {
      long i = (long)d;
      if (d > 0.0 && d != (double)i) {
         return d > 9.223372036854776E18 ? 9223372036854775807L : i + 1L;
      } else {
         return i;
      }
   }

   private MathUtil() {
   }

   public static float halfFloatToFloat(int hbits) {
      int mant = hbits & 1023;
      int exp = hbits & 31744;
      if (exp == 31744) {
         exp = 261120;
      } else if (exp != 0) {
         exp += 114688;
         if (mant == 0 && exp > 115712) {
            return Float.intBitsToFloat((hbits & '耀') << 16 | exp << 13 | 1023);
         }
      } else if (mant != 0) {
         exp = 115712;

         do {
            mant <<= 1;
            exp -= 1024;
         } while((mant & 1024) == 0);

         mant &= 1023;
      }

      return Float.intBitsToFloat((hbits & '耀') << 16 | (exp | mant) << 13);
   }

   public static int halfFloatFromFloat(float fval) {
      int fbits = Float.floatToIntBits(fval);
      int sign = fbits >>> 16 & '耀';
      int val = (fbits & 2147483647) + 4096;
      if (val >= 1199570944) {
         if ((fbits & 2147483647) >= 1199570944) {
            return val < 2139095040 ? sign | 31744 : sign | 31744 | (fbits & 8388607) >>> 13;
         } else {
            return sign | 31743;
         }
      } else if (val >= 947912704) {
         return sign | val - 939524096 >>> 13;
      } else if (val < 855638016) {
         return sign;
      } else {
         val = (fbits & 2147483647) >>> 23;
         return sign | (fbits & 8388607 | 8388608) + (8388608 >>> val - 102) >>> 126 - val;
      }
   }

   public static int byteCount(int i) {
      if (i > 65535) {
         return 4;
      } else if (i > 255) {
         return 2;
      } else {
         return i > 0 ? 1 : 0;
      }
   }

   public static int packInt(int x, int z) {
      return x << 16 | z & '\uffff';
   }

   public static int unpackLeft(int packed) {
      int i = packed >> 16 & '\uffff';
      if ((i & '耀') != 0) {
         i |= -65536;
      }

      return i;
   }

   public static int unpackRight(int packed) {
      int i = packed & '\uffff';
      if ((i & '耀') != 0) {
         i |= -65536;
      }

      return i;
   }

   public static long packLong(int left, int right) {
      return (long)left << 32 | (long)right & 4294967295L;
   }

   public static int unpackLeft(long packed) {
      return (int)(packed >> 32);
   }

   public static int unpackRight(long packed) {
      return (int)packed;
   }

   @Nonnull
   public static Vector3i rotateVectorYAxis(@Nonnull Vector3i vector, int angle, boolean clockwise) {
      float radAngle = 0.017453292F * (float)angle;
      int x1;
      int z1;
      if (clockwise) {
         x1 = (int)((float)vector.x * TrigMathUtil.cos(radAngle) - (float)vector.z * TrigMathUtil.sin(radAngle));
         z1 = (int)((float)vector.x * TrigMathUtil.sin(radAngle) + (float)vector.z * TrigMathUtil.cos(radAngle));
      } else {
         x1 = (int)((float)vector.x * TrigMathUtil.cos(radAngle) + (float)vector.z * TrigMathUtil.sin(radAngle));
         z1 = (int)((float)(-vector.x) * TrigMathUtil.sin(radAngle) + (float)vector.z * TrigMathUtil.cos(radAngle));
      }

      return new Vector3i(x1, vector.y, z1);
   }

   @Nonnull
   public static Vector3d rotateVectorYAxis(@Nonnull Vector3d vector, int angle, boolean clockwise) {
      float radAngle = 0.017453292F * (float)angle;
      double x1;
      double z1;
      if (clockwise) {
         x1 = vector.x * (double)TrigMathUtil.cos(radAngle) - vector.z * (double)TrigMathUtil.sin(radAngle);
         z1 = vector.x * (double)TrigMathUtil.sin(radAngle) + vector.z * (double)TrigMathUtil.cos(radAngle);
      } else {
         x1 = vector.x * (double)TrigMathUtil.cos(radAngle) + vector.z * (double)TrigMathUtil.sin(radAngle);
         z1 = -vector.x * (double)TrigMathUtil.sin(radAngle) + vector.z * (double)TrigMathUtil.cos(radAngle);
      }

      return new Vector3d(x1, vector.y, z1);
   }

   public static float wrapAngle(float angle) {
      angle %= 6.2831855F;
      if (angle <= -3.1415927F) {
         angle += 6.2831855F;
      } else if (angle > 3.1415927F) {
         angle -= 6.2831855F;
      }

      return angle;
   }

   public static float lerp(float a, float b, float t) {
      return lerpUnclamped(a, b, clamp(t, 0.0F, 1.0F));
   }

   public static float lerpUnclamped(float a, float b, float t) {
      return a + t * (b - a);
   }

   public static double lerp(double a, double b, double t) {
      return lerpUnclamped(a, b, clamp(t, 0.0, 1.0));
   }

   public static double lerpUnclamped(double a, double b, double t) {
      return a + t * (b - a);
   }

   public static float shortAngleDistance(float a, float b) {
      float distance = (b - a) % 6.2831855F;
      return 2.0F * distance % 6.2831855F - distance;
   }

   public static float lerpAngle(float a, float b, float t) {
      return a + shortAngleDistance(a, b) * t;
   }

   public static double floorMod(double x, double y) {
      return x - Math.floor(x / y) * y;
   }

   public static double compareAngle(double a, double b) {
      double diff = b - a;
      return floorMod(diff + 3.141592653589793, 6.283185307179586) - 3.141592653589793;
   }

   public static double percentile(@Nonnull long[] sortedData, double percentile) {
      if (sortedData.length == 1) {
         return (double)sortedData[0];
      } else if (percentile >= 1.0) {
         return (double)sortedData[sortedData.length - 1];
      } else {
         double position = (double)(sortedData.length + 1) * percentile;
         double n = percentile * (double)(sortedData.length - 1) + 1.0;
         long left;
         long right;
         if (position >= 1.0) {
            left = sortedData[floor(n) - 1];
            right = sortedData[floor(n)];
         } else {
            left = sortedData[0];
            right = sortedData[1];
         }

         if (left == right) {
            return (double)left;
         } else {
            double part = n - (double)floor(n);
            return (double)left + part * (double)(right - left);
         }
      }
   }

   public static double distanceToLineSq(double x, double y, double ax, double ay, double bx, double by) {
      double dx0 = x - ax;
      double dy0 = y - ay;
      double dx1 = bx - ax;
      double dy1 = by - ay;
      return distanceToLineSq(x, y, ax, ay, bx, by, dx0, dy0, dx1, dy1);
   }

   public static double distanceToLineSq(double x, double y, double ax, double ay, double bx, double by, double dxAx, double dyAy, double dBxAx, double dByAy) {
      double t = dxAx * dBxAx + dyAy * dByAy;
      t /= dBxAx * dBxAx + dByAy * dByAy;
      double px = ax;
      double py = ay;
      if (t > 1.0) {
         px = bx;
         py = by;
      } else if (t > 0.0) {
         px = ax + t * dBxAx;
         py = ay + t * dByAy;
      }

      dBxAx = x - px;
      dByAy = y - py;
      return dBxAx * dBxAx + dByAy * dByAy;
   }

   public static double distanceToInfLineSq(double x, double y, double ax, double ay, double bx, double by) {
      double dx0 = x - ax;
      double dy0 = y - ay;
      double dx1 = bx - ax;
      double dy1 = by - ay;
      return distanceToInfLineSq(x, y, ax, ay, dx0, dy0, dx1, dy1);
   }

   public static double distanceToInfLineSq(double x, double y, double ax, double ay, double dxAx, double dyAy, double dBxAx, double dByAy) {
      double t = dxAx * dBxAx + dyAy * dByAy;
      t /= dBxAx * dBxAx + dByAy * dByAy;
      double px = ax + t * dBxAx;
      double py = ay + t * dByAy;
      dBxAx = x - px;
      dByAy = y - py;
      return dBxAx * dBxAx + dByAy * dByAy;
   }

   public static int sideOfLine(double x, double y, double ax, double ay, double bx, double by) {
      return (ax - x) * (by - y) - (ay - y) * (bx - x) >= 0.0 ? 1 : -1;
   }

   public static Vector3f getRotationForHitNormal(Vector3f normal) {
      if (normal == null) {
         return Vector3f.ZERO;
      } else if (normal.y == 1.0F) {
         return Vector3f.ZERO;
      } else if (normal.y == -1.0F) {
         return new Vector3f(0.0F, 0.0F, 3.1415927F);
      } else if (normal.x == 1.0F) {
         return new Vector3f(0.0F, 0.0F, -1.5707964F);
      } else if (normal.x == -1.0F) {
         return new Vector3f(0.0F, 0.0F, 1.5707964F);
      } else if (normal.z == 1.0F) {
         return new Vector3f(1.5707964F, 0.0F, 0.0F);
      } else {
         return normal.z == -1.0F ? new Vector3f(-1.5707964F, 0.0F, 0.0F) : Vector3f.ZERO;
      }
   }

   public static String getNameForHitNormal(Vector3f normal) {
      if (normal == null) {
         return "UP";
      } else if (normal.y == 1.0F) {
         return "UP";
      } else if (normal.y == -1.0F) {
         return "DOWN";
      } else if (normal.x == 1.0F) {
         return "WEST";
      } else if (normal.x == -1.0F) {
         return "EAST";
      } else if (normal.z == 1.0F) {
         return "NORTH";
      } else {
         return normal.z == -1.0F ? "SOUTH" : "UP";
      }
   }

   public static float mapToRange(float value, float valueMin, float valueMax, float rangeMin, float rangeMax) {
      float alpha = (value - valueMin) / (valueMax - valueMin);
      return rangeMin + alpha * (rangeMax - rangeMin);
   }
}