Fix 3x3 rotation matrix generation + clean up a bit

3x3 rotation matrices were being generated incorrectly, it took me quite a while until I found out the bug isn't in my own code. :)