python宽度优先搜索算法并输出路径

2019独角兽企业重金招聘Python工程师标准>>>

[hadoop@iZ25s7cmfyrZ test]$ cat uda_city.py 
# ----------

# ----------

# Grid format:
#   0 = Navigable space
#   1 = Occupied space
import numpy as np

grid = [[0, 0, 1, 0, 0, 0],
        [0, 0, 1, 0, 0, 0],
        [0, 0, 0, 0, 1, 0],
        [0, 0, 1, 1, 1, 0],
        [0, 0, 0, 0, 1, 0]]
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1

delta = [[-1, 0], # go up
        [ 0,-1], # go left
        [ 0, 1], # go right
        [ 1, 0]] # go down

#delta_name = ['^', '<', 'v', '>']
delta_name = ['^', '<', '>', 'v']

def search(grid,init,goal,cost):
    # ----------------------------------------
    # insert code here
    # ----------------------------------------
    closed=[[0 for _ in range(len(grid[0]))] for _ in range(len(grid))]
    action=[[-1 for row in range(len(grid[0]))] for col in range(len(grid))]

    x=init[0]
    y=init[1]
    g=0
    closed[x][y]=1
    open=[[g,x,y]]
    found=False  
    resign=False
    grid[x][y]=2
    n=0
    
    while found is False and resign is False:
        #print open
        n=n+1
        #print "*"*20
        #print n
        #print np.array(grid)
        if len(open)==0:
            resign=True
            print "fail"
        else:
            ##remove node from list
            open.sort()
            open.reverse()
            next=open.pop()
            ## take 
            x=next[1]
            y=next[2]
            g=next[0]
            #print g
            if x==goal[0] and y==goal[1]:
                found=True
                print next
            else:
                for i in range(len(delta)):
                    x2=x+delta[i][0]
                    y2=y+delta[i][1]
                    #print i, delta[i], x2, y2
                    #print x2, y2
                    if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]):
                        if grid[x2][y2]==0 and closed[x2][y2]==0:
                            g2=g+cost
                            open.append([g2, x2, y2])
                            grid[x2][y2]=2
                            action[x2][y2]=i
                            closed[x2][y2]=1
    return action
print np.array(grid)
action=search(grid,init,goal,cost)   ##注意action中存储的在上一时刻位置 t-1 经过何种动作到达本位置
print np.array(action)
policy=[[" " for row in range(len(grid[0]))] for col in range(len(grid))]  ## policy 中存储的是在当前位置经过何种动作到达下一位置；即由t到t+1
x=goal[0]
y=goal[1]
policy[x][y]="*"
while x!=init[0] or y!=init[1]:
    x2=x-delta[action[x][y]][0]
    y2=y-delta[aciton[x][y]][1]
    policy[x2][y2]=delta_name[action[x][y]]
    x=x2
    y=y2
print np.array(policy)

 

转载于:https://my.oschina.net/lCQ3FC3/blog/859127