A*算法的启发式函数或者启发式矩阵生成方法

结果：

[[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)