Fixed a bug where initial endpoint is not calculated properly. Limited the magnitude of dtheta.