def aStar(start, goal, neighbor_func, distance_func, heuristic_func):
"""Returns a sequence of nodes that optmizes for the least cost
from the start node to the goal.
Let's describe the data that we pass to this function:
start: the start of the search.
goal: the goal of the search.
neighbor_func: a function that, given a state, returns a list of
neighboring states.
distance_func: a function that takes two nodes, and returns
the distance between them.
heuristic_func: a function that takes two nodes, and returns
the heuristic distance between them.
Each state mush be hashable --- each state must support the
hash() function.
"""
pqueue = PriorityQueue()
g_costs = {start : 1}
parents = {start : start}
pqueue.push(heuristic_func(start, goal), start)
while not pqueue.isEmpty():
next_cost, next_node = pqueue.pop()
g_costs[next_node] = g_costs[parents[next_node]] \
+ distance_func(next_node, parents[next_node])
if next_node == goal: break
children = neighbor_func(next_node)
for child in children:
updateChild(goal, distance_func, heuristic_func,
child, next_node, parents, g_costs, pqueue)
return getPathToGoal(start, goal, parents)
def prim(agraph, start):
"""
Prim's algorithm for minimum spanning tree
Using min-heap data structure
return a minimum spanning tree
"""
# vertex of minimun spanning tree
mst_vertex = []
pq = PriorityQueue()
for v in agraph:
v.setDistance(sys.maxsize)
v.setPred(None)
start.setDistance(0)
pq.buildHeap([(v.getDistance(), v) for v in agraph])
while not pq.isEmpty():
u = pq.delMin()
mst_vertex.append(u)
for adjacent in u.getConnections():
newcost = u.getWeight(adjacent)
if adjacent in pq and newcost < adjacent.getDistance():
adjacent.setPred(u)
adjacent.setDistance(newcost)
pq.decreaseKey(adjacent, newcost)
# edges of minimum spanning tree
mst = []
for i in range(1, len(mst_vertex)):
# u, v, cost
mst.append(
(mst_vertex[i - 1], mst_vertex[i], mst_vertex[i].getDistance()))
return mst
def prim(G):
cost = {}
parent = {}
u = None
P = PriorityQueue()
for v in G.vertexes:
if u is None:
u = v
cost[v] = float('inf')
P.add(float('inf'), v)
parent[v] = None
cost[u] = 0
P.change_priority(u, 0)
for i in P.Q:
print(i)
while not P.isEmpty():
print('wtf')
v_ele = P.get_min()
vertex = v_ele.data
print('minimum', v_ele)
for u, v, w in G.get_all_vertex(vertex):
print(u, v, w)
if P.check_ele(v) and cost[v] > cost[u] + w:
cost[v] = cost[u] + w
parent[v] = u
P.change_priority(v, cost[v])
print(cost)
print(parent)
def dijkstra_pyds3(agraph, start):
pq = PriorityQueue()
start.setDistance(0)
pq.buildHeap([(v.getDistance(), v) for v in agraph])
while not pq.isEmpty():
currentVert = pq.delMin()
for nextVert in currentVert.getConnections():
newDist = currentVert.getDistance() + currentVert.getWeight(
nextVert)
if newDist < nextVert.getDistance():
nextVert.setDistance(newDist)
nextVert.setPred(currentVert)
pq.decreaseKey(nextVert, newDist)
def dijkstra(agraph, start):
pq = PriorityQueue()
start.setdistance(0)
pq.buildheap([(v.getdistance(), v) for v in agraph])
while not pq.isEmpty():
currentvert = pq.delmin()
for nextvert in currentvert.get_connections():
newdist = currentvert.getdistance() + currentvert.get_weight(
nextvert)
if newdist < nextvert.getdistance():
nextvert.setdistance(newdist)
nextvert.setpred(currentvert)
pq.decreasekey(newvert, newdist)
def a_star_search(start, goal, heuristics=None):
visited = []
frontier = PriorityQueue()
start.set_dist_from_start(0)
if heuristics is None:
h_dist = 0
else:
h_dist = heuristics.get((start.getName(), goal.getName()), 0)
frontier.insertItem(
h_dist, start) #key should be the sum of distance from start and h()
#implement the algorithm
while not frontier.isEmpty():
#start your code here ...
if heuristics is None:
h_dist2 = 0
else:
h_dist2 = heuristics.get(
(frontier.queue[0][1].getName(), goal.getName()), 0)
#update the distance from start in the node
frontier.queue[0][1].set_dist_from_start((frontier.minKey() - h_dist2))
v = frontier.removeMin() #return the 1st node in the priority queue
visited.append(v)
if v.getName() == goal.getName():
#Include this line before returning the path (when the goal is found)
print("\nThe total number of nodes visited:", len(visited))
return retrieve_path(v, start)
else:
neighbors = v.getNeighbors()
for (item, diskey) in neighbors:
if not (item in visited):
if heuristics is None:
h_dist3 = 0
else:
h_dist3 = heuristics.get(
(item.getName(), goal.getName()), 0)
dist = diskey + v.get_dist_from_start() + h_dist3
if not (frontier.contains(item)):
frontier.insertItem(dist, item)
item.setParent(v)
else:
for tup in frontier.queue:
if tup[1] == item:
if dist < tup[0]:
frontier.update(dist, item)
tup[1].setParent(v)
def prim(g, start):
pq = PriorityQueue()
for v in g:
v.setPred(None)
v.setdistance(sys.maxsize)
start.setdistance(0)
pq = buildheap([(v.getdistance(), v) for v in g])
while not pq.isEmpty():
currentvert = pq.delmin()
for nextvert in currentvert.get_connections():
newcost = currentvert.get_weight(nextvert)
if nextvert in pq and newcost < nextvert.getdistance():
nextvert.setpred(currentvert)
nextvert.setdistance(newcost)
pq.decreasekey(nextvert, newcost)
def prim(agraph, start):
pq = PriorityQueue()
for v in agraph:
v.setDistance(sys.maxsize)
v.setPred(None)
start.setDistance(0)
pq.buildHeap([(v.getDistance(), v) for v in G])
while not pq.isEmpty():
u = pq.delMin()
for adjacent in u.getConnections():
newCost = u.getWeight(adjacent)
if adjacent in pq and newCost < adjacent.getDistance():
adjacent.setPred(u)
adjacent.setDistance(newCost)
pq.decreaseKey(adjacent, newCost)
def prim(self, start):
# Ejecutamos el algoritmo in-place
pq = PriorityQueue()
for v in self:
v.set_distance(sys.maxsize)
v.set_pred(None)
start.set_distance(0)
pq.buildHeap([(v.get_distance(), v) for v in self])
while not pq.isEmpty():
currentVert = pq.delMin()
for nextVert in currentVert.get_connections():
newCost = currentVert.get_weight(nextVert)
if nextVert in pq and newCost < nextVert.get_distance():
nextVert.set_pred(currentVert)
nextVert.set_distance(newCost)
pq.decreaseKey(nextVert, newCost)
def dijkstra(G, start):
pq = PriorityQueue()
for v in G:
v.setDistance(sys.maxsize)
v.setPred(None)
start.setDistance(0)
pq.buildHeap([(v.getDistance(),v) for v in G])
print(pq)
while not pq.isEmpty():
currentVert = pq.delMin()
for nextVert in currentVert.getConnections():
newDist = currentVert.getDistance() + currentVert.getWeight(nextVert)
if newDist < nextVert.getDistance():
nextVert.setDistance(newDist)
nextVert.setPred(currentVert)
pq.decreaseKey(nextVert, newDist)
print(pq)
def dijkstra(self, label: str):
index: int = self.findIndexByLabel(label)
self.vertices[index].weight = 0
pq = PriorityQueue()
pq.buildHeap(self.vertices)
current: Vertex
while not pq.isEmpty():
current = pq.deleteMin()
for neighbour in self.adjacencyList[current.label]:
if current.weight + neighbour.weight < self.vertices[
neighbour.index].weight:
self.prev[self.vertices[
neighbour.index].label] = current.label
self.vertices[
neighbour.
index].weight = current.weight + neighbour.weight
pq.decreaseKey(self.vertices[neighbour.index].key)
def djkistra(sourceId: int, vertices: list, vertexDict: dict, edgeList: list):
sourcePair = Pair(sourceId)
visited = set()
unvisited = PriorityQueue(contents=[sourcePair])
# build mapping labels -> vertexId & vertexId -> labels
labelsDict = {
int(v.attributes["label"].value): int(v.attributes["vertexId"].value)
for v in vertices
}
labels = [labelsDict[i] for i in range(len(labelsDict))]
previousLabelsDict = {v: k for k, v in labelsDict.items()}
vertexDict[sourceId].setCost(0)
vertexDict[sourceId].setPrevious(sourceId)
while not unvisited.isEmpty():
currentPair = unvisited.dequeue()
visited.add(currentPair.getVertexId())
currentVertex = vertexDict[currentPair.getVertexId()]
# grab adjacents.
adjacents = currentVertex.getAdjacents(edgeList)
for e in adjacents:
dist = vertexDict[currentVertex.vertexId].getCost() + e.weight
for vertex in [e.v1, e.v2]:
if vertex not in visited:
if vertexDict[vertex].getCost() > dist:
vertexDict[vertex].setCost(dist)
vertexDict[vertex].setPrevious(
currentVertex.getVertexId())
# heapq.heappush(unvisited, Pair(vertex, dist))
unvisited.enqueue(Pair(vertex, dist))
for i in range(len(visited)):
print("Vertex:")
print(" label: {}".format(i))
print(" cost: {:.2f}".format(vertexDict[labels[i]].getCost()))
print(" previous: {}\n".format(
previousLabelsDict[vertexDict[labelsDict[i]].getPrevious()]))
return labelsDict
def dijkstra(aGraph, start):
"""
Find Single-Source shortest-paths on a weighted, directed graph
Return shortest path
aGraph: class Graph
start: class Vertex
"""
pq = PriorityQueue()
start.setDistance(0)
pq.buildHeap([(v.getDistance(), v) for v in aGraph])
while not pq.isEmpty():
u = pq.delMin()
for adjacent in u.getConnections():
newDist = u.dist + u.getWeight(adjacent)
if adjacent.dist > newDist:
adjacent.setDistance(newDist)
adjacent.setPred(u)
pq.decreaseKey(adjacent, newDist)
def prims(self, label: str):
result: str = ""
index: int = self.findIndexByLabel(label)
self.vertices[index].weight = 0
pq = PriorityQueue()
pq.buildHeap(self.vertices)
current: Vertex
while not pq.isEmpty():
current = pq.deleteMin()
print(current.label)
if self.prev[current.label] is not None:
result += self.prev[
current.label] + " -> " + current.label + ", "
for neighbour in self.adjacencyList[current.label]:
if neighbour.weight < self.vertices[neighbour.index].weight:
self.prev[self.vertices[
neighbour.index].label] = current.label
self.vertices[neighbour.index].weight = neighbour.weight
pq.decreaseKey(self.vertices[neighbour.index].key)
print(result)
def prims(G):
cost = {}
parent = {}
u = None
P = PriorityQueue()
for v in G.vertexes:
if u is None:
u = v
cost[v] = float('inf')
P.add(float('inf'), v)
parent[v] = None
cost[u] = 0
P.change_priority(u, 0)
while not P.isEmpty():
v_ele = P.get_min()
vertex = v_ele.data
for u, v, w in G.get_all_vertex(vertex):
if P.check_ele(v) and cost[v] > cost[u] + w:
cost[v] = cost[u] + w
parent[v] = u
P.change_priority(v, cost[v])
def dijkstra(source, graph):
pQueue = PriorityQueue()
graph[source]['dist'] = 0
for v in graph:
pQueue.enqueue(v, graph[v]['dist'])
while not pQueue.isEmpty():
u = pQueue.dequeue()
baseDist = graph[u]['dist']
for w in graph[u]['edgeTo']:
edgeLen = graph[u]['edgeTo'][w]
newDist = baseDist + edgeLen
currentDist = graph[w]['dist']
if newDist < currentDist:
graph[w]['dist'] = newDist
pQueue.changePriority(w, newDist)
distanceList = []
for v in graph:
distanceList.append((v, graph[v]['dist']))
return distanceList
class AStar():
'''
Properties:
public:
- world: 2D array of Nodes
internal:
- size: (width, height) tuple of world
- open: Nodes queue to evaluate (heap-based priority queue)
'''
#----------------------------------------------------------------------
def __init__(self, world):
self.world = world
self.size = (len(world), len(world[0]))
# self.open = SortedList()
self.open = PriorityQueue()
self.openValue = 1
self.closedValue = 2
#----------------------------------------------------------------------
def initSearch(self, start, goal, obstacles):
''' first, check we can achieve the goal'''
if goal.type in obstacles:
return False
''' clear open list and setup new open/close value state to avoid the clearing of a closed list'''
self.open.clear()
self.openValue += 2
self.closedValue += 2
''' then init search variables'''
self.start = start
self.goal = goal
self.obstacles = obstacles
self.start.cost = 0
self.addToOpen(self.start)
self.goal.parent = None
return True
#----------------------------------------------------------------------
def search(self):
while not self.openIsEmpty():
current = self.popFromOpen()
if current == self.goal:
break
self.removeFromOpen(current)
self.addToClosed(current)
''' generator passes : look at the 8 neighbours around the current node from open'''
for (di, dj) in [(-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1),
(1, -1), (1, 0), (1, 1)]:
neighbour = self.getNode(current.i + di, current.j + dj)
if (not neighbour) or (neighbour.type in self.obstacles):
continue
'''the cost to get to this node is the current cost plus the movement
cost to reach this node. Note that the heuristic value is only used
in the open list'''
nextStepCost = current.cost + self.getNeighbourCost(
current, neighbour)
'''if the new cost we've determined for this node is lower than
it has been previously makes sure the node has not been
determined that there might have been a better path to get to
this node, so it needs to be re-evaluated'''
if nextStepCost < neighbour.cost and (
self.inOpenList(neighbour)
or self.inClosedList(neighbour)):
self.invalidateState(neighbour)
'''if the node hasn't already been processed and discarded then
step (i.e. to the open list)'''
if (not self.inOpenList(neighbour)) and (
not self.inClosedList(neighbour)):
neighbour.cost = nextStepCost
neighbour.heuristic = self.getHeuristicCost(
neighbour, self.goal)
neighbour.parent = current
self.addToOpen(neighbour)
''' exit with None = path not yet found'''
yield None
'''since we've run out of search
there was no path. Just return'''
if self.goal.parent is None:
return
'''At this point we've definitely found a path so we can uses the parent
references of the nodes to find out way from the target location back
to the start recording the nodes on the way.'''
path = []
goal = self.goal
while goal is not self.start:
path.insert(0, (goal.i, goal.j))
goal = goal.parent
''' done, exit with path'''
yield path
#-----------------------------------------------------------------------------
def getNode(self, i, j):
if i >= 0 and i < self.size[0] and j >= 0 and j < self.size[1]:
return self.world[i][j]
else:
return None
#----------------------------------------------------------------------
def getNeighbourCost(self, n1, n2):
return (abs(n2.i - n1.i) + abs(n2.j - n1.j))
#----------------------------------------------------------------------
def getHeuristicCost(self, n1, n2):
return (abs(n2.i - n1.i) + abs(n2.j - n1.j))
#----------------------------------------------------------------------
def invalidateState(self, node):
node.state = 0
#----------------------------------------------------------------------
def popFromOpen(self):
# return self.open.first()
return self.open.pop()
#----------------------------------------------------------------------
def addToOpen(self, node):
# self.open.add(node)
self.open.insert(node)
node.state = self.openValue
#----------------------------------------------------------------------
def inOpenList(self, node):
return node.state is self.openValue
#----------------------------------------------------------------------
def removeFromOpen(self, node):
# self.open.remove(node)
self.open.remove(node)
node.state = 0
#----------------------------------------------------------------------
def openIsEmpty(self):
# return not self.open.size()
return self.open.isEmpty()
#----------------------------------------------------------------------
def addToClosed(self, node):
node.state = self.closedValue
#----------------------------------------------------------------------
def inClosedList(self, node):
return node.state is self.closedValue
import priorityqueue
from priorityqueue import PriorityQueue
import numpy as np
pq = PriorityQueue(0, 20)
ints = np.random.randint(1, 100, size=20)
print("Inserting 20 integers into pq: {0}".format(ints))
[pq.insert(i) for i in ints]
print("pq is full: {0}".format(pq.isFull()))
print("pq size: {0}".format(pq.size()))
print("Deleting 20 integers from pq: {0}".format(
[pq.delMin() for i in range(20)], sep=','))
print("pq is empty: {0}".format(pq.isEmpty()))
print("pq size: {0}".format(pq.size()))
def sweep_line_algorithm(self):
self.current = Point()
pointsPQ = PriorityQueue()
tree = TreeSet()
pointsPQ.pushAll([seg.p for seg in self.segments])
pointsPQ.pushAll([seg.q for seg in self.segments])
res = 0
#print [str(x) for x in pointsPQ]
while not pointsPQ.isEmpty():
self.current.__update__(pointsPQ.pop())
#print "Round", current
if self.current.status == 'left':
#print "Adding", self.current.segment
low, high = tree.add_high_low(self.current.segment)
low = tree.lower(self.current.segment)
high = tree.higher(self.current.segment)
#print "Actual:", self.current.segment
#print "Low:", low, self.current.segment.intersect(low) if low else False
#print "High:", high, self.current.segment.intersect(high) if high else False
if low:
if self.current.segment.intersect(low):
a = self.current.segment.intersection_point(low)
#print "Adding a:", a, self.current.segment, low
pointsPQ.push(a)
if high:
if self.current.segment.intersect(high):
a = self.current.segment.intersection_point(high)
#print "Adding 2:", a, self.current.segment, high
pointsPQ.push(a)
elif self.current.status == "right":
low = tree.lower(self.current.segment)
high = tree.higher(self.current.segment)
if low and high:
if low.intersect(high):
a = low.intersection_point(high)
#print "Adding 3:", a, low, high
pointsPQ.push(a)
tree.remove(self.current.segment)
#print "Removing", self.current.segment
elif self.current.status == "int":
# exchange the position in tree of the two segments intersecting in current
s1, s2 = self.current.segment
#print "Between, swapping:", str(s1), str(s2)
tree.swap(s1, s2)
#print "After swap:", s1, s2, s1 is tree.lower(s2), s2 is tree.lower(s1)
#print "Modifying segments starts"
old_s1 = s1.p.node
old_s2 = s2.p.node
s1.set_p_node(self.current.node)
s2.set_p_node(self.current.node)
#print "Tree after modification:", [str(x) for x in tree]
# s1
if s1 is tree.lower(s2):
#print "... s1, s2, ..."
low = tree.lower(s1)
#print "s1:", s1, "low:", low, s1.intersect(low) if low else False
if low is not None:
if s1.intersect(low):
pointsPQ.push(s1.intersection_point(low))
high = tree.higher(s2)
#print "s2:", s2, "high:", high, s2.intersect(high) if high else False
if high is not None:
if s2.intersect(high):
pointsPQ.push(s2.intersection_point(high))
elif s2 is tree.lower(s1):
#print "... s2, s1, ..."
high = tree.higher(s1)
#print "s1:", s1, "high:", high, s1.intersect(high) if high else False
if high is not None:
if s1.intersect(high):
pointsPQ.push(s1.intersection_point(high))
low = tree.lower(s2)
#print "s2:", s2, "low:", low, s2.intersect(low) if low else False
if low is not None:
if s2.intersect(low):
pointsPQ.push(s2.intersection_point(low))
else:
print "Error" #raise SweepPlaneException("Intersection point error!")
res += 1
s1.set_p_node(old_s1)
s2.set_p_node(old_s2)
else:
print "Error 2" #raise SweepPlaneException("Node without status!")
#print "Tree", [str(x) for x in tree]
#print ""
self.nodes = self.nodes[:self.original_n_nodes]
return res
class AStar():
'''
Properties:
public:
- world: 2D array of Nodes
internal:
- size: (width, height) tuple of world
- open: Nodes queue to evaluate (heap-based priority queue)
'''
#----------------------------------------------------------------------
def __init__(self, world):
self.world = world
self.size = (len(world), len(world[0]))
# self.open = SortedList()
self.open = PriorityQueue()
self.openValue = 1
self.closedValue = 2
#----------------------------------------------------------------------
def initSearch(self, start, goal, obstacles):
''' first, check we can achieve the goal'''
if goal.type in obstacles:
return False
''' clear open list and setup new open/close value state to avoid the clearing of a closed list'''
self.open.clear()
self.openValue += 2
self.closedValue += 2
''' then init search variables'''
self.start = start
self.goal = goal
self.obstacles = obstacles
self.start.cost = 0
self.addToOpen(self.start)
self.goal.parent = None
return True
#----------------------------------------------------------------------
def search(self):
while not self.openIsEmpty():
current = self.popFromOpen()
if current == self.goal:
break
self.removeFromOpen(current)
self.addToClosed(current)
''' generator passes : look at the 8 neighbours around the current node from open'''
for (di, dj) in [(-1,-1), (-1,0), (-1,1), (0,-1), (0,1), (1,-1), (1,0), (1,1)]:
neighbour = self.getNode(current.i + di, current.j + dj)
if (not neighbour) or (neighbour.type in self.obstacles):
continue
'''the cost to get to this node is the current cost plus the movement
cost to reach this node. Note that the heuristic value is only used
in the open list'''
nextStepCost = current.cost + self.getNeighbourCost(current, neighbour)
'''if the new cost we've determined for this node is lower than
it has been previously makes sure the node has not been
determined that there might have been a better path to get to
this node, so it needs to be re-evaluated'''
if nextStepCost < neighbour.cost and (self.inOpenList(neighbour) or self.inClosedList(neighbour)):
self.invalidateState(neighbour)
'''if the node hasn't already been processed and discarded then
step (i.e. to the open list)'''
if (not self.inOpenList(neighbour)) and (not self.inClosedList(neighbour)):
neighbour.cost = nextStepCost
neighbour.heuristic = self.getHeuristicCost(neighbour, self.goal)
neighbour.parent = current
self.addToOpen(neighbour)
''' exit with None = path not yet found'''
yield None
'''since we've run out of search
there was no path. Just return'''
if self.goal.parent is None:
return
'''At this point we've definitely found a path so we can uses the parent
references of the nodes to find out way from the target location back
to the start recording the nodes on the way.'''
path = []
goal = self.goal
while goal is not self.start:
path.insert(0, (goal.i, goal.j))
goal = goal.parent
''' done, exit with path'''
yield path
#-----------------------------------------------------------------------------
def getNode(self, i, j):
if i >=0 and i < self.size[0] and j >= 0 and j < self.size[1]:
return self.world[i][j]
else:
return None
#----------------------------------------------------------------------
def getNeighbourCost(self, n1, n2):
return (abs(n2.i - n1.i) + abs(n2.j - n1.j))
#----------------------------------------------------------------------
def getHeuristicCost(self, n1, n2):
return (abs(n2.i - n1.i) + abs(n2.j - n1.j))
#----------------------------------------------------------------------
def invalidateState(self, node):
node.state = 0
#----------------------------------------------------------------------
def popFromOpen(self):
# return self.open.first()
return self.open.pop()
#----------------------------------------------------------------------
def addToOpen(self, node):
# self.open.add(node)
self.open.insert(node)
node.state = self.openValue
#----------------------------------------------------------------------
def inOpenList(self, node):
return node.state is self.openValue
#----------------------------------------------------------------------
def removeFromOpen(self, node):
# self.open.remove(node)
self.open.remove(node)
node.state = 0
#----------------------------------------------------------------------
def openIsEmpty(self):
# return not self.open.size()
return self.open.isEmpty()
#----------------------------------------------------------------------
def addToClosed(self, node):
node.state = self.closedValue
#----------------------------------------------------------------------
def inClosedList(self, node):
return node.state is self.closedValue
import priorityqueue
from priorityqueue import PriorityQueue
import numpy as np
pq = PriorityQueue(0,20)
ints = np.random.randint(1,100, size=20)
print("Inserting 20 integers into pq: {0}".format(ints))
[pq.insert(i) for i in ints]
print("pq is full: {0}".format(pq.isFull()))
print("pq size: {0}".format(pq.size()))
print("Deleting 20 integers from pq: {0}".format([pq.delMin() for i in range(20)], sep=','))
print("pq is empty: {0}".format(pq.isEmpty()))
print("pq size: {0}".format(pq.size()))
