Added the wavefront algorithm implementation. Also this can make the obstacles bigger.