def solve(num_arr, start, end, w=1): goal = vertex.Vertex(end, 0) g = {} g[utils.coord_id(start)] = 0 g_h_f = {} g_h_f[utils.coord_id(start)] = [0, heur(start, end), w * heur(start, end)] parent = {} parent[utils.coord_id(start)] = start fr = fringe.Fringe() fr.insert(vertex.Vertex(start, g[utils.coord_id(start)] + heur(start, end))) closed = set() while len(fr) != 0: s = fr.pop() if s == goal: path = utils.goal_path(start, end, parent) return True, path, g_h_f, len(closed) closed.add(s) for s_p in utils.succ(num_arr, s, closed): if s_p not in fr.to_list(): sp_cid = utils.coord_id([s_p.x, s_p.y]) g[sp_cid] = math.inf parent[sp_cid] = None update_vertex(s, s_p, g, parent, fr, end, num_arr, w, g_h_f) return False, [], [], len(closed)
def solve(num_arr, start, end): goal = vertex.Vertex(end, 0) # g is now cost g = {} g[utils.coord_id(start)] = 0 parent = {} parent[utils.coord_id(start)] = start fr = fringe.Fringe() fr.insert(vertex.Vertex(start, 0)) closed = set() while len(fr) != 0: s = fr.pop() s_c = [s.x, s.y] if s == goal: path = utils.goal_path(start, end, parent) return True, path, g, len(closed) closed.add(s) for s_p in utils.succ(num_arr,s, closed): sp_c = [s_p.x, s_p.y] sp_cid = utils.coord_id(sp_c) ch_cost = s.g_h + utils.cost(num_arr,s_c, sp_c) if s_p not in fr.to_list() and s_p not in closed: g[sp_cid] = ch_cost parent[sp_cid] = s_c fr.insert(vertex.Vertex(sp_c, ch_cost)) elif s_p in fr.to_list() and ch_cost < s.g_h: fr.update(s_p, ch_cost) g[sp_cid] = ch_cost parent[sp_cid] = s_c return False, [], [], len(closed)
def solve(num_arr, start, end, w=[1, 1.25]): # open_i fr = [] # closed_i closed = [] #g_i g = [] #bp_i parent = [] # stuff to display g_h_f = [] goal = vertex.Vertex(end, 0) for i in range(n): g.append({}) g[i][utils.coord_id(start)] = 0 g[i][utils.coord_id(end)] = math.inf parent.append({}) g_h_f.append({}) parent[i][utils.coord_id(start)] = start parent[i][utils.coord_id(end)] = None fr.append(fringe.Fringe(False)) fr[i].insert(vertex.Vertex(start, key(start, i, g, end, w, g_h_f))) closed.append(set()) goal_cid = utils.coord_id(end) while fr[0].minkey().g_h < math.inf: for i in range(n): if fr[i].minkey().g_h <= w[1] * fr[i].minkey().g_h: if g[i][goal_cid] <= fr[i].minkey().g_h: if g[i][goal_cid] < math.inf: avg_expanded = [len(y) for y in closed] return True, utils.goal_path( start, end, parent[i]), g_h_f[i], np.mean(avg_expanded) else: s = fr[i].top() expand_state(s, i, fr, g, parent, num_arr, closed, end, w, g_h_f) closed[i].add(s) else: if g[0][goal_cid] <= fr[0].minkey().g_h: if g[0][goal_cid] < math.inf: avg_expanded = [len(y) for y in closed] return True, utils.goal_path( start, end, parent[i]), g_h_f[0], np.mean(avg_expanded) else: s = fr[i].top() expand_state(s, 0, fr, g, parent, num_arr, closed, end, w, g_h_f) closed[0].add(s) return False, [], []
def update_vertex(s, s_p, g, parent, fr, end, num_arr, w, g_h_f): s_c = [s.x, s.y] sp_c = [s_p.x, s_p.y] s_cid = utils.coord_id(s_c) sp_cid = utils.coord_id(sp_c) next_cost = utils.cost(num_arr, s_c, sp_c) if g[s_cid] + next_cost < g[sp_cid]: g[sp_cid] = g[s_cid] + next_cost parent[sp_cid] = s_c if s_p in fr.to_list(): fr.remove(sp_c) g_c = g[sp_cid] h_c = heur(sp_c, end) f_c = g_c + w * h_c g_h_f[utils.coord_id(sp_c)] = [g_c, h_c, f_c] fr.insert(vertex.Vertex(sp_c, f_c))
def expand_state(s, i, fr, g, parent, num_arr, closed, end, w, g_h_f): s_c = [s.x, s.y] s_cid = utils.coord_id(s_c) fr[i].remove(s_c) for s_p in utils.succ(num_arr, s, closed[i]): sp_c = [s_p.x, s_p.y] sp_cid = utils.coord_id(sp_c) tmp_cost = utils.cost(num_arr, s_c, sp_c) # may be wrong if s_p not in fr[i].to_list(): g[i][sp_cid] = math.inf parent[i][sp_cid] = None if g[i][sp_cid] > g[i][s_cid] + tmp_cost: g[i][sp_cid] = g[i][s_cid] + tmp_cost parent[i][sp_cid] = s_c if s_p not in closed[i]: fr[i].insert( vertex.Vertex(sp_c, key(sp_c, i, g, end, w, g_h_f)))
def succ(num_arr, point, closed): not_succ = [] succs = [] # check left not_succ.append(vertex.Vertex([point.x - 1, point.y], 0)) # check right not_succ.append(vertex.Vertex([point.x + 1, point.y], 0)) # check up not_succ.append(vertex.Vertex([point.x, point.y - 1], 0)) # check down not_succ.append(vertex.Vertex([point.x, point.y + 1], 0)) # check left up not_succ.append(vertex.Vertex([point.x - 1, point.y - 1], 0)) # check right up not_succ.append(vertex.Vertex([point.x + 1, point.y - 1], 0)) # check left down not_succ.append(vertex.Vertex([point.x - 1, point.y + 1], 0)) # check right down not_succ.append(vertex.Vertex([point.x + 1, point.y + 1], 0)) for ss in not_succ: if not (check_bounds(num_arr, [ss.x, ss.y]) or (ss in closed)): succs.append(ss) return succs
def remove(self, s): tmp_vert = vertex.Vertex(s, 0) self.fringe.remove(tmp_vert)