Self packing texture atlas

Praised be GL3+

public class TextureAtlas {
  private int size;
  public byte[] buffer;
  private byte[] occupied;

  /**
   * Creates a new {@link TextureAtlas}.
   *
   * @param size        the size of the atlas as an exponent for a power of two.
   * @param elementSize the size of the elements for the atlas in bytes.
   *
   * @throws IllegalArgumentException if the size is less than two.
   * @throws IllegalArgumentException if the element size is less than one.
   */
  public TextureAtlas(int size, int elementSize) {
    if (size < 1) {
      throw new IllegalArgumentException("Expected size to be greater than one.");
    }

    if (elementSize < 1) {
      throw new IllegalArgumentException("Expected element size to be greater than zero.");
    }

    this.size = size;
    buffer = new byte[(1 << size + size) * elementSize];
    occupied = new byte[(1 << size + size >> 4) + 1];
  }

  /**
   * Finds a free section in the atlas of a provided size. The space will be aligned with
   * all the other spaces of the same size.
   *
   * @param exp the size of the space as an exponent for a power of two.
   * @return the z order number of the free space. The z order number will be in the lowest
   * unit of measurement allowed so 2**1.
   */
  private int findFree(int exp) {
    int bitLen = (1 << exp + exp) >> 2;
    for (int bitOff = 0; bitOff < (1 << size + size) >> 2; bitOff += bitLen) {
      if (!occupied(bitOff, bitLen)) {
        return bitOff;
      }
    }
    return -1;
  }

  /**
   * Gets if a section of bits are marked as occupied.
   *
   * @param start the start bit offset.
   * @param len   the amount of bits to test if occupied.
   * @return if the section of bits aren't occupied.
   */
  private boolean occupied(int start, int len) {
    int off = start;
    while (len > 0) {
      int readLen = Math.min(len, ((off >> 3) + 1 << 3) - off);
      int mask = (1 << readLen) - 1;
      byte b = occupied[off >> 3];
      if (((b >>> (off & 7)) & mask) != 0) {
        return true;
      }
      off += readLen;
      len -= readLen;
    }
    return false;
  }

  /**
   * Marks a section of bits as occupied.
   *
   * @param start the start bit offset.
   * @param len   the amount of bits to mark as occupied.
   */
  private void mark(int start, int len) {
    int off = start;
    while (len > 0) {
      int writeLen = Math.min(len, ((off >> 3) + 1 << 3) - off);
      int mask = (1 << writeLen) - 1;
      occupied[off >> 3] |= mask << (off & 7);
      off += writeLen;
      len -= writeLen;
    }
  }

  public boolean insert(byte[] bytes, int off, int len) {
    if(off < 0 || off + len >= bytes.length) {
      throw new ArrayIndexOutOfBoundsException();
    }
    if (!isPo2(len)) {
      throw new IllegalArgumentException("Expected length to be a power of two.");
    }
    int exp = log2(len) >> 1;
    int free = findFree(exp);
    if (free == -1) {
      return false;
    }
    mark(free, (1 << exp + exp) >> 2);
    int mx = mortonCodeX(free) << 1;
    int my = mortonCodeY(free) << 1;
    int span = 1 << exp;
    for (int y = my; y < my + span; y++) {
      int destOff = y * (1 << size) + mx;
      System.arraycopy(bytes, off, buffer, destOff, span);
      off += span;
    }
    return true;
  }

  private static boolean isPo2(int n) {
    return (n & (-n)) == n;
  }

  private static int mortonCodeX(int code) {
    return compactBy1(code);
  }

  private static int mortonCodeY(int code) {
    return compactBy1(code >> 1);
  }

  private static int compactBy1(int x) {
    x &= 0x55555555;
    x = (x ^ (x >> 1)) & 0x33333333;
    x = (x ^ (x >> 2)) & 0x0f0f0f0f;
    x = (x ^ (x >> 4)) & 0x00ff00ff;
    x = (x ^ (x >> 8)) & 0x0000ffff;
    return x;
  }

  private static int log2(int i) {
    int n = 1;
    if (i >>> 16 == 0) {
      n += 16;
      i <<= 16;
    }
    if (i >>> 24 == 0) {
      n += 8;
      i <<= 8;
    }
    if (i >>> 28 == 0) {
      n += 4;
      i <<= 4;
    }
    if (i >>> 30 == 0) {
      n += 2;
      i <<= 2;
    }
    n -= i >>> 31;
    return 31 - n;
  }
}