Finished, again. Still only works 0<=theta<=2*pi Should be easy to use mod to make it general for all rotation angles)