def example_func(graph, edges, edge_id, start, goal): """This function is just show some basic feature that you can use your project. @param graph: list - contain information of graph (same value as global_graph) list of object: [0] : (x,y) coordinate in UI [1] : adjacent node indexes [2] : node edge color [3] : node fill color Ex: graph = [ [ (139, 140), # position of node when draw on UI [1, 2], # list of adjacent node (100, 100, 100), # grey - node edged color (0, 0, 0) # black - node fill color ], [(312, 224), [0, 4, 2, 3], (100, 100, 100), (0, 0, 0)], ... ] It means this graph has Node 0 links to Node 1 and Node 2. Node 1 links to Node 0,2,3 and 4. @param edges: dict - dictionary of edge_id: [(n1,n2), color]. Ex: edges[edge_id(0,1)] = [(0,1), (0,0,0)] : set color of edge from Node 0 to Node 1 is black. @param edge_id: id of each edge between two nodes. Ex: edge_id(0, 1) : id edge of two Node 0 and Node 1 @param start: int - start vertices/node @param goal: int - vertices/node to search @return: """ # Ex1: Set all edge from Node 1 to Adjacency node of Node 1 is green edges. node_1 = graph[1] for adjacency_node in node_1[1]: edges[edge_id(1, adjacency_node)][1] = green graphUI.updateUI() #Ex2: Set color of Node 2 is Red graph[1][3] = red graphUI.updateUI()
def _markResult(graph, path, edges, edge_id, start, goal): # green graph[start][3] = orange graph[goal][3] = purple for i in range(len(path) - 1): edges[edge_id(path[i], path[i + 1])][1] = green graphUI.updateUI()
def TrackTrack(graph, edges, edge_id, start, goal, track): temp = goal graph[start][3] = orange graph[goal][3] = purple while(temp != start): edges[edge_id(temp, track[temp])][1] = green temp = track[temp] graphUI.updateUI()
def AStar(graph, edges, edge_id, start, goal): """ A star search """ # TODO: your code print("Implement A* algorithm.") parent = {} parent[start] = -1 #-----#-----#-----#-----#-----#----- weightedMatrix = ComputeweightedMatrix(graph) heuF = heuristicsFunc(graph, goal) frontier = {} frontier[start] = heuF[start] # 0: Euclidean Distance explored = set() while True: if len(frontier) == 0: print("Can't find the goal!!!") return -1 node = min(frontier, key=frontier.get) node_distance = frontier.pop(node) if node == goal: return print( "The length of the path:", Solution(graph, edges, edge_id, goal, parent, weightedMatrix)) graph[node][3] = yellow graph[node][2] = white graphUI.updateUI() explored.add(node) index = 0 for successor in graph[node][1]: distance = node_distance + weightedMatrix[node][successor] - heuF[ node] + heuF[successor] if (successor not in explored) and (successor not in frontier): parent[successor] = node edges[edge_id(node, successor)][1] = white graph[successor][2] = white graph[successor][3] = red graphUI.updateUI() parent[successor] = node frontier[successor] = distance elif successor in frontier and distance < frontier[successor]: frontier[successor] = distance parent[successor] = node edges[edge_id(node, successor)][1] = white index += 1 graph[node][3] = blue print("Implement A* algorithm.") pass
def BFS(graph, edges, edge_id, start, goal): """ BFS search """ # TODO: your code # Start and Goal are at the same place. print("Implement BFS algorithm.") parent = {} parent[start] = -1 n = len(graph) weightedMatrix = np.ones((n, n)) if start == goal: return print( "The length of the path:", Solution(graph, edges, edge_id, goal, parent, weightedMatrix)) #-----#-----#-----#-----#-----#----- frontier = [] frontier.append(start) explored = set() while True: if len(frontier) == 0: print("Can't find the goal!!!") return -1 node = frontier.pop(0) graph[node][3] = yellow graph[node][2] = white graphUI.updateUI() explored.add(node) for successor in graph[node][1]: if (successor not in explored) and (successor not in frontier): parent[successor] = node edges[edge_id(node, successor)][1] = white graph[successor][2] = white graph[successor][3] = red graphUI.updateUI() if successor == goal: return print( "The length of the path:", Solution(graph, edges, edge_id, goal, parent, weightedMatrix)) else: frontier.append(successor) graph[node][3] = blue print("Implement BFS algorithm.") pass
def drawPath0(visited_list, graph, edge_id, edges): print(visited_list) length = len(visited_list) next_index = 1 for x in range(len(visited_list) - 1): adjecency_node = graph[visited_list[x]][1] while next_index < length and visited_list[ next_index] in adjecency_node: edges[edge_id(visited_list[x], visited_list[next_index])][1] = green graphUI.updateUI() next_index += 1
def colorPath(mark_path, path, edges, edge_id, start, goal): for i in range(len(mark_path)): for j in range(len(mark_path)): if j == goal: path.append(goal) goal = mark_path[j] break if goal == start: path.append(start) break for i in range(len(path) - 1): edges[edge_id(path[i], path[i + 1])][1] = green graphUI.updateUI()
def AStar(graph, edges, edge_id, start, goal): """ A star search """ # TODO: your code weight = [] wei1 = 0 initWeighAF(graph, weight, goal) k = 0 mark_path = [] initVertexCrossed(graph, mark_path) queue = [] queueNode = [] a = (start, 0) queue.append(a) while (len(queue) != 0): vertex = queue[0] queue.remove(queue[0]) queueNode.append(vertex[0]) graph[vertex[0]][3] = yellow node_1 = graph[vertex[0]] for adjacency_node in node_1[1]: if adjacency_node not in queueNode: a = int(getWeight( graph, vertex[0], adjacency_node)) #Chi phí từ đỉnh gốc tới đỉnh kề đang xét b = int(weight[adjacency_node] ) # Heuristic chi phí từ đỉnh kề đang xét tới đích # Bộ đỉnh, chi phí đường đi giữa 2 đỉnh và chi phí từ đỉnh đó tới đích queue.append((adjacency_node, a + b + vertex[1])) graph[adjacency_node][3] = red mark_path[adjacency_node] = vertex[0] edges[edge_id(vertex[0], adjacency_node)][1] = white sortTuple(queue) queueNode.append(adjacency_node) graphUI.updateUI() time.sleep(.1) if goal == adjacency_node: k = 1 break graphUI.updateUI() time.sleep(1) graph[vertex[0]][3] = blue if k == 1: break fillNode(graph, vertex[0], start, goal) path = [] colorPath(mark_path, path, edges, edge_id, start, goal) print("Implement A* algorithm.") pass
def dfs_path(graph, edges, edge_id, current, goal, visited): #hàm hỗ trợ để tìm đường đi if current == goal: # nếu đỉnh đang xét là đích graph[goal][3] = purple # tô màu graphUI.updateUI() time.sleep(0.5) return [current] else: graph[current][3] = yellow # tô màu đỉnh hiện tại graphUI.updateUI() time.sleep(0.5) for neighbor in graph[current][1]: graph[neighbor][3] = blue edges[edge_id(current, neighbor)][1] = white graphUI.updateUI() time.sleep(0.1) if neighbor not in visited: # nếu đỉnh kề chưa được xét graph[neighbor][3] = red # tô màu graphUI.updateUI() time.sleep(0.5) visited.add(neighbor) # thêm vào tập đã duyệt path = dfs_path(graph, edges, edge_id, neighbor, goal, visited) if path is not None: # nếu tập đường đi khác null path.insert(0, current) # path[0] = current return path # trả về đường đi
def DFS(graph, edges, edge_id, start, goal): """ DFS search """ # TODO: your code graph[start][2] = white graph[start][3] = orange graph[goal][3] = purple graphUI.updateUI() old_node = {} old_node.update({start: -1}) def dfsList(current_node): for near_node in graph[current_node][1]: if near_node not in old_node: graph[near_node][3] = red graph[near_node][2] = white edges[edge_id(current_node, near_node)][1] = white graphUI.updateUI() graph[near_node][3] = yellow graphUI.updateUI() old_node.update({near_node: current_node}) if near_node == goal: return True elif dfsList(near_node): return True graph[current_node][3] = blue graphUI.updateUI() return False if dfsList(start): temp_node = goal dfs_path = [] while temp_node != start: dfs_path.append(int(temp_node)) temp_node = old_node[temp_node] dfs_path.append(start) for i in range(0, len(dfs_path) - 1): edges[edge_id(dfs_path[i], dfs_path[i + 1])][1] = green graph[start][3] = orange graph[goal][3] = purple graphUI.updateUI() print("Implement DFS algorithm.") pass
def UCS(graph, edges, edge_id, start, goal): """ Uniform Cost Search search """ # TODO: your code k = 0 mark_path = [] initVertexCrossed(graph, mark_path) queue = [] queueNode = [] a = (start, 0) queue.append(a) while (len(queue) != 0): #Kiểm tra stack rỗng thì ngưng vertex = queue[0] queue.remove(queue[0]) queueNode.append(vertex[0]) graph[vertex[0]][3] = yellow node_1 = graph[vertex[0]] for adjacency_node in node_1[1]: if adjacency_node not in queueNode: a = int(getWeight( graph, vertex[0], adjacency_node)) #Chi phí từ đỉnh gốc tới đỉnh kề đang xét queue.append( (adjacency_node, a + vertex[1])) #Bộ đỉnh, chi phí đường đi giữa 2 đỉnh graph[adjacency_node][3] = red mark_path[adjacency_node] = vertex[0] edges[edge_id(vertex[0], adjacency_node)][1] = white sortTuple(queue) queueNode.append(adjacency_node) time.sleep(.1) graphUI.updateUI() if goal == adjacency_node: k = 1 break graphUI.updateUI() time.sleep(1) graph[vertex[0]][3] = blue if k == 1: break fillNode(graph, vertex[0], start, goal) path = [] colorPath(mark_path, path, edges, edge_id, start, goal) print("Implement Uniform Cost Search algorithm.") pass
def BFS(graph, edges, edge_id, start, goal): """ BFS search """ # TODO: your code graph[start][2] = white graph[start][3] = orange graph[goal][3] = purple graphUI.updateUI() bfsPath = [[start]] #tìm goal while graph[goal][2] != white: path = bfsPath.pop(0) # duyệt temp = path[-1] if graph[temp][3] != blue: graph[temp][3] = yellow graph[temp][2] = white graphUI.updateUI() tmp_node = list(graph[temp][1]) for near_node in graph[temp][1]: if graph[near_node][3] != black and near_node != goal: tmp_node.remove(near_node) for near_node in tmp_node: graph[near_node][3] = red graph[near_node][2] = white edges[edge_id(temp, near_node)][1] = white graphUI.updateUI() new_path = list(path) new_path.append(near_node) bfsPath.append(new_path) if near_node == goal: finalPath = new_path break graph[temp][3] = blue graphUI.updateUI() # ket qua for i in range(0, len(finalPath) - 1): edges[edge_id(finalPath[i], finalPath[i + 1])][1] = green graph[start][3] = orange graph[goal][3] = purple graphUI.updateUI() print("Implement BFS algorithm.") pass
def GreedySearch(graph, edges, edge_id, start, goal): # TODO: your code print("Implement Greedy Search algorithm.") parent = {} parent[start] = -1 #-----#-----#-----#-----#-----#----- heuF = heuristicsFunc(graph, goal) weightedMatrix = ComputeweightedMatrix(graph) explored = set() node = start while True: if node == goal: return print( "The length of the path:", Solution(graph, edges, edge_id, goal, parent, weightedMatrix)) graph[node][3] = yellow graph[node][2] = white graphUI.updateUI() explored.add(node) minHeuDistance = math.inf nodeChosen = -1 for successor in graph[node][1]: if (successor not in explored) and (heuF[successor] < minHeuDistance): nodeChosen = successor minHeuDistance = heuF[nodeChosen] if nodeChosen != -1: parent[nodeChosen] = node edges[edge_id(node, nodeChosen)][1] = white graph[nodeChosen][2] = white graph[nodeChosen][3] = red graphUI.updateUI() else: print("Can't find the goal!!!") return -1 graph[node][3] = blue node = nodeChosen pass
def BFS(graph, edges, edge_id, start, goal): """ BFS search """ # TODO: your code k = 0 mark_path = [] #mảng lấy vết đường đi initVertexCrossed(graph, mark_path) # hàm khởi tạo mả_path = -1 queue = [] # stack2: lưu đỉnh chờ duyệt queueNode = [] #stack1: lưu những đỉnh đã duyệt qua queue.append(start) while (len(queue) != 0): vertex = queue[0] queue.remove(queue[0]) # Xóa đỉnh đầu sau mỗi lần duyệt queueNode.append(vertex) graph[vertex][3] = yellow node_1 = graph[vertex] for adjacency_node in node_1[1]: if adjacency_node not in queueNode: #queueNode kiểm tra đỉnh đã có trong danh sách chưa? queue.append( adjacency_node) #Thêm vào nếu chưa có trong danh sách graph[adjacency_node][3] = red # Tô màu đỉnh duyệt đến mark_path[ adjacency_node] = vertex #Đánh dấu dường đi của các đỉnh edges[edge_id( vertex, adjacency_node)][1] = white #Tô màu cạnh đã duyệt qua queueNode.append(adjacency_node) graphUI.updateUI() time.sleep(.1) if goal in queue: k = 1 break graphUI.updateUI() time.sleep(1) graph[vertex][3] = blue if k == 1: break fillNode(graph, vertex, start, goal) # Hàm tô màu đỉnh sau khi kết thúc duyệt path = [] colorPath(mark_path, path, edges, edge_id, start, goal) #Tô mày đường đi print("Implement BFS algorithm.") pass
def DFS(graph, edges, edge_id, start, goal): stack = [(start, [start])] visit = [start] pathGraph = [] flagEnd = 0 while stack: #print(stack) (v, path) = stack.pop() #set node graphic graph[v][3] = yellow graphUI.updateUI() for w in graph[v][1]: if w not in visit: visit.append(v) edges[edge_id(v, w)] = [(v, w), white] graph[w][3] = red graphUI.updateUI() time.sleep(1) if w == goal: flagEnd = 1 pathGraph = path + [w] break stack.append((w, path + [w])) if (flagEnd == 1): break graph[v][3] = blue graphUI.updateUI() time.sleep(1) graph[start][3] = orange for i in range(0, len(pathGraph) - 1): edges[edge_id(pathGraph[i], pathGraph[i+1])] = [(pathGraph[i], pathGraph[i + 1]), green] #end graph[goal][3] = purple graphUI.updateUI() print("Implement DFS algorithm.") pass
def DFS(graph, edges, edge_id, start, goal): """ DFS search """ # TODO: your code print("Implement DFS algorithm.") if start not in range(0, len(graph)) or goal not in range(0, len(graph)): print("Error: Input invaild range.") return for i in range(len(graph)): graph[i][3] = black graphUI.updateUI() # keep track of explored nodes explored = set() explored.add(start) if start == goal: print("Algorithm finished - your path is: {} -> {}".format( start, goal)) # Set color yellow for the goal node graph[goal][3] = purple graphUI.updateUI() count = 0 path = find_path_dfs(graph, edges, edge_id, start, goal, explored, count) if not path: print("There is no way to reach the goal.") print(path) graph[start][3] = orange for i in range(len(path) - 1): edges[edge_id(path[i], path[i + 1])][1] = green graph[goal][3] = purple graphUI.updateUI()
def DFS(graph, edges, edge_id, start, goal): """ DFS search """ # TODO: your code pos = -1 k = 0 mark_path = [] initVertexCrossed(graph, mark_path) stack = [] queueNode = [] stack.append(start) while (stack): vertex = stack.pop( ) #Lấy đỉnh cuối ra để duyệt đồng thời xóa khỏi stack queueNode.append(vertex) graph[vertex][3] = yellow node_1 = graph[vertex] time.sleep(.3) for adjacency_node in node_1[1]: if adjacency_node not in queueNode: stack.append(adjacency_node) graph[adjacency_node][3] = red mark_path[adjacency_node] = vertex edges[edge_id(vertex, adjacency_node)][1] = white queueNode.append(adjacency_node) graphUI.updateUI() if goal in stack: k = 1 break time.sleep(1) graph[vertex][3] = blue if k == 1: break fillNode(graph, vertex, start, goal) path = [] colorPath(mark_path, path, edges, edge_id, start, goal) print("Implement DFS algorithm.") pass
def UCS(graph, edges, edge_id, start, goal): """ Uniform Cost Search search """ # TODO: your code print("Implement Uniform Cost Search algorithm.") discovered = set() frontier = PriorityQueue() previous = set() frontier.put((-1, 0, start)) while frontier: prev, weight, curr = frontier.get() if curr not in discovered: graph[curr][3] = yellow graphUI.updateUI() time.sleep(0.5) previous.add((prev, curr)) discovered.add(curr) if curr == goal: path = [curr] while 1: for i in previous: if i[1] == curr: prev = i[0] path.append(prev) curr = prev if curr == start: break path.reverse() print('Path found: {}'.format(path)) graph[start][3] = orange for i in range(len(path) - 1): edges[edge_id(path[i], path[i + 1])][1] = green graph[goal][3] = purple graphUI.updateUI() time.sleep(0.1) return for neighbor in graph[curr][1]: totalW = weight + getWeight(graph[curr][0], graph[neighbor][0]) if neighbor not in frontier.queue: graph[neighbor][3] = red edges[edge_id(curr, neighbor)][1] = white graphUI.updateUI() time.sleep(0.1) frontier.put((curr, totalW, neighbor)) graph[curr][3] = blue graphUI.updateUI() time.sleep(0.5) pass
def DFS(graph, edges, edge_id, start, goal): """ DFS search """ # TODO: your code print("Implement DFS algorithm.") discovered = set() discovered.add(start) if start == goal: graph[goal][3] = purple graphUI.updateUI() time.sleep(0.5) else: path = dfs_path(graph, edges, edge_id, start, goal, discovered) graph[start][3] = orange graphUI.updateUI() time.sleep(0.5) for i in range(len(path) - 1): edges[edge_id(path[i], path[i + 1])][1] = green graph[goal][3] = purple graphUI.updateUI() time.sleep(0.1) print('Path found: {}'.format(path)) pass
def BeamSearch(graph, edges, edge_id, start, goal): k = 2 #k = int(input("Input k beam states: ")) weightedMatrix = ComputeweightedMatrix(graph) parent = {} parent[start] = -1 if start == goal: return print( "The length of the path:", Solution(graph, edges, edge_id, goal, parent, weightedMatrix)) frontier = [] frontier.append(start) explored = set() heuF = heuristicsFunc(graph, goal) print(heuF) while True: n = len(frontier) if n == 0: print("Can't find the goal!!!") return -1 for i in range(n): node = frontier.pop(0) graph[node][3] = yellow graph[node][2] = white graphUI.updateUI() for successor in graph[node][1]: if (successor not in explored) and (successor not in frontier): parent[successor] = node graph[successor][3] = red graph[successor][2] = white edges[edge_id(node, successor)][1] = white graphUI.updateUI() if successor == goal: return print( "The length of the path:", Solution(graph, edges, edge_id, goal, parent, weightedMatrix)) frontier.append(successor) explored.add(node) graph[node][3] = blue n = len(frontier) heuF_2 = [] for i in range(n): heuF_2.append(heuF[frontier[i]]) frontier = [x for _, x in sorted(zip(heuF_2, frontier))] while len(frontier) > k: node = frontier.pop() graph[node][3] = blue graphUI.updateUI() explored.add(node)
def UCS(graph, edges, edge_id, start, goal): """ Uniform Cost Search search """ # TODO: your code queueList = PriorityQueue() queueList.put((0, start, [start])) tmpList = [] while queueList: cost, v, path = queueList.get() if v in tmpList: continue if v == goal: ucsPath = path break graph[v][3] = yellow graphUI.updateUI() tmpList.append(v) for w in graph[v][1]: if w not in tmpList: queueList.put( (cost + distance(graph[v], graph[w]), w, path + [w])) edges[edge_id(v, w)] = [(v, w), white] graph[w][3] = red graphUI.updateUI() time.sleep(1) graph[v][3] = blue graphUI.updateUI() time.sleep(1) graph[start][3] = orange for i in range(0, len(ucsPath) - 1): edges[edge_id(ucsPath[i], ucsPath[i + 1])] = [(ucsPath[i], ucsPath[i + 1]), green] graph[goal][3] = purple graphUI.updateUI() print("Implement Uniform Cost Search algorithm.") pass
def Solution(graph, edges, edge_id, goal, parent, weightedMatrix): child = goal graph[child][2] = white graph[child][3] = purple graphUI.updateUI() PathCost = 0 while parent[child] != -1: PathCost = PathCost + weightedMatrix[parent[child]][child] edges[edge_id(parent[child], child)][1] = green child = parent[child] graphUI.updateUI() if child != goal: graph[child][3] = orange graphUI.updateUI() return PathCost
def dfsList(current_node): for near_node in graph[current_node][1]: if near_node not in old_node: graph[near_node][3] = red graph[near_node][2] = white edges[edge_id(current_node, near_node)][1] = white graphUI.updateUI() graph[near_node][3] = yellow graphUI.updateUI() old_node.update({near_node: current_node}) if near_node == goal: return True elif dfsList(near_node): return True graph[current_node][3] = blue graphUI.updateUI() return False
def drawPath2(visited_list, graph, edge_id, edges): for x in range(0, len(visited_list) - 1): edges[edge_id(visited_list[x], visited_list[x + 1])][1] = green graphUI.updateUI() x += 2 pass
def drawPath1(visited_list, graph, edge_id, edges): for x in range(1, len(visited_list)): edges[edge_id(visited_list[x][0], visited_list[x][1])][1] = green graphUI.updateUI() pass
def DFS(graph, edges, edge_id, start, goal): """ DFS search """ # TODO: your code global pathed pathed.append(start) global start_vertex1 start_vertex.append(start) # Set color of node start global visited visited[start] = 1 graph[start][3] = yellow graphUI.updateUI() pygame.time.delay(500) # get adjacency list adjacency = custom_func.Adjacency(graph) # if node has no adjacent vertex, then trace back to previous node for y in adjacency[start]: if visited[y] == 0: visited[y] = 1 graph[y][3] = red graphUI.updateUI() edges[edge_id(start, y)][1] = white graphUI.updateUI() if y == goal: graph[y][3] = purple graph[start_vertex[0]][3] = orange graph[start][3] = blue graphUI.updateUI() pathed.append(goal) for x in range(len(pathed) - 1): edges[edge_id(pathed[x], pathed[x + 1])][1] = green graphUI.updateUI() return False graph[start][3] = blue graphUI.updateUI() return DFS(graph,edges,edge_id,y,goal) #continue # if present vertex is a hang vertex or is a leaf, then we trace back to previous root node graph[start][3] = blue graphUI.updateUI() if visited[start] == 0: visited[start] = 1 global present_index present_index = pathed.index(start) return DFS(graph,edges,edge_id,pathed[present_index - 1],goal) print("Implement DFS algorithm.") pass
def AStar(graph, edges, edge_id, start, goal): """ A star search """ # TODO: your code visited = [0 for i in repeat(None,len(graph))] # list of costed vertex closed = [] # list of reached vertex cost = [] # list contain cost to reach vertex (as a open vertex list) # get adjacency list adjacency = custom_func.Adjacency(graph) # Set color of node start graph[start][3] = orange graphUI.updateUI() # begin with start node visited[start] = 1 cost.append((start,0,0)) # cost[(this_vertex,cost1,f(1)),(this_vertex2,cost2,f(2)),...] #---- f(x) = g(x) + h(x) whith h(x) is a heuristic function get distance from x to goal # and g(x) is length of path that we traced breaker = False while all(cost) != False: visit = cost[0][0] closed.append(visit) # set present graph[visit][3] = yellow graphUI.updateUI() pygame.time.delay(500) for y in adjacency[visit]: # check if route exists and that node is not visited if visited[y] == 0: visited[y] = 1; # set color graph[y][3] = red graphUI.updateUI() # set all edge between node in a array. edges[edge_id(visit, y)][1] = white graphUI.updateUI() if y == goal: graph[y][3] = purple graph[visit][3] = blue graph[start][3] = orange graphUI.updateUI() closed.append(goal) breaker = True break # calculate cost to reach # f(x) = g(x) + h(x) from_node = graph[visit][0] to_node = graph[y][0] present_cost = custom_func.Distance(from_node,to_node) # g(x) h_x = custom_func.Distance(graph[y][0],graph[goal][0]) # h(x) g_x = h_x + present_cost present_cost += cost[0][1] cost.append((y,present_cost,g_x)) if breaker == True: break graph[visit][3] = blue graphUI.updateUI() cost.pop(0) cost.sort(key = custom_func.takeThird) # sort by heuristic function custom_func.drawPath2(closed,graph,edge_id,edges) # draw found-path print("Implement A* algorithm.") pass
def UCS(graph, edges, edge_id, start, goal): """ Uniform Cost Search search """ # TODO: your code # initialize visited = [0 for i in repeat(None,len(graph))] # list of costed vertex closed = [] # list of reached vertex cost = [] # list contain cost to reach vertex (as a open vertex list) # get adjacency list adjacency = custom_func.Adjacency(graph) # Set color of node start graph[start][3] = orange graphUI.updateUI() # begin with start node visited[start] = 1 cost.append((start,0,start)) # cost[(this_vertex,cost1,previous_vertex1),(this_vertex2,cost2,previous_vertex2),...] breaker = False while all(cost) != False: visit = cost[0][0] previous = cost[0][2] closed.append((visit,previous)) # set present graph[visit][3] = yellow graphUI.updateUI() pygame.time.delay(500) for y in adjacency[visit]: # check if route exists and that node is not visited if visited[y] == 0: visited[y] = 1 # set color graph[y][3] = red graphUI.updateUI() # set all edge between node in a array. edges[edge_id(visit, y)][1] = white graphUI.updateUI() if y == goal: graph[y][3] = purple graph[visit][3] = blue graph[start][3] = orange graphUI.updateUI() closed.append((goal,visit)) breaker = True break # calculate cost to reach from_node = graph[visit][0] to_node = graph[y][0] present_cost = custom_func.Distance(from_node,to_node) present_cost += cost[0][1] cost.append((y,present_cost,visit)) if breaker == True: break graph[visit][3] = blue graphUI.updateUI() cost.pop(0) cost.sort(key = custom_func.takeSecond) # sort by cost of each vertex custom_func.drawPath1(closed,graph,edge_id,edges) # draw found-path print("Implement Uniform Cost Search algorithm.") pass
def BFS(graph, edges, edge_id, start, goal): """ BFS search """ # TODO: your code # Initialize visited = [0 for i in repeat(None,len(graph))] #visited = [] #for x in range(len(graph)): #visited.append(0) # add the start node to the queue -> 0 is start node q = Queue(maxsize = 11) q.put(start) pathed = [] # save path we traced # set the visited value of node 0 to visited visited[start] = 1 # get adjacency list adjacency = custom_func.Adjacency(graph) # Set color of node start graph[start][3] = orange graphUI.updateUI() breaker = False while q.empty() == False: visit = q.get(); pathed.append(visit) # set color of present vertex graph[visit][3] = yellow graphUI.updateUI() pygame.time.delay(500) # if present vertex is a hang vertex or is a leaf, then we trace back to previous root node # find adjacent vertex of the present vertex for y in adjacency[visit]: # check if route exists and that node is not visited if visited[y] == 0: visited[y] = 1; # mark as visited # set color graph[y][3] = red; graphUI.updateUI(); pygame.time.delay(500) edges[edge_id(visit, y)][1] = white graphUI.updateUI() if y == goal: graph[y][3] = purple graph[visit][3] = blue graph[start][3] = orange graphUI.updateUI() pathed.append(y) breaker = True break # put adjacent vertex to queue q.put(y); if breaker == True: break graph[visit][3] = blue graphUI.updateUI() #print(pathed,graph,edge_id,edges) custom_func.drawPath0(pathed,graph,edge_id,edges) print("Implement BFS algorithm.") # set shortest path color pass
def AStar(graph, edges, edge_id, start, goal): """ A star search """ # TODO: your code open = set([start]) # tập mở closed = set() # tập đóng g = {} # g chứa khoảng cách tới đỉnh bắt đầu parents = {} # tập đỉnh kề của các đỉnh g[start] = 0 # khoảng cách từ đỉnh bắt đầu tới chính nó bằng 0 # start là đỉnh bắt đầu nên không có parent nên start là parent của chính nó parents[start] = start while len(open) > 0: pygame.event.get() n = None # f(n) = g(n) + h(n) # đỉnh có f(n) nhỏ nhất được xét for v in open: if n == None or g[v] + manhattan_distance( graph[v][0][0], graph[v][0][1], graph[goal][0][0], graph[goal][0][1]) < g[n] + manhattan_distance( graph[n][0][0], graph[n][0][1], graph[goal][0][0], graph[goal][0][1]): n = v if n == goal or graph[n][1] == None: pass else: lst = list() graph[n][3] = yellow graphUI.updateUI() time.sleep(0.5) for neighbor in graph[n][1]: graph[neighbor][3] = red edges[edge_id(n, neighbor)][1] = white graphUI.updateUI() time.sleep(0.1) lst.append( (neighbor, euclidean_distance(graph[neighbor][0][0], graph[neighbor][0][1], graph[n][0][0], graph[n][0][1]))) for (m, weight) in lst: # đỉnh m không nằm trong tập mở và tập đóng sẽ được thêm vào tập đóng if m not in open and m not in closed: graph[m][3] = blue graphUI.updateUI() time.sleep(0.5) open.add(m) parents[m] = n g[m] = g[n] + weight # với mỗi đỉnh m,so sánh khoảng cách của đỉnh m so với n else: if g[m] > g[n] + weight: # update g(m) g[m] = g[n] + weight # thay parent của m thành n parents[m] = n # nếu m nằm trong tập đóng, lấy ra khỏi tập đóng rồi thêm vào tập mở if m in closed: closed.remove(m) open.add(m) if n == None: print('Path does not exist!') return None # nếu đỉnh đang duyệt là đỉnh đích # thi xây dựng đường đi từ đỉnh bắt đầu tới nó if n == goal: graph[start][3] = orange graph[goal][3] = purple graphUI.updateUI() time.sleep(0.5) path = [] while parents[n] != n: path.append(n) n = parents[n] path.append(start) path.reverse() print('Path found: {}'.format(path)) for i in range(len(path) - 1): edges[edge_id(path[i], path[i + 1])][1] = green graphUI.updateUI() time.sleep(0.1) return path # lấy n ra khỏi tập m rồi thêm vào tập đóng # vì tất cả đỉnh kề đã được duyệt graph[n][3] = yellow graphUI.updateUI() time.sleep(0.5) open.remove(n) closed.add(n) print('Path does not exist!') return None print("Implement AStar algorithm.") pass