def dijkstra_shortest_path(self, start_vetex):
"""
Find the shortest distance from a starting vertex to all other vertices.
Keyword arguments:
start_vertex -- a vertex object.
Time complexity:
O(VLogV + ELogV)
Space complexity:
O(V)
"""
unvisited_queue = PriorityQueue(self.size, lambda el: el.distance,
lambda el: el.data)
# Set all vertices to a distance of infinity and a previous vertex
# of None. Set the start_vertex's distance to 0.
for vertex in self.adjacency_list:
vertex.distance = float('inf')
vertex.previous_vertex = None
if vertex == start_vetex:
vertex.distance = 0
unvisited_queue.push(vertex)
# Time complexity: O(V)
while not unvisited_queue.is_empty():
# Time complexity: O(LogV)
current_vertex = unvisited_queue.pop()
# Go through each of the current_vertex's adjacent vertices checking
# to see if the path from the current vertex is shorter than the
# previously found paths.
# Time complexity: O(E)
for adjacent_vertex in self.adjacency_list[current_vertex]:
edge_weight = self.edge_weights[(current_vertex,
adjacent_vertex)]
new_distance = current_vertex.distance + edge_weight
# If a shorter path is found, update the adjacent vertex's distance
# to the new distance and set it's previous pointer to the
# current vertex.
if new_distance < adjacent_vertex.distance:
adjacent_vertex.distance = new_distance
adjacent_vertex.previous_vertex = current_vertex
# Time complexity: O(LogV)
unvisited_queue.update_priority(adjacent_vertex)
def astar(problem, heur):
"""
Implement A* search.
The given heuristic function will take in a state of the search problem
and produce a real number
Your implementation should be able to work with any heuristic, heur
that is for the given search problem (but, of course, without a
guarantee of optimality if heur is not admissible).
"""
start = problem.get_start_state()
pq = PriorityQueue(True)
visited = {}
cost = {}
cur = start
pq.push_with_priority(start, heur(start))
visited[start] = None
cost[start] = 0
while not pq.is_empty():
cur = pq.pop()
cur_cost = cost[cur]
if problem.is_goal_state(cur):
break
dict = problem.get_successors(cur)
for state in dict:
if state not in visited:
pq.push_with_priority(state,
cur_cost + dict[state] + heur(state))
visited[state] = cur
cost[state] = cur_cost + dict[state]
path = []
while cur is not None:
path.append(cur)
cur = visited[cur]
path.reverse()
print len(visited)
return path
def create_path(self, dest, gamemap):
if not gamemap.is_valid_pixel_pos(dest):
return []
if not self.can_traverse(dest, gamemap):
dest = self.__find_traversable_point(dest, gamemap)
def neighbors(point):
neighbors = []
for x in range(-1, 2):
for y in range(-1, 2):
if (x * gamemapping.squ_width, y * gamemapping.squ_width
) != (0, 0) and gamemap.is_valid_pixel_pos(
(point[0] + x * gamemapping.squ_width,
point[1] + y * gamemapping.squ_width)):
neighbors.append(
(point[0] + x * gamemapping.squ_width,
point[1] + y * gamemapping.squ_width))
return neighbors
def heuristic(point, dest):
return math.sqrt((point[0] - dest[0])**2 + (point[1] - dest[1])**2)
def collide_squ_width(a, b):
return a[0] >= b[0] and a[1] >= b[1] and a[
0] <= b[0] + gamemapping.squ_width and a[
1] <= b[1] + gamemapping.squ_width
came_from = {}
cost_so_far = {}
start_point = (int(self.location[0]), int(self.location[1]))
current_point = start_point
came_from[start_point] = None
cost_so_far[start_point] = 0
frontier = PriorityQueue()
frontier.put(start_point, 0)
while not frontier.is_empty():
# print("path finding...")
current_point = frontier.pop()
# print("current point = ", current_point, " dest = ", dest, " ", current_point == dest)
if collide_squ_width(current_point, dest):
break
for neighbor in neighbors(current_point):
# print("neighbor ", neighbor)
new_cost = cost_so_far[current_point] + self.get_traverse_cost(
neighbor, gamemap)
if (neighbor not in cost_so_far.keys() or new_cost <
cost_so_far[neighbor]) and self.can_traverse(
neighbor, gamemap):
cost_so_far[neighbor] = new_cost
priority = cost_so_far[neighbor] + heuristic(
neighbor, dest)
frontier.put(neighbor, priority)
came_from[neighbor] = current_point
# rel_point = (gamemap.rect.x + current_point[0], gamemap.rect.y + current_point[1])
# screen.set_at(rel_point, (0, 0, 0))
# pygame.display.flip()
if not collide_squ_width(current_point, dest):
return []
path = []
while current_point != start_point:
# print("path construction...")
path.insert(0, current_point)
current_point = came_from[current_point]
return path
