Shifted gears to getting a reliable go_to_point behavior. Moves some
general math functionality into utils module