Manipulating matrices for 3D drawing
As far as readability goes, maybe you'd like to add an arg1 arg2 arg3 -- result1 result2 type of comment to each function? So we know what was on the stack before invoking, and what we expect to find afterward? Almost equivalently, you might show a unit test invoking your function and verifying the result.