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]