ADD: path finding algorithm for Map