Defined cost functions and changed termination condition to get closer approximations of angle destinations