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
