def UCSRoute(graph, startVert, goalVert):
if startVert == goalVert:
return []
minHeap = PriorityQueue()
minHeap.insert(0, startVert)
pred_cost = {}
visited = set()
pred = {}
while not minHeap.isEmpty():
nextVert = minHeap.firstElement() #nextVert = [cost, vert]
minHeap.delete()
print("--------------")
print("Popping", nextVert)
if nextVert[1] in visited:
continue
else:
visited.add(nextVert[1])
if (nextVert[0], nextVert[1]) in pred_cost.keys():
pred[nextVert[1]] = pred_cost[(nextVert[0], nextVert[1])]
else:
pred[nextVert[1]] = None
if goalVert == nextVert[1]:
return reconstructPath(startVert, goalVert, pred)
neighbors = graph.getNeighbors(nextVert[1])
for n in neighbors:
neighNode = n[0]
edgeCost = n[1]
if neighNode not in visited:
minHeap.insert(edgeCost + nextVert[0], neighNode)
pred_cost[(edgeCost + nextVert[0], neighNode)] = nextVert[1]
return "NO PATH"
def UCSRoute(graph, startVert, goalVert):
"""This algorithm search a graph using Uniform Cost Search algorithm"""
if startVert == goalVert:
return []
q = PriorityQueue()
q.insert(0, (startVert, None))
visited = set()
pred = {}
while not q.isEmpty():
weight, (nextVert, predNextvert) = q.firstElement()
q.delete()
if nextVert in visited:
pass
else:
visited.add(nextVert)
pred[nextVert] = predNextvert
if nextVert == goalVert:
print("UCS number of visited: ", len(visited))
return reconstructPath(startVert, goalVert, pred)
neighbors = graph.getNeighbors(nextVert)
for n in neighbors:
if type(n) != int:
# NOTICE: From getNeighbors, the order is (vert, weight)
weight_n = n[1]
n = n[0]
if n not in visited:
q.insert(weight + weight_n, (n, nextVert))
return "NO PATH"
def AStarRoute(graph, startVert, goalVert):
"""This algorithm search a graph using A star Search algorithm"""
if startVert == goalVert:
return []
q = PriorityQueue()
q.insert(0, (startVert, None))
visited = set()
pred = {}
while not q.isEmpty():
weight, (nextVert, predNextvert) = q.firstElement()
# The weight in PriorityQueue also contains the heuristicDist, not the actual weight
# Calculate the actual weight
weight = weight - graph.heuristicDist(nextVert, goalVert)
q.delete()
if nextVert in visited:
pass
else:
visited.add(nextVert)
pred[nextVert] = predNextvert
if nextVert == goalVert:
print("Astar number of visited: ", len(visited))
return reconstructPath(startVert, goalVert, pred)
neighbors = graph.getNeighbors(nextVert)
for n in neighbors:
if type(n) != int:
# NOTICE: From getNeighbors, the order is (vert, weight)
weight_n = n[1]
n = n[0]
if n not in visited:
q.insert(weight + weight_n + graph.heuristicDist(n, goalVert), (n, nextVert))
return "NO PATH"
def initialize(self):
"""The Initialize algorithm from the pseudocode."""
self.u = PriorityQueue()
vertices = self.graph.getVertices()
for vert in vertices:
self.rhs[vert] = self.maxVal
self.g[vert] = self.maxVal
self.rhs[self.startVert] = 0
self.u.insert(priority=self.calculateKey(self.startVert),
val=self.startVert)
def dijkstras(graph, startVert, goalVert):
""" This algorithm searches a graph using Dijkstras algorithm to find
the shortest path from every point to a goal point (actually
searches from goal to every point, but it's the same thing.
It uses a priority queue to store the indices of vertices that it still
needs to examine.
It returns the best path frmo startVert to goalVert, but otherwise
startVert does not play into the search."""
num_q_nodes_removed = 0
max_queue_size = -1
if startVert == goalVert:
return []
q = PriorityQueue()
visited = set()
pred = {}
cost = {}
for vert in graph.getVertices():
cost[vert] = 1000.0
pred[vert] = None
q.insert(cost[vert], vert)
visited.add(goalVert)
cost[goalVert] = 0
q.update(cost[goalVert], goalVert)
while not q.isEmpty():
(nextCTG, nextVert) = q.firstElement()
if q.getSize() > max_queue_size:
max_queue_size = q.getSize()
q.delete()
num_q_nodes_removed += 1
visited.add(nextVert)
# print("--------------")
# print("Popping", nextVert, nextCTG)
neighbors = graph.getNeighbors(nextVert)
for n in neighbors:
neighNode = n[0]
edgeCost = n[1]
if neighNode not in visited and\
cost[neighNode] > nextCTG + edgeCost:
# print("Node", neighNode, "From", nextVert)
# print("New cost =", nextCTG + edgeCost)
cost[neighNode] = nextCTG + edgeCost
pred[neighNode] = nextVert
q.update(cost[neighNode], neighNode)
# This part is finding the best path from all nodes to the goal. Not necessary
# for vert in graph.getVertices():
# bestPath = reconstructPath(goalVert, vert, pred)
# bestPath.reverse()
# print("Best path from ", vert, "to", goalVert, "is", bestPath)
print("====> Dijkstra number of nodes visited: ", len(visited))
print("====> Dijkstra number of nodes removed: ", num_q_nodes_removed)
print("====> Dijkstra max size of queue: ", max_queue_size)
finalPath = reconstructPath(goalVert, startVert, pred)
finalPath.reverse()
return finalPath
def initialize(self):
"""The Initialize algorithm from the pseudocode."""
self.U = PriorityQueue()
self.nodesRemoved = 0
self.maxSize = 0
self.rhs = {}
self.g = {}
for node in self.graph.getVertices():
self.rhs[node] = self.maxVal
self.g[node] = self.maxVal
self.rhs[self.startVert] = 0
self.U.insert(
self.calculateKey(self.startVert), self.startVert
) # The priority queue stores the priority first, then the vertex
def AStarRoute(graph, startVert, goalVert):
""" This algorithm searches a graph using Uniform Cost Search
looking for a path from some start vertex to some goal vertex using
lowest cost. It uses a PriorityQueue to store the indices of
vertices that it still needs to examine."""
maxQueueSize = 0
nodesVisited = 0
if startVert == goalVert:
return []
q = PriorityQueue()
q.insert(graph.heuristicDist(startVert, goalVert), startVert)
visited = {startVert}
pred = {startVert: None}
totalCost = {startVert: 0}
while not q.isEmpty():
if q.getSize() > maxQueueSize:
maxQueueSize = q.getSize()
nextCost, nextVert = q.firstElement()
nodesVisited += 1
nextCost = nextCost - graph.heuristicDist(nextVert, goalVert)
if nextVert == goalVert:
print("Total Cost is : " + str(nextCost))
print("Total Nodes Visited : " + str(nodesVisited))
print("Max queue size: " + str(maxQueueSize))
return reconstructPath(startVert, goalVert, pred)
q.delete()
neighbors = graph.getNeighbors(nextVert)
for (node, edgeCost) in neighbors:
cost = nextCost + edgeCost
# cost = nextCost + edgeCost + graph.heuristicDist(node, goalVert)
if node not in visited:
visited.add(node)
pred[node] = nextVert
totalCost[node] = cost
q.insert(totalCost[node] + graph.heuristicDist(node, goalVert),
node)
else:
if cost < totalCost[node]:
pred[node] = nextVert
totalCost[node] = cost
q.insert(
totalCost[node] + graph.heuristicDist(node, goalVert),
node)
return "NO PATH"
def AStarRoute(graph, startVert, goalVert):
nodesRemoved = 0
maxSize = 0
if startVert == goalVert:
return []
minHeap = PriorityQueue()
minHeap.insert(0, startVert)
pred_cost = {}
visited = set()
pred = {}
while not minHeap.isEmpty():
if minHeap.size > maxSize:
maxSize = minHeap.size
nextVert = minHeap.firstElement() # nextVert = [cost, vert]
# print("--------------")
# print("Popping", nextVert)
minHeap.delete()
nodesRemoved = nodesRemoved + 1
if nextVert[1] in visited:
continue
else:
visited.add(nextVert[1])
if (nextVert[0], nextVert[1]) in pred_cost.keys():
pred[nextVert[1]] = pred_cost[(nextVert[0], nextVert[1])]
else:
pred[nextVert[1]] = None
if goalVert == nextVert[1]:
return nodesRemoved, maxSize, reconstructPath(
startVert, goalVert, pred)
neighbors = graph.getNeighbors(nextVert[1])
# print('Adding neighbors to the queue: ')
for n in neighbors:
neighNode = n[0]
edgeCost = n[1]
if neighNode not in visited:
Gcost = edgeCost + nextVert[0]
Hcost = graph.heuristicDist(neighNode, goalVert)
if startVert != nextVert[1]:
Gcost = Gcost - graph.heuristicDist(nextVert[1], goalVert)
Fcost = Gcost + Hcost
# print('Node ' + str(neighNode) + ' from ' + str(nextVert[1]))
# print('G cost = ' + str(Gcost) + ' H cost = ' + str(Hcost) + ' F cost = ' + str(Fcost))
minHeap.insert(Fcost, neighNode)
pred_cost[(Fcost, neighNode)] = nextVert[1]
return "NO PATH"
def dijkstras(graph, startVert, goalVert):
""" This algorithm searches a graph using Dijkstras algorithm to find
the shortest path from every point to a goal point (actually
searches from goal to every point, but it's the same thing.
It uses a priority queue to store the indices of vertices that it still
needs to examine.
It returns the best path frmo startVert to goalVert, but otherwise
startVert does not play into the search."""
if startVert == goalVert:
return []
q = PriorityQueue()
visited = set()
pred = {}
cost = {}
for vert in graph.getVertices():
cost[vert] = 1000.0
pred[vert] = None
q.insert(cost[vert], vert)
visited.add(goalVert)
cost[goalVert] = 0
q.update(cost[goalVert], goalVert)
while not q.isEmpty():
(nextCTG, nextVert) = q.firstElement()
q.delete()
visited.add(nextVert)
print("--------------")
print("Popping", nextVert, nextCTG)
neighbors = graph.getNeighbors(nextVert)
for n in neighbors:
neighNode = n[0]
edgeCost = n[1]
if neighNode not in visited and\
cost[neighNode] > nextCTG + edgeCost:
print("Node", neighNode, "From", nextVert)
print("New cost =", nextCTG + edgeCost)
cost[neighNode] = nextCTG + edgeCost
pred[neighNode] = nextVert
q.update( cost[neighNode], neighNode )
for vert in graph.getVertices():
bestPath = reconstructPath(goalVert, vert, pred)
bestPath.reverse()
print("Best path from ", vert, "to", goalVert, "is", bestPath)
finalPath = reconstructPath(goalVert, startVert, pred)
finalPath.reverse()
return finalPath
def __init__(self, graph, startVert, goalVert):
"""Takes in a graph, start vertex and goal vertex, and sets up the D* Lite
search, initializing all the data structures and the priority queue."""
self.graph = graph
self.startVert = startVert
self.goalVert = goalVert
self.maxVal = 1000
# PriorityQueue with values [fEst, gEst]
self.U = PriorityQueue()
# estimated cost from g and c for a vertex
self.rhs = {}
# estimated cost from start vertex to other vertex
self.g = {}
# print(self.startVert, self.goalVert)
self.initialize()
def _setupFringe(self, startState):
"""This method sets up the proper kind of fringe set for this particular search.
In this case, it creates a priority queue and adds the start state to it."""
self.fringe = PriorityQueue()
self.fringe.insert(startState, startState.getCost())