Fixed theta calculations by reversing sinTheta and cosTheta arguments for atan2() based on Radke's advice.
Cleaned up comments and check functions now that rotmat2axisAngle is working as it should.
Added check function (currently shows that my calculations from axisAngle2rotmat are still incorrect (would expect R2_axisAngle2rotmat == R2)
Attempts at optimizing and fixing axisAngle2rotmat(), optimized rn assignment.
Cleaning up formatting to make code easier  to read