Optimized some MatrixUtils code.