def priority_queue(name):
if name == "heappq":
return HeapPQ()
elif name == "fibheap":
return FibHeap()
elif name == "fibpq":
return FibPQ()
elif name == "queuepq":
return QueuePQ()
else:
raise ValueError("Unrecognized priorty queue implementation '%s'" %
name)
def solve(maze):
width = maze.width
total = maze.width * maze.height
start = maze.start
startpos = start.Position
end = maze.end
endpos = end.Position
visited = [False] * total
prev = [None] * total
infinity = float("inf")
distances = [infinity] * total
# The priority queue. There are multiple implementations in priority_queue.py
# unvisited = FibHeap()
unvisited = HeapPQ()
# unvisited = FibPQ()
# unvisited = QueuePQ()
nodeindex = [None] * total
distances[start.Position[0] * width + start.Position[1]] = 0
startnode = FibHeap.Node(0, start)
nodeindex[start.Position[0] * width + start.Position[1]] = startnode
unvisited.insert(startnode)
count = 0
completed = False
while len(unvisited) > 0:
count += 1
n = unvisited.removeminimum()
u = n.value
upos = u.Position
uposindex = upos[0] * width + upos[1]
if distances[uposindex] == infinity:
break
if upos == endpos:
completed = True
break
for v in u.Neighbours:
if v != None:
vpos = v.Position
vposindex = vpos[0] * width + vpos[1]
if visited[vposindex] == False:
d = abs(vpos[0] - upos[0]) + abs(vpos[1] - upos[1])
# New path cost to v is distance to u + extra. Some descriptions of A* call this the g cost.
# New distance is the distance of the path from the start, through U, to V.
newdistance = distances[uposindex] + d
# A* includes a remaining cost, the f cost. In this case we use manhattan distance to calculate the distance from
# V to the end. We use manhattan again because A* works well when the g cost and f cost are balanced.
# https://en.wikipedia.org/wiki/Taxicab_geometry
remaining = abs(vpos[0] - endpos[0]) + abs(vpos[1] -
endpos[1])
# Notice that we don't include f cost in this first check. We want to know that the path *to* our node V is shortest
if newdistance < distances[vposindex]:
vnode = nodeindex[vposindex]
if vnode == None:
# V goes into the priority queue with a cost of g + f. So if it's moving closer to the end, it'll get higher
# priority than some other nodes. The order we visit nodes is a trade-off between a short path, and moving
# closer to the goal.
vnode = FibHeap.Node(newdistance + remaining, v)
unvisited.insert(vnode)
nodeindex[vposindex] = vnode
# The distance *to* the node remains just g, no f included.
distances[vposindex] = newdistance
prev[vposindex] = u
else:
# As above, we decrease the node since we've found a new path. But we include the f cost, the distance remaining.
unvisited.decreasekey(vnode,
newdistance + remaining)
# The distance *to* the node remains just g, no f included.
distances[vposindex] = newdistance
prev[vposindex] = u
visited[uposindex] = True
from collections import deque
path = deque()
current = end
while (current != None):
path.appendleft(current)
current = prev[current.Position[0] * width + current.Position[1]]
return [path, [count, len(path), completed]]
def solve(maze):
print("Using dijkstra algorithm")
# Width is used for indexing, total is used for array sizes
width = maze.width
total = maze.width * maze.height
# Start node, end node
start = maze.start
startpos = start.Position
end = maze.end
endpos = end.Position
# Visited holds true/false on whether a node has been seen already. Used to stop us returning to nodes multiple times
visited = [False] * total
# Previous holds a link to the previous node in the path. Used at the end for reconstructing the route
prev = [None] * total
# Distances holds the distance to any node taking the best known path so far. Better paths replace worse ones as we find them.
# We start with all distances at infinity
infinity = float("inf")
distances = [infinity] * total
# The priority queue. There are multiple implementations in priority_queue.py
# unvisited = FibHeap()
unvisited = HeapPQ()
# unvisited = FibPQ()
# unvisited = QueuePQ()
# This index holds all priority queue nodes as they are created. We use this to decrease the key of a specific node when a shorter path is found.
# This isn't hugely memory efficient, but likely to be faster than a dictionary or similar.
nodeindex = [None] * total
# To begin, we set the distance to the start to zero (we're there) and add it into the unvisited queue
distances[start.Position[0] * width + start.Position[1]] = 0
startnode = FibHeap.Node(0, start)
nodeindex[start.Position[0] * width + start.Position[1]] = startnode
unvisited.insert(startnode)
# Zero nodes visited, and not completed yet.
count = 0
completed = False
# Begin Dijkstra - continue while there are unvisited nodes in the queue
while len(unvisited) > 0:
count += 1
# Find current shortest path point to explore
n = unvisited.removeminimum()
# Current node u, all neighbours will be v
u = n.value
upos = u.Position
uposindex = upos[0] * width + upos[1]
if distances[uposindex] == infinity:
break
# If upos == endpos, we're done!
if upos == endpos:
completed = True
break
for v in u.Neighbours:
if v != None:
vpos = v.Position
vposindex = vpos[0] * width + vpos[1]
if visited[vposindex] == False:
# The extra distance from where we are (upos) to the neighbour (vpos) - this is manhattan distance
# https://en.wikipedia.org/wiki/Taxicab_geometry
d = abs(vpos[0] - upos[0]) + abs(vpos[1] - upos[1])
# New path cost to v is distance to u + extra
newdistance = distances[uposindex] + d
# If this new distance is the new shortest path to v
if newdistance < distances[vposindex]:
vnode = nodeindex[vposindex]
# v isn't already in the priority queue - add it
if vnode == None:
vnode = FibHeap.Node(newdistance, v)
unvisited.insert(vnode)
nodeindex[vposindex] = vnode
distances[vposindex] = newdistance
prev[vposindex] = u
# v is already in the queue - decrease its key to re-prioritise it
else:
unvisited.decreasekey(vnode, newdistance)
distances[vposindex] = newdistance
prev[vposindex] = u
visited[uposindex] = True
# We want to reconstruct the path. We start at end, and then go prev[end] and follow all the prev[] links until we're back at the start
from collections import deque
path = deque()
current = end
while (current != None):
path.appendleft(current)
current = prev[current.Position[0] * width + current.Position[1]]
return [path, [count, len(path), completed]]
def solve(maze):
width = maze.width
total = maze.width * maze.height
start = maze.start
startpos = start.Position
end = maze.end
endpos = end.Position
visited = [False] * total
prev = [None] * total
infinity = float("inf")
distances = [infinity] * total
# You can change what kind of priority queues you wish to use.
# unvisited = FibHeap()
unvisited = HeapPQ()
# unvisited = FibPQ()
# unvisited = QueuePQ()
nodeindex = [None] * total
distances[start.Position[0] * width + start.Position[1]] = 0
startnode = FibHeap.Node(0, start)
nodeindex[start.Position[0] * width + start.Position[1]] = startnode
unvisited.insert(startnode)
count = 0
completed = False
while len(unvisited) > 0:
count += 1
n = unvisited.removeminimum()
u = n.value
upos = u.Position
uposindex = upos[0] * width + upos[1]
if distances[uposindex] == infinity:
break
if upos == endpos:
completed = True
break
for v in u.Neighbours:
if v != None:
vpos = v.Position
vposindex = vpos[0] * width + vpos[1]
if visited[vposindex] == False:
d = abs(vpos[0] - upos[0]) + abs(vpos[1] - upos[1])
newdistance = distances[uposindex] + d
remaining = abs(vpos[0] - endpos[0]) + abs(vpos[1] - endpos[1])
if newdistance < distances[vposindex]:
vnode = nodeindex[vposindex]
if vnode == None:
vnode = FibHeap.Node(newdistance + remaining, v)
unvisited.insert(vnode)
nodeindex[vposindex] = vnode
distances[vposindex] = newdistance
prev[vposindex] = u
else:
unvisited.decreasekey(vnode, newdistance + remaining)
distances[vposindex] = newdistance
prev[vposindex] = u
visited[uposindex] = True
from collections import deque
path = deque()
current = end
while (current != None):
path.appendleft(current)
current = prev[current.Position[0] * width + current.Position[1]]
return [path, [count, len(path), completed]]
def a_star(to_solve: maze.Maze) -> list:
"""Uses the A* search algorithm (variant of Dijkstra's algorithm) to solve a maze, 'maze'.
Note that due to the way some mazes are structured -- very dense mazes with short paths -- A* may not outperform
a breadth-first search, and in fact may be almost identical in its operation with extra computational overhead.
However, this depends on the variety of maze supplied"""
# get our start and end nodes
start = to_solve.get_start()
end = to_solve.get_end()
# we will use the start and end positions so frequently that it is worth having variables for them; calling a
# function is not free, so this trades off a little memory in exchange for a little better performance
start_pos = start.get_position()
end_pos = end.get_position()
# set up our "visited" dictionary, like we have for BFS and DFS
visited = {}
# set up our priority queues
# the unvisited list will be a Heap Priority Queue; the nodes we want to visit will be ordered according to the
# heuristics we set for them -- get_distance from current node + Euclidian get_distance to end coordinate
unvisited = HeapPQ(
) # we can use any priority queue, but I have found HeapPQ to be the fastest for this purpose
start_node = FibHeap.Node(0, start)
unvisited.insert(start_node) # we start with the start node unvisited
# we also need an object to equate nodes from the Maze object with nodes from the FibHeap object
node_index = {}
node_index[start_pos] = start_node
# the distances of all nodes start at infinity, because we haven't visited them yet and we don't know what the
# get_distance is given the best known path
infinity = float("inf")
# track the get_distance to a given node using the best path known so far; better paths will replace worse ones as
# we go. Note, however that this will NOT include the additional heuristic of the get_distance from the point to the
# end position -- that information is included in the FibHeap node (we don't care about this additional heuristic
# when we aren't adding new ones to the queue)
distances = {} # the get_distance associated with S is 0
distances[start_pos] = 0
# track the number of considered nodes and whether we have completed the maze
node_count = 0
completed = False
# as long as we have nodes to visit, continue working
while not completed and len(unvisited) > 0:
node_count += 1
# get the node with the minimum combined heuristic and explore that first
node = unvisited.remove_minimum()
current = node.value # get the Maze.Node object
current_pos = current.get_position()
# if we are at the end, we have completed the maze; however, we can't exit just yet -- we need to wait until the
# queue is empty to be sure we have found the best solution
if current_pos == end_pos:
completed = True
# otherwise, check each unvisited child node, as in the breadth-first search
else:
# get our node's neighbors, and check each of them
neighbors = [
current.neighbors[maze.Direction.NORTH],
current.neighbors[maze.Direction.SOUTH],
current.neighbors[maze.Direction.EAST],
current.neighbors[maze.Direction.WEST]
]
for child in neighbors:
# if there is a neighbor at that position that we have not visited check it; otherwise, continue on
if child is not None and child.get_position() not in visited:
# get the positions so we can calculate our distances
parent_pos = current_pos
child_pos = child.get_position()
# get the get_distance of parent to child
parent_to_child = get_distance(parent_pos, child_pos)
# we also want to know the get_distance to this child without our additional heuristic of the get
# distance to the end node -- just the get_distance to the parent plus parent_to_child
path_length = distances[parent_pos] + parent_to_child
# get the get_distance of child to end -- this is our additional heuristic
remaining_distance = get_distance(child_pos, end_pos)
# if we have a get_distance associated with the node, fetch it; otherwise, it's infinity
if child_pos in distances:
current_distance = distances[child_pos]
else:
current_distance = infinity
# We will only update the get_distance if path length is less than the get_distance currently
# associated with this node's position -- if it's not, then the other path we have found to this
# node is shorter, and so we shouldn't make any changes in the path to that node
if path_length < current_distance:
# if we have a node for the child already, update it; otherwise, create a new node for the child
if child_pos in node_index:
# we want to decrease the get_distance heuristic of the child node; but first, we need to
# fetch the node from node_index, as decrease_key operates on a FibHeap.Node object
to_decrease = node_index[child_pos]
# the key is the coordinate, the new value is the path length plus the extra heuristic
unvisited.decrease_key(
to_decrease, path_length + remaining_distance)
# update the get_distance to this node as well -- we have found a shorter path; again, this
# does not include the additional heuristic
distances[child_pos] = path_length
# we also need to update the new parent node of that child
child.parent = current
# if we don't have a node for the child yet, create one
else:
# create a FibHeap node and add it to our priority queue
new_node = FibHeap.Node(
path_length + remaining_distance, child)
node_index[child_pos] = new_node
unvisited.insert(new_node)
# update the entry at the get_distance vector for the child and make sure we mark the
# current node as its previous node
distances[child_pos] = path_length
child.parent = current
# add this node to "visited" so we don't try to evaluate it again
visited[current_pos] = True
# in the same manner as in in the BFS algorithm, construct the path by going back through the parent of each node,
# starting at the end node and working backwards
if completed:
path = deque()
current = end
while current is not None:
path.appendleft(current.get_position(
)) # has a complexity of O(1) at each end, so do this instead of
current = current.parent # appending to a list and reversing it at the end
else:
path = []
# we must return (bool)solved, (int)node_count, (list< tuple< int, int > >)path
return completed, node_count, path
def solve(maze):
width = maze.width
total = maze.width * maze.height
start = maze.start
startpos = start.Position
end = maze.end
endpos = end.Position
visited = [False] * total
prev = [None] * total
infinity = float("inf")
distances = [infinity] * total
# The priority queue. There are multiple implementations in priority_queue.py
# unvisited = FibHeap()
unvisited = HeapPQ()
# unvisited = FibPQ()
# unvisited = QueuePQ()
nodeindex = [None] * total
distances[start.Position[0] * width + start.Position[1]] = 0
startnode = FibHeap.Node(0, start)
nodeindex[start.Position[0] * width + start.Position[1]] = startnode
unvisited.insert(startnode)
count = 0
completed = False
while len(unvisited) > 0:
count += 1
n = unvisited.removeminimum()
u = n.value
upos = u.Position
uposindex = upos[0] * width + upos[1]
if distances[uposindex] == infinity:
break
if upos == endpos:
completed = True
break
for v in u.Neighbours:
if v != None:
vpos = v.Position
vposindex = vpos[0] * width + vpos[1]
if visited[vposindex] == False:
d = abs(vpos[0] - upos[0]) + abs(vpos[1] - upos[1])
# New path cost to v is distance to u + extra. Some descriptions of A* call this the g cost.
# New distance is the distance of the path from the start, through U, to V.
newdistance = distances[uposindex] + d
# A* includes a remaining cost, the f cost. In this case we use manhattan distance to calculate the distance from
# V to the end. We use manhattan again because A* works well when the g cost and f cost are balanced.
# https://en.wikipedia.org/wiki/Taxicab_geometry
remaining = abs(vpos[0] - endpos[0]) + abs(vpos[1] - endpos[1])
# Notice that we don't include f cost in this first check. We want to know that the path *to* our node V is shortest
if newdistance < distances[vposindex]:
vnode = nodeindex[vposindex]
if vnode == None:
# V goes into the priority queue with a cost of g + f. So if it's moving closer to the end, it'll get higher
# priority than some other nodes. The order we visit nodes is a trade-off between a short path, and moving
# closer to the goal.
vnode = FibHeap.Node(newdistance + remaining, v)
unvisited.insert(vnode)
nodeindex[vposindex] = vnode
# The distance *to* the node remains just g, no f included.
distances[vposindex] = newdistance
prev[vposindex] = u
else:
# As above, we decrease the node since we've found a new path. But we include the f cost, the distance remaining.
unvisited.decreasekey(vnode, newdistance + remaining)
# The distance *to* the node remains just g, no f included.
distances[vposindex] = newdistance
prev[vposindex] = u
visited[uposindex] = True
from collections import deque
path = deque()
current = end
while (current != None):
path.appendleft(current)
current = prev[current.Position[0] * width + current.Position[1]]
return [path, [count, len(path), completed]]
def AStar_Lat(start, goal, neighbor_nodes, distance, cost_estimate, weights):
width, height = 512, 512
def idx(pos):
return pos[1] * width + pos[0]
total_size = width * height
infinity = float("inf")
distances = [infinity] * total_size
visited = [False] * total_size
prev = [None] * total_size
unvisited = HeapPQ()
node_index = [None] * total_size
distances[idx(start)] = 0
start_node = FibHeap.Node(0, start)
node_index[idx(start)] = start_node
unvisited.insert(start_node)
count = 0
aa = 0 ## to make sure not get too long roots
completed = False
plant_id = -1
final_goal_position = None
while len(unvisited) > 0:
n = unvisited.removeminimum()
upos = n.value
uposindex = idx(upos)
if distances[uposindex] == infinity:
break
if upos in goal:
completed = True
#plant_id = goal[upos]
final_goal_position = upos
print(final_goal_position)
break
for v in neighbor_nodes(upos):
vpos = v[0]
vposindex = idx(vpos)
if is_blocked_edge(vpos):
continue
if visited[vposindex]:
continue
# Calculate distance to travel to vpos
d = weights[vpos[1], vpos[0]]
new_distance = distances[uposindex] + d * v[1]
if new_distance < distances[vposindex]:
aa = distances[vposindex]
vnode = node_index[vposindex]
if vnode is None:
vnode = FibHeap.Node(new_distance, vpos)
unvisited.insert(vnode)
node_index[vposindex] = vnode
distances[vposindex] = new_distance
prev[vposindex] = upos
aa = distances[vposindex]
else:
unvisited.decreasekey(vnode, new_distance)
distances[vposindex] = new_distance
prev[vposindex] = upos
aa = distances[vposindex]
visited[uposindex] = True
if completed and aa <= 200:
from collections import deque
path = deque()
current = final_goal_position
while current is not None:
path.appendleft(current)
current = prev[idx(current)]
return path
else:
return []
