Updated algorithm to consider obsticals as well