Updated IK simulation to use more optimized methods
Improved way of calculating theta 3 for IK