Code cleanup, complete Matrix4 and Vector test