Use Matrix4 class instead of MyMatrixUtil2
Some modifications