(Glide64) Refactor ucode2 code as gSPInsertMatrix function