結果：

[[100  99  98 ...  97  98  99]
 [ 99  98  97 ...  96  97  98]
 [ 98  97  96 ...  95  96  97]
 ...
 [ 97  96  95 ...  94  95  96]
 [ 98  97  96 ...  95  96  97]
 [ 99  98  97 ...  96  97  98]]

'''
此程式：建立啟發式A*函式如圖4，4 為起始點向外擴充套件

[[8 7 6 5 4 5]
 [7 6 5 4 3 4]
 [6 5 4 3 2 3]
 [5 4 3 2 1 2]
 [4 3 2 1 0 1]]

 虛擬碼：
 先思考需要什麼變數：1，map矩陣，2，啟發函式 3，方向，上下左右 4，目標點 5，open=[g,y,x] y,x指的是螢幕座標
 1 ，先對一個點擴充套件上下左右擴充套件，迴圈4次 ---列表裡面沒有點了，就沒有辦法擴充套件了
 2，在1之前 檢測open是否含有點，如果沒有--》結束程式
 3，檢測有點，則將open裡面最小的g拿出來，（不是檢視，而是拿出來一個就少一個）======先對open排序，從小到大排序，在翻轉，在pop出來，
 4  將拿出來的點進行上下左右擴充套件迴圈4次：
    1，每一次擴充套件都要檢測是不是被擴充套件過了，是不是超出界限了， 如果是則不進行擴充套件而繼續下一次擴充套件
    2，如果可以擴充套件則將g值+1放到擴充套件到的位置上，，並將這個點放入open中，在將這個位置Mark一下

'''

import numpy as np

def Geo_matrix(m,n,goal=[]):
    '''

    :param m:
    :param n:
    :param goal: 是一個兩個元素的列表
    :return:
    '''
    map = np.zeros((m, n), dtype=int)  # 2x1矩陣
    heuristic2 = np.zeros((m, n), dtype=int)
    mark_heuristic2 = np.zeros((m, n), dtype=int)
    # if heuristic2[goal[0]][goal[1]] == 0:
    delta = [[-1, 0],  # 上
             [0, -1],  # 左
             [1, 0],  # 下
             [0, 1]]  # 右


    x = goal[0]
    y = goal[1]
    g = 0
    open = [[g, x, y]]


    mark_heuristic2[goal[0]][goal[1]] = True
    while True:
        if len(open) == 0:
            print('沒有可擴充套件的點了')
            break

        open.sort()
        open.reverse()
        next = open.pop()

        y = next[1]
        x = next[2]
        g = next[0]


        for i in range(len(delta)):
            y2 = y + delta[i][0]   #矩陣設為列為x，行數為y
            x2 = x + delta[i][1]

            if y2< 0 or y2 >= len(heuristic2) or x2 <0 or x2 >=len(heuristic2[0]):
                continue
            if mark_heuristic2[y2][x2]:
                continue
            else :
                g2 = g + 1
                open.append([g2,y2,x2])
                heuristic2[y2][x2] = g2
                mark_heuristic2[y2][x2]  = True   #表示這個被被標記了


    # print(heuristic2)
    return heuristic2
heuristic2 = Geo_matrix(100,100,[50,50])
print(heuristic2)