Matrix4 improvement

Merge with MasDennis/Rajawali#1018