def build_distance_table(graph, source):
distance_table = {}
for v in range(graph.numVertices):
distance_table[v] = (None, None)
distance_table[source] = (0, source)
# Node with the lowest value has the highest priority
_priority_queue = priority_queue.priority_dict()
_priority_queue[source] = 0
while len(_priority_queue.keys()) > 0:
cur_vertex = _priority_queue.pop_smallest()
cur_distance = distance_table[cur_vertex][0]
for neighbour in graph.get_adjacent_vertices(cur_vertex):
distance = cur_distance + graph.get_edge_weight(
cur_vertex, neighbour)
neighbour_distance = distance_table[neighbour][0]
if neighbour_distance is None or neighbour_distance > distance:
distance_table[neighbour] = (distance, cur_vertex)
_priority_queue[neighbour] = distance
return distance_table
def plan(self, start_state, dest_state):
"""
Returns the shortest path as a sequence of states [start_state, ..., dest_state]
if dest_state is reachable from start_state. Otherwise returns [start_state].
Assume both source and destination are in free space.
"""
assert (self.state_is_free(start_state))
assert (self.state_is_free(dest_state))
# Q is a mutable priority queue implemented as a dictionary
Q = priority_dict()
Q[start_state] = 0.0
# Array that contains the optimal distance we've found from the starting state so far
best_dist_found = float("inf") * np.ones(
(world.shape[1], world.shape[0]))
best_dist_found[start_state.x, start_state.y] = 0
# Boolean array that is true iff the distance to come of a state has been
# finalized
visited = np.zeros((world.shape[1], world.shape[0]), dtype='uint8')
# Contains key-value pairs of states where key is the parent of the value
# in the computation of the shortest path
parents = {start_state: None}
while Q:
# s is also removed from the priority Q with this
s = Q.pop_smallest()
# Assert s hasn't been visited before
assert (visited[s.x, s.y] == 0)
# Mark it visited because here we will go over every neighbor,
# so there's no need to come back after this (by greedy property)
visited[s.x, s.y] = 1
if s == dest_state:
return self._follow_parent_pointers(parents, s)
# for all free neighboring states
for ns in self.get_neighboring_states(s):
if visited[ns.x, ns.y] == 1:
continue
transition_distance = sqrt((ns.x - s.x)**2 + (ns.y - s.y)**2)
alternative_best_dist_ns = best_dist_found[
s.x, s.y] + transition_distance
# if the state ns has not been visited before or we just found a shorter path
# to visit it then update its priority in the queue, and also its
# distance to come and its parent
if (ns not in Q) or (alternative_best_dist_ns <
best_dist_found[ns.x, ns.y]):
Q[ns] = alternative_best_dist_ns
best_dist_found[ns.x, ns.y] = alternative_best_dist_ns
parents[ns] = s
return [start_state]
def spanning_tree(graph):
# Holds a mapping from a pair of edges to the edge weight
# The edge weight is the priority of the edge
_priority_queue = priority_queue.priority_dict()
for v in range(graph.numVertices):
for neighbor in graph.get_adjacent_vertices(v):
_priority_queue[(v, neighbor)] = graph.get_edge_weight(v, neighbor)
visited_vertices = set()
# Maps a node to all its adjacent nodes which are in the
# minimum spanning tree
spanning_tree = {}
for v in range(graph.numVertices):
spanning_tree[v] = set()
# Number of edges we have got so far
num_edges = 0
while len(
_priority_queue.keys()) > 0 and num_edges < graph.numVertices - 1:
v1, v2 = _priority_queue.pop_smallest()
if v1 in spanning_tree[v2]:
continue
# Arrange the spanning tree so the node with the smaller
# vertex id is always first. This greatly simplifies the
# code to find cycles in this tree
vertex_pair = sorted([v1, v2])
spanning_tree[vertex_pair[0]].add(vertex_pair[1])
# Check if adding the current edge causes a cycle
if has_cycle(spanning_tree):
spanning_tree[vertex_pair[0]].remove(vertex_pair[1])
continue
num_edges = num_edges + 1
visited_vertices.add(v1)
visited_vertices.add(v2)
print("Visited vertices: ", visited_vertices)
if len(visited_vertices) != graph.numVertices:
print("Minimum spanning tree not found")
else:
print("Minimum spanning tree:")
for key in spanning_tree:
for value in spanning_tree[key]:
print(key, "-->", value)
def spanning_tree(graph, source):
distance_table = {}
for v in range(graph.numVertices):
distance_table[v] = (None, None)
distance_table[source] = (0, source)
_priority_queue = priority_queue.priority_dict()
_priority_queue[source] = 0
visited_vertices = set()
spanning_tree = set()
while len(_priority_queue.keys()):
vertex = _priority_queue.pop_smallest()
if vertex in visited_vertices:
continue
visited_vertices.add(vertex)
if vertex != source:
last_vertex = distance_table[vertex][1]
spanning_tree.add('%s -> %s' % (last_vertex, vertex))
for neighbour in graph.get_adjacent_vertices(vertex):
# do not sum up the previous distances.
distance = graph.get_edge_weight(vertex, neighbour)
neighbour_distance = distance_table[neighbour][0]
if neighbour_distance is None or neighbour_distance > distance:
distance_table[neighbour] = (distance, vertex)
_priority_queue[neighbour] = distance
for edge in spanning_tree:
print(edge)
def plan(self, start_state, dest_state):
"""
Returns the shortest path as a sequence of states [start_state, ..., dest_state]
if dest_state is reachable from start_state. Otherwise returns [start_state].
Assume both source and destination are in free space.
"""
assert (self.state_is_free(start_state))
assert (self.state_is_free(dest_state))
# Q is a mutable priority queue implemented as a dictionary
Q = priority_dict()
# Using euclidean distance as the heuristic function for this A* implementation
Q[start_state] = self.euclidean_dist(start_state, dest_state)
# Array that contains the optimal distance to come from the starting state
dist_to_come = float("inf") * np.ones((world.shape[0], world.shape[1]))
dist_to_come[start_state.x, start_state.y] = 0
# Boolean array that is true iff the distance to come of a state has been
# finalized
evaluated = np.zeros((world.shape[0], world.shape[1]), dtype='uint8')
# Contains key-value pairs of states where key is the parent of the value
# in the computation of the shortest path
parents = {start_state: None}
while Q:
# s is also removed from the priority Q with this
s = Q.pop_smallest()
# Assert s hasn't been evaluated before
assert (evaluated[s.x, s.y] == 0)
evaluated[s.x, s.y] = 1
if s == dest_state:
# Returning evaluated as a list of states
visited_locs = []
m, n = evaluated.shape
for i in range(m):
for j in range(n):
if evaluated[i, j] == 1:
visited_locs.append(State(i, j))
return self._follow_parent_pointers(parents, s), visited_locs
# for all free neighboring states
for ns in self.get_neighboring_states(s):
if evaluated[ns.x, ns.y] == 1:
continue
transition_distance = sqrt((ns.x - s.x)**2 + (ns.y - s.y)**2)
alternative_dist_to_come_to_ns = dist_to_come[
s.x, s.y] + transition_distance
# if the state ns has not been visited before or we just found a shorter path
# to visit it then update its priority in the queue, and also its
# distance to come and its parent
if (ns not in Q) or (alternative_dist_to_come_to_ns <
dist_to_come[ns.x, ns.y]):
Q[ns] = alternative_dist_to_come_to_ns + self.euclidean_dist(
ns, dest_state)
dist_to_come[ns.x, ns.y] = alternative_dist_to_come_to_ns
parents[ns] = s
return [start_state]
def plan(self, start_state, dest_state):
"""
Returns the shortest path as a sequence of states [start_state, ..., dest_state]
if dest_state is reachable from start_state. Otherwise returns [start_state].
Assume both source and destination are in free space.
"""
assert (self.state_is_free(start_state))
assert (self.state_is_free(dest_state))
# Q is a mutable priority queue implemented as a dictionary
Q = priority_dict()
#Q[start_state] = 0.0 #For A*, insert F(v) in Q
# Array that contains the optimal distance to come from the starting state
dist_to_come = float("inf") * np.ones((world.shape[0], world.shape[1]))
dist_to_come[start_state.x, start_state.y] = 0
## Init the f(x) array
lower_bound = float("inf") * np.ones((world.shape[0], world.shape[1]))
lower_bound[start_state.x,
start_state.y] = self.heuristic(start_state, dest_state)
## Add the source to the priority queue
Q[start_state] = lower_bound[start_state.x, start_state.y]
# Boolean array that is true iff the distance to come of a state has been
# finalized
evaluated = np.zeros((world.shape[0], world.shape[1]), dtype='uint8')
# Contains key-value pairs of states where key is the parent of the value
# in the computation of the shortest path
parents = {start_state: None}
while Q:
# s is also removed from the priority Q with this
s = Q.pop_smallest()
# Assert s hasn't been evaluated before
assert (evaluated[s.x, s.y] == 0)
evaluated[s.x, s.y] = 1
if s == dest_state:
return self._follow_parent_pointers(parents, s)
# for all free neighboring states
for ns in self.get_neighboring_states(s):
if evaluated[ns.x, ns.y] == 1:
continue
transition_distance = sqrt((ns.x - s.x)**2 + (ns.y - s.y)**2)
alternative_dist_to_come_to_ns = dist_to_come[
s.x, s.y] + transition_distance
# if the state ns has not been visited before or we just found a shorter path
# to visit it then update its priority in the queue, and also its
# distance to come and its parent
if (ns not in Q):
##Update cost to come of ns
dist_to_come[ns.x, ns.y] = alternative_dist_to_come_to_ns
##Priority (Lower bound)
lower_bound[ns.x,
ns.y] = dist_to_come[ns.x,
ns.y] + self.heuristic(
ns, dest_state)
##Add to Q
Q[ns] = lower_bound[ns.x, ns.y]
##Set parent to be s
parents[ns] = s
elif (alternative_dist_to_come_to_ns < dist_to_come[ns.x,
ns.y]):
dist_to_come[ns.x, ns.y] = alternative_dist_to_come_to_ns
lower_bound[ns.x,
ns.y] = dist_to_come[ns.x,
ns.y] + self.heuristic(
ns, dest_state)
Q[ns] = lower_bound[ns.x, ns.y]
##Set parent to be s
parents[ns] = s
return [start_state]
def plan(self, start_state, dest_state):
"""
Returns the shortest path as a sequence of states [start_state, ..., dest_state]
if dest_state is reachable from start_state. Otherwise returns [start_state].
Assume both source and destination are in free space.
"""
assert (self.state_is_free(start_state))
assert (self.state_is_free(dest_state))
# Q is a mutable priority queue implemented as a dictionary
Q = priority_dict()
#Q[start_state] = 0.0
# For A* Priority is givien by f(V) so insert f(V) in Q
# Array that contains the optimal distance we've found from the starting state so far
best_dist_found = float("inf") * np.ones(
(world.shape[1], world.shape[0]))
best_dist_found[start_state.x, start_state.y] = 0
# lower bound definition for f(V)
lower_bound = float("inf") * np.ones((world.shape[0], world.shape[1]))
lower_bound[start_state.x, start_state.y] = self.heurisitic_calculator(
start_state, dest_state)
# adding the starting node to the priority queue.
# Initially Queue has just Start node
Q[start_state] = lower_bound[start_state.x, start_state.y]
# Boolean array that is true iff the distance to come of a state has been
# finalized
visited = np.zeros((world.shape[1], world.shape[0]), dtype='uint8')
# Contains key-value pairs of states where key is the parent of the value
# in the computation of the shortest path
parents = {start_state: None}
while Q:
# s is also removed from the priority Q with this
# this is also the start node initially since start node has lowest f cost in Q
s = Q.pop_smallest()
# Assert s hasn't been visited before
assert (visited[s.x, s.y] == 0)
# Mark it visited because here we will go over every neighbor,
# so there's no need to come back after this (by greedy property)
visited[s.x, s.y] = 1
# If we reach the destination stop and return the path.
if s == dest_state:
return self._follow_parent_pointers(parents, s)
# for all free neighboring states
for ns in self.get_neighboring_states(s):
if visited[ns.x, ns.y] == 1:
continue
transition_distance = sqrt((ns.x - s.x)**2 + (ns.y - s.y)**2)
alternative_best_dist_ns = best_dist_found[
s.x, s.y] + transition_distance
# if the state ns has not been visited before or we just found a shorter path
# to visit it then update its priority in the queue, and also its
# distance to come and its parent
# If neightbor node ns is not in queue:
# update the cost to come to ns
# update the lower bound in the priority queue
# set the parent of neighbor node ns to the current node s
if (ns not in Q) or (alternative_best_dist_ns <
best_dist_found[ns.x, ns.y]):
best_dist_found[ns.x, ns.y] = alternative_best_dist_ns
lower_bound[ns.x, ns.y] = best_dist_found[
ns.x, ns.y] + self.heurisitic_calculator(
ns, dest_state)
Q[ns] = lower_bound[ns.x, ns.y]
parents[ns] = s
return [start_state]