人工知性を作りたい

私が日々、挑戦したことや学んだことなどを紹介していく雑記ブログです。 (新しいAI技術HTM, 専門の音声信号処理, 趣味のアニメ等も書いてます。)

global navigation menu page top

ソースコード置き場 「pythonによる迷路の自動探索 深さ優先探索編」

f:id:hiro-htm877:20190511223808p:plain



 

記事「pythonによる迷路の自動探索 深さ優先探索編」のソースコート置き場です。

 

下記URL:「pythonによる迷路の自動探索 深さ優先探索編」の記事

 

www.hiro877.com

 

 

ソースコード

import numpy as np
import matplotlib.pyplot as plt
import random
import sys

ROW,COL = 15,15

def presentposition(x, y, Px, Py):
    
    ### mapの端にぶつかった時の処理
    preposi = [0 for i in range(4)]
    if x == COL-1:
        preposi[0] = -1
    else:
        preposi[0] = maze[y][x+1]

    if x == 0:
        preposi[1] = -1
    else:
        preposi[1] = maze[y][x-1]
        
    if y == ROW-1:
        preposi[2] = -1
    else:
        preposi[2] = maze[y+1][x]

    if y == 0:
        preposi[3] = -1
    else:
        preposi[3] = maze[y-1][x]
        
    print(preposi)
    #後ろの座標を確認、通れなくする(=1)
    x2, y2 = x-Px, y-Py
    if x2 == 1:
        preposi[1] = 1 
    if x2 == -1:
        preposi[0] = 1
    if y2 == 1:
        preposi[3] = 1
    if y2 == -1:
        preposi[2] = 1
        
    print ("------------")
    print ("x, y", x, y)
    print ("x2, y2", x2, y2)
    print ("Px, Py", Px, Py)
    
    return preposi

#迷路を定義
maze = np.array(
                [
                    [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1],
                    [1, 0, 1, 1, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1],
                    [1, 0, 1, 1, 1, 1, 0, 1, 1, 0, 0, 0, 0, 1, 1],
                    [1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1],
                    [1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1],
                    [1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1],
                    [1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                ]
    )

mazecopy =  np.array(
                [
                    [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 1],
                    [1, 0, 1, 1, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1],
                    [1, 0, 1, 1, 1, 1, 0, 1, 1, 0, 0, 0, 0, 1, 1],
                    [1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1],
                    [1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1],
                    [1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1],
                    [1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                    [1, 1, 1, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1],
                    [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1],
                ]
    )



x1=1
y1=1      #スタート位置
# x2=13
# y2=13   #ゴール位置
x2=6
y2=3   #ゴール位置

start = (y1, x1)
goal  = (y2, x2)
plt.plot(start[1], start[0], "*", color="tab:grey", markersize=10)
plt.plot(goal[1], goal[0], "*", color="tab:red", markersize=10)

Px, Py = x1, y1-1 #一つ前の座標
x, y = x1, y1     #現在の座標
Px_list, Py_list, U_list, V_list = [Px], [Py], [x-Px], [y-Py]
branch_road_stack = []
branch_road_stack_flag = False
intersection_delete_flag = False


###ゴールするまで進む
while(Px != x2 or Py != y2):
    
    ###壁に突き当たるまで進む
    while(1):
        #現在地の前3つの座標を取得
        preposi = presentposition(x, y, Px, Py)
        print ("preposi", preposi)

        #進める場所のみを抽出
        preposi_Index = [i for i, j in enumerate(preposi) if j == 0]
        print ("preposi_Index", preposi_Index)
        
        ###交差点検知
        intersection_detect = preposi.count(0)
        print ("intersection_detect", intersection_detect)
        if  intersection_detect > 1:
            print ("ここは交差点です")
            print(preposi)
            print (preposi_Index)
            intersection_posi = (x, y)
            
            if branch_road_stack_flag == True:
                print("----test----")
                print (branch_road_stack)
                branch_road_stack.pop(0)
                print ( branch_road_stack)
                branch_road_stack_flag = False
                print(maze)
                print(Px, Py)
                maze[Py][Px] = 1
                print(maze)
                print(x, y)
            
            else:
                ###スタックに状態を追加
                for i in preposi_Index:
                    if i == 0:
                        branch_road_stack.insert(0, (x+1, y))
                    elif i == 1:
                         branch_road_stack.insert(0, (x-1, y))
                    elif i == 2:
                         branch_road_stack.insert(0, (x, y+1))
                    elif i == 3:
                         branch_road_stack.insert(0, (x, y-1))

            print ("branch_road_stack", branch_road_stack)
            Px, Py = x, y
            x, y = branch_road_stack[0]
            break
        
        ###壁判定
        if len(preposi_Index) > 0:
            nextposi = preposi_Index[0]
        else:    ###壁に突き当たった時
            Px, Py = x, y
            branch_road_stack_flag = True
            intersection_delete_flag =True     
            break
                
        if intersection_delete_flag == True:
            if (x,y) == intersection_posi:
                maze[Py][Px] = 1
                print(maze)
                print(branch_road_stack)
                branch_road_stack.pop(0)
                intersection_delete_flag =False
                print(branch_road_stack)            
        
        Px, Py = x, y
        ###進む処理
        if nextposi == 0:
                x += 1 #右に1進む
        elif nextposi == 1:
                x -= 1 #左に1進む
        elif nextposi == 2:
                y += 1 #下に1進む
        elif nextposi == 3:
                y -= 1 #上に1進む
        print("x, y", x, y)
        
        ###プロット処理
        plt.quiver(Px_list, Py_list, U_list, V_list, angles='xy', scale_units='xy', scale=1, color='grey',
          width=0.007, headwidth=2, headlength=5.)
        #迷路の高さと幅を取得
        height, width = maze.shape
        #迷路のプロット
        plt.imshow(mazecopy, cmap="GnBu")
        #縦横比1:1にする
        plt.gca().set_aspect("equal")
        #x軸のラベルを90度回転
        plt.xticks(rotation=90)
        #x軸のメモリをすべて表示
        plt.xticks(np.arange(width), np.arange(width))
        #y軸のメモリをすべて表示
        plt.yticks(np.arange(height), np.arange(height))
        #表示
        plt.show()

        ###プロットで使う定数の更新
        U_list.append(x-Px)
        V_list.append(y-Py)
        
        Px_list.append(Px)
        Py_list.append(Py)



###プロット処理

plt.plot(start[1], start[0], "*", color="tab:grey", markersize=10)
plt.plot(goal[1], goal[0], "*", color="tab:red", markersize=10)

###プロット処理
plt.quiver(Px_list, Py_list, U_list, V_list, angles='xy', scale_units='xy', scale=1, color='grey',
           width=0.007, headwidth=6., headlength=8.)#迷路の高さと幅を取得
height, width = maze.shape
#迷路のプロット
plt.imshow(mazecopy, cmap="GnBu")
#縦横比1:1にする
plt.gca().set_aspect("equal")
#x軸のラベルを90度回転
plt.xticks(rotation=90)
#x軸のメモリをすべて表示
plt.xticks(np.arange(width), np.arange(width))
#y軸のメモリをすべて表示
plt.yticks(np.arange(height), np.arange(height))
#表示
plt.show()