python写A*写到中途

  1 import numpy
  2 import matplotlib.pyplot as plt
  3 from pylab import *
  4 
  5 
  6 map_grid = numpy.full((20, 20), int(10), dtype=numpy.int8)
  7 # print(map_grid)
  8 map_grid[3, 3:8] = 0
  9 map_grid[3:10, 7] = 0
 10 map_grid[10, 3:8] = 0
 11 map_grid[17, 13:17] = 0
 12 map_grid[10:17, 13] = 0
 13 map_grid[10, 13:17] = 0
 14 map_grid[5, 2] = 7
 15 map_grid[15, 15] = 5
 16 
 17 plt.imshow(map_grid, cmap=plt.cm.hot, interpolation=‘nearest‘, vmin=0, vmax=10)
 18 plt.colorbar()
 19 xlim(-1, 20)  # 设置x轴范围
 20 ylim(-1, 20)     # 设置y轴范围
 21 my_x_ticks = numpy.arange(0, 20, 1)
 22 my_y_ticks = numpy.arange(0, 20, 1)
 23 plt.xticks(my_x_ticks)
 24 plt.yticks(my_y_ticks)
 25 plt.grid(True)
 26 plt.show()
 27 
 28 
 29 class AStar(object):
 30     def __init__(self):
 31         self.f = 0
 32         self.g = 0
 33         self.h = 0
 34         self.last_point = numpy.array([[5], [2]])  # 上一个目标点不断取得更新
 35         self.current_point = numpy.array([[5], [2]])  # 当前目标点不断取得更新
 36         self.open = numpy.array([[], []])  # 先创建一个空的open表
 37         self.closed = numpy.array([[], []])  # 先创建一个空的closed表
 38         self.start = numpy.array([[5], [2]])  # 起点坐标
 39         self.goal = numpy.array([[15], [15]])  # 终点坐标
 40 
 41     def h_value(self):
 42         h = (self.current_point[0][0] - 15)**2 + (self.current_point[1][0]-15)**2
 43         h = numpy.sqrt(h)  # 计算h
 44         self.h = h  # 更新h
 45         return h
 46 
 47     def g_value(self):
 48         g1 = self.current_point[0][0]-self.last_point[0][0]
 49         g2 = self.current_point[1][0]-self.last_point[1][0]
 50         g = g1**2 + g2**2
 51         g = numpy.sqrt(g)
 52         self.g = self.g + g
 53         return g
 54 
 55     def f_value(self):
 56         f = self.g_value() + self.h_value()
 57         return f
 58 
 59     def h_value_tem(self, cur_p):
 60         h = (cur_p[0] - 15)**2 + (cur_p[1]-15)**2
 61         h = numpy.sqrt(h)  # 计算h
 62 
 63         return h
 64 
 65     def g_value_tem(self, las_p, cu_p):
 66         g1 = cu_p[0]-las_p[0][0]
 67         g2 = cu_p[1]-las_p[1][0]
 68         g = g1**2 + g2**2
 69         g = numpy.sqrt(g)
 70 
 71         return g
 72 
 73     def f_value_tem(self, las_p, cu_p):
 74 
 75         f = self.g_value_tem(las_p, cu_p) + self.h_value_tem(cu_p)
 76         return f
 77 
 78     def min_f(self):
 79         tem_f = []
 80 
 81         for i in range(self.open.shape[1]):
 82             f_list = self.f_value_tem(self.last_point, self.open[:, i])
 83             tem_f.append(f_list)
 84 
 85         index = tem_f.index(min(tem_f))  # 返回最小值
 86         location = self.open[:, index]
 87 
 88         numpy.column_stack((self.closed, location))
 89         return location
 90 
 91     def main(self):
 92         self.open = numpy.column_stack((self.open, self.start))
 93         x = self.open
 94         f = self.h
 95 
 96         # self.open.shape[0]
 97         if self.open.shape[0] != 0:
 98             if self.min_f() == [15,15]:
 99                 print(‘搜索成功啦！‘)
100                 break
101             else:
102                 
103             
104             
105 
106 
107 
108 
109 
110         else:
111             print(‘没有搜索到路径！‘)