Corrected behavior for input theta: now it is w.r.t. to the vertical pointing up
Corrected behavior for input theta: now it is w.r.t. to the vertical pointing up