1 # -*- coding: utf-8 -*- 2 import math 3 import numpy 4 import matplotlib.pyplot as plt 5 from pylab import * 6 7 8 9 # 地图 10 tm = [ 11 '############################################################', 12 '#..........................................................#', 13 '#.............................#............................#', 14 '#.............................#............................#', 15 '#.............................#............................#', 16 '#.......S....#.................#...........................#', 17 '#.............................#............................#', 18 '#.............................#............................#', 19 '#.............................#............................#', 20 '#........................#....##...........................#', 21 '#.............................#............................#', 22 '#.............................#............................#', 23 '#.............................#............................#', 24 '#######.#######################################............#', 25 '#....#........#............................................#', 26 '#....#........#............................................#', 27 '#....##########............................................#', 28 '#..........................................................#', 29 '#..........................................................#', 30 '#..........................................................#', 31 '#..........................................................#', 32 '#..........................................................#', 33 '#...............................##############.............#', 34 '#...............................#........E...#.............#', 35 '#...............................#............#.............#', 36 '#...............................#............#.............#', 37 '#...............................#............#.............#', 38 '#...............................###########..#.............#', 39 '#..........................................................#', 40 '#..........................................................#', 41 '############################################################'] 42 43 # 因为python里string不能直接改变某一元素，所以用test_map来存储搜索时的地图 44 test_map = [] 45 46 47 ######################################################### 48 class Node_Elem: 49 """ 50 开放列表和关闭列表的元素类型，parent用来在成功的时候回溯路径 51 """ 52 53 def __init__(self, parent, x, y, dist): 54 self.parent = parent 55 self.x = x 56 self.y = y 57 self.dist = dist 58 59 60 class A_Star: 61 """ 62 A星算法实现类 63 """ 64 65 # 注意w,h两个参数，如果你修改了地图，需要传入一个正确值或者修改这里的默认参数 66 def __init__(self, s_x, s_y, e_x, e_y, w=60, h=30): 67 self.s_x = s_x 68 self.s_y = s_y 69 self.e_x = e_x 70 self.e_y = e_y 71 72 self.width = w 73 self.height = h 74 75 self.open = [] 76 self.close = [] 77 self.path = [] 78 79 # 查找路径的入口函数 80 def find_path(self): 81 # 构建开始节点 82 p = Node_Elem(None, self.s_x, self.s_y, 0.0) 83 while True: 84 # 扩展F值最小的节点 85 self.extend_round(p) 86 # 如果开放列表为空，则不存在路径，返回 87 if not self.open: 88 return 89 # 获取F值最小的节点 90 idx, p = self.get_best() 91 # 找到路径，生成路径，返回 92 if self.is_target(p): 93 self.make_path(p) 94 return 95 # 把此节点压入关闭列表，并从开放列表里删除 96 self.close.append(p) 97 del self.open[idx] 98 99 def make_path(self, p): 100 # 从结束点回溯到开始点，开始点的parent == None 101 while p: 102 self.path.append((p.x, p.y)) 103 p = p.parent 104 105 def is_target(self, i): 106 return i.x == self.e_x and i.y == self.e_y 107 108 def get_best(self): 109 best = None 110 bv = 1000000 # 如果你修改的地图很大，可能需要修改这个值 111 bi = -1 112 for idx, i in enumerate(self.open): 113 value = self.get_dist(i) # 获取F值 114 if value < bv: # 比以前的更好，即F值更小 115 best = i 116 bv = value 117 bi = idx 118 return bi, best 119 120 def get_dist(self, i): 121 # F = G + H 122 # G 为已经走过的路径长度， H为估计还要走多远 123 # 这个公式就是A*算法的精华了。 124 return i.dist + math.sqrt( 125 (self.e_x - i.x) * (self.e_x - i.x) 126 + (self.e_y - i.y) * (self.e_y - i.y)) * 1.2 127 128 def extend_round(self, p): 129 # 可以从8个方向走 130 xs = (-1, 0, 1, -1, 1, -1, 0, 1) 131 ys = (-1, -1, -1, 0, 0, 1, 1, 1) 132 133 for x, y in zip(xs, ys): 134 new_x, new_y = x + p.x, y + p.y 135 # 无效或者不可行走区域，则勿略 136 if not self.is_valid_coord(new_x, new_y): 137 continue 138 # 构造新的节点 139 node = Node_Elem(p, new_x, new_y, p.dist + self.get_cost( 140 p.x, p.y, new_x, new_y)) 141 # 新节点在关闭列表，则忽略 142 if self.node_in_close(node): 143 continue 144 i = self.node_in_open(node) 145 if i != -1: 146 # 新节点在开放列表 147 if self.open[i].dist > node.dist: 148 # 现在的路径到比以前到这个节点的路径更好~ 149 # 则使用现在的路径 150 self.open[i].parent = p 151 self.open[i].dist = node.dist 152 continue 153 self.open.append(node) 154 155 def get_cost(self, x1, y1, x2, y2): 156 """ 157 上下左右直走，代价为1.0，斜走，代价为1.4 158 """ 159 if x1 == x2 or y1 == y2: 160 return 1.0 161 return 1.4 162 163 def node_in_close(self, node): 164 for i in self.close: 165 if node.x == i.x and node.y == i.y: 166 return True 167 return False 168 169 def node_in_open(self, node): 170 for i, n in enumerate(self.open): 171 if node.x == n.x and node.y == n.y: 172 return i 173 return -1 174 175 def is_valid_coord(self, x, y): 176 if x < 0 or x >= self.width or y < 0 or y >= self.height: 177 return False 178 return test_map[y][x] != '#' 179 180 def get_searched(self): 181 l = [] 182 for i in self.open: 183 l.append((i.x, i.y)) 184 for i in self.close: 185 l.append((i.x, i.y)) 186 return l 187 188 189 ######################################################### 190 def print_test_map(): 191 """ 192 打印搜索后的地图 193 """ 194 for line in test_map: 195 print(''.join(line)) 196 197 198 def get_start_XY(): 199 return get_symbol_XY('S') 200 201 202 def get_end_XY(): 203 return get_symbol_XY('E') 204 205 206 def get_symbol_XY(s): 207 for y, line in enumerate(test_map): 208 try: 209 x = line.index(s) 210 except: 211 continue 212 else: 213 break 214 return x, y 215 216 217 ######################################################### 218 def mark_path(l): 219 mark_symbol(l, '*') 220 221 222 def mark_searched(l): 223 mark_symbol(l, ' ') 224 225 226 def mark_symbol(l, s): 227 for x, y in l: 228 test_map[y][x] = s 229 230 231 def mark_start_end(s_x, s_y, e_x, e_y): 232 test_map[s_y][s_x] = 'S' 233 test_map[e_y][e_x] = 'E' 234 235 236 def tm_to_test_map(): 237 for line in tm: 238 test_map.append(list(line)) 239 240 241 def find_path(): 242 s_x, s_y = get_start_XY() 243 e_x, e_y = get_end_XY() 244 a_star = A_Star(s_x, s_y, e_x, e_y) 245 a_star.find_path() 246 searched = a_star.get_searched() 247 path = a_star.path 248 # 标记已搜索区域 249 mark_searched(searched) 250 # 标记路径 251 mark_path(path) 252 print("path length is %d" % (len(path))) 253 print("searched squares count is %d" % (len(searched))) 254 # 标记开始、结束点 255 mark_start_end(s_x, s_y, e_x, e_y) 256 257 258 if __name__ == "__main__": 259 # 把字符串转成列表 260 tm_to_test_map() 261 find_path() 262 print_test_map()