Update MathUtil.java

More efficient implementation