Navigation function with BFS based Wavefront Algorithm