Path finding algorithm improvements