def dijkstra_search(root, goal, init, domprob, nb_robots, width, height):
"""Dijkstra search of a solution in a graph.
Returns a path if there is any.
The priority of each node represent the cost
(if each action costs 1) to go from the root
to the node.
Parameters:
root: Node object, the root of the state graph we're
searching and building at the same time.
goal: bitvector, the state we're searching.
init: bitvector, the initial state of the maze.
domprob: Domain-Problem object from the pddlpy lib.
nb_robots: integer, the number of robots in the maze.
width, height: integers, the dimensions of the maze.
"""
# Priority queue
pqueue = PriorityQueue()
# an empty set to maintain visited nodes
closed_set = set()
# a dictionary for path formation
meta = dict() # key -> (parent state, action to reach child)
# Operator list
op_list = list(domprob.operators())
# initialize
pqueue.insert(root)
meta[root] = (None, None)
ground_op_bv = get_ground_operator(
op_list, domprob, init, nb_robots, width, height)
print("Taille de ground_op : {}".format(len(ground_op_bv[0])))
while not pqueue.empty():
subtree_root = pqueue.dequeue()
current_priority = subtree_root.priority
if is_goal(subtree_root, goal):
return construct_path(subtree_root, meta)
# Create current node's children
for op in ground_op_bv:
subtree_root.build_children(op)
for (child, action) in subtree_root.children:
# The node has already been processed, so skip over it
if child in closed_set:
continue
# The child is not enqueued to be processed,
# so enqueue this level of children to be expanded
if child not in pqueue.queue:
child.priority = current_priority + 1
# Update the path
meta[child] = (subtree_root, action)
# Enqueue this node
pqueue.insert(child)
closed_set.add(subtree_root)
def get_surrounding_tiles(self, position, order='finish'):
""" Liefert eine Queue der angrenzenden Tiles zurück."""
tiles = PriorityQueue(order)
# print('Order: ', order)
# self.surroundings
for surround in self.surroundings:
# Ränder abfragen
# y unten
if position[0] == len(self.tiles) - 1 and surround[0] == +1:
continue
# y oben
if position[0] == 0 and surround[0] == -1:
continue
# x rechts
if position[1] == len(self.tiles[0]) - 1 and surround[1] == +1:
continue
# x links
if position[1] == 0 and surround[1] == -1:
continue
x = position[1] + surround[1]
y = position[0] + surround[0]
tiles.insert(self.tiles[y][x])
# Wenn Position am unteren Rande der y-Achse ist
# tiles.sort(key=lambda x: x.estimated_cost_to_finish, reverse=False)
return tiles
def get_path(self, start, end, board, cost_estimate=get_distance):
t0 = time.time()
explored = set()
previous = {}
previous[start] = None
moves = {}
moves[start] = 0
frontier = PriorityQueue()
frontier.insert(start, cost_estimate(start, end))
if VERBOSE_ASTAR: print 'get_path start, end:', start, end
while not frontier.is_empty():
if (time.time() - t0 > PATHFINDER_TIMEOUT):
print 'PATHFINDING TIMEOUT: Averting disconnect...'
print ' get_path: Probably could not find a valid path from', start, 'to', end
return [start, start]
if VERBOSE_ASTAR: print 'get_path frontier:', frontier
current = frontier.remove()
explored.add(current)
if VERBOSE_ASTAR: print 'get_path explored set', explored
if VERBOSE_ASTAR: print 'get_path current pos:', current
if (current == end):
if VERBOSE_ASTAR: print 'Found end loc'
break
else:
neighbors = get_neighboring_locs(current, board)
if VERBOSE_ASTAR: print 'get_path neighbors:', neighbors
for n in neighbors:
if n not in explored and (board.passable(n)
or n in (start, end)):
moves[n] = moves[current] + MOVE_COST
frontier.insert(n, cost_estimate(n, end) + moves[n])
previous[n] = current
# found goal, now reconstruct path
i = end
path = [i]
while i != start:
if (i in previous):
path.append(previous[i])
i = previous[i]
else:
print 'get_path error: probably could not find a valid path from', start, 'to', end
return [start, start] # return something valid
path.reverse()
return path
def get_path(self, start, end, board, cost_estimate=get_distance):
t0 = time.time()
explored = set()
previous = {}
previous[start] = None
moves = {}
moves[start] = 0
frontier = PriorityQueue()
frontier.insert(start, cost_estimate(start, end))
if VERBOSE_ASTAR: print 'get_path start, end:', start, end
while not frontier.is_empty():
if (time.time() - t0 > PATHFINDER_TIMEOUT):
print 'PATHFINDING TIMEOUT: Averting disconnect...'
print ' get_path: Probably could not find a valid path from', start, 'to', end
return [start, start]
if VERBOSE_ASTAR: print 'get_path frontier:', frontier
current = frontier.remove()
explored.add(current)
if VERBOSE_ASTAR: print 'get_path explored set', explored
if VERBOSE_ASTAR: print 'get_path current pos:', current
if (current == end):
if VERBOSE_ASTAR: print 'Found end loc'
break
else:
neighbors = get_neighboring_locs(current, board)
if VERBOSE_ASTAR: print 'get_path neighbors:', neighbors
for n in neighbors:
if n not in explored and (board.passable(n) or n in (start, end)):
moves[n] = moves[current] + MOVE_COST
frontier.insert(n, cost_estimate(n, end) + moves[n])
previous[n] = current
# found goal, now reconstruct path
i = end
path = [i]
while i != start:
if (i in previous):
path.append(previous[i])
i = previous[i]
else:
print 'get_path error: probably could not find a valid path from', start, 'to', end
return [start, start] # return something valid
path.reverse()
return path
def choose_best(self, locs):
best = None
q = PriorityQueue()
print 'choose best, locs:', locs
if locs != None:
for loc in locs:
q.insert(loc, -self.score_loc(loc)) # by highest score
best = q.remove()
print 'choose best, best:', best
return best
def main():
maze = Maze("maze2.png")
paths = PriorityQueue()
start = maze.getStart()
end = maze.getEnd()
paths.insert(PathHead(start[0], start[1], calcDistance(start, end)))
current = paths.minimum()
while paths.size() > 0 and current.getDistance() != 0.0:
current = paths.extractMin()
maze.setNodeVisited(current.getCords())
surroundings = maze.checkSurroundings(current.getCords())
insertSurroundings(paths, surroundings, current, end)
solved = maze.getMaze()
solved = numpy.array(solved)
img = Image.fromarray(solved.astype('uint8'), 'RGB')
img.save('solved.png')
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()))
class Navi:
def __init__(self, position_start, position_finish, map):
tiles = []
self.open_list = PriorityQueue()
self.closed_list = PriorityQueue()
# queue = PriorityQueue()
self.map = map
self.finish_reached = False
self.route = []
self.position_start = position_start
self.position_finish = position_finish
self.open_list.insert(
TileInfo(position_start,
self.get_estimated_cost_to_finish(position_start), 0, -1,
-1))
for y in range(0, map.height):
tiles.append([])
for x in range(0, map.width):
tile = TileInfo((y, x), map.tiles[y][x],
self.get_estimated_cost_to_finish((y, x)),
99999, -1)
tiles[y].append(tile)
self.tiles = tiles
self.navi_active = False
self.recursion_level = 0
self.max_recursion_level = 100
self.use_diagonal_tiles = True
# Array für die Abfrage der umgebenden Tiles
self.surroundings = []
if self.use_diagonal_tiles == True:
self.surroundings.append((-1, -1))
self.surroundings.append((-1, +1))
self.surroundings.append((+1, -1))
self.surroundings.append((+1, +1))
self.surroundings.append((-1, 0))
self.surroundings.append((0, -1))
self.surroundings.append((0, +1))
self.surroundings.append((+1, 0))
def navi_step(self, tile_work='next'):
# map = self.map
# print('navistep')
self.recursion_level += 1
if tile_work == 'next':
tile_work = self.open_list.get_and_delete()
# pre_tile = self.tiles[tile_work.position[0]][tile_work.position[1]].pre_tile
# Den Vorgänger-Tile des work-Tiles holen
pre_tile = self.get_pre_tile(tile_work)
# Wenn der Tile > -1 ist, hole die Kosten zum Start.
if not pre_tile == -1:
pre_tile_cost_from_start = self.tiles[pre_tile[0]][
pre_tile[1]].cost_from_start
else:
pre_tile_cost_from_start = -1
# Wenn der Work-Tile die Zielposition, also das Ziel erreicht ist.
if tile_work.position == self.position_finish:
self.map.add_status_text_with_clear("FINISH")
tile_work.set_route_cost(pre_tile_cost_from_start + 1)
self.route_finished(tile_work)
self.finish_reached = True
if pre_tile_cost_from_start >= 99999:
pre_tile_cost_from_start = 0
# Work-Tile: Die Kosten zum Start sind Pre-Tile + 1
tile_work_cost_from_start = pre_tile_cost_from_start + 1
tile_work.set_cost_from_start(tile_work_cost_from_start)
tile_work.set_route_cost(
self.get_estimated_cost_to_finish(tile_work.position) +
tile_work.cost_from_start)
tile_work.status = 0
# Der Work-Tile wurde berechnet und kann also auf die Closed-List
self.closed_list.insert(tile_work)
self.tiles[tile_work.position[0]][
tile_work.position[1]].type = "closed"
# Um weiter zu machen, holen wir uns die umgebenden Tiles
surrounding_tiles = self.get_surrounding_tiles(tile_work.position)
# Solange wir noch nicht alle Tiles bearbeitet haben, durchlaufen wir die while-Schleife
while not surrounding_tiles.isEmpty():
# print(surrounding_tiles.get_size())
surrounding_tile = surrounding_tiles.get_and_delete()
if surrounding_tile == False:
# print("Surround: no next tiles")
break
if surrounding_tile.type == "wall":
# print('Surround: wall')
continue
tile_cost_from_start = tile_work_cost_from_start + 1
if self.closed_list.exist(surrounding_tile):
# Wenn ein Tile bereits in der closedlist ist, wurde er schon mal hinzugefügt
# Es wird dann gecheckt, ob ...?
# print('Surround: is in closedlist')
continue
elif self.open_list.exist(surrounding_tile):
# Wenn ein Tile bereits in der openlist ist, wurde er schon mal hinzugefügt
# Es wird dann gecheckt, ob ...?
# print('Surround: is in openlist')
tile_from_open_list = self.open_list.get_tile_and_delete(
surrounding_tile)
# print(tile_from_open_list.cost_from_start, tile_cost_from_start)
if tile_from_open_list.cost_from_start + 1 >= tile_cost_from_start:
# print('Surround: Neuer Weg ist teurer')
continue
else:
# print('Surround: Neuer Weg ist günstiger')
tile_from_open_list.cost_from_start = surrounding_tile.cost_from_start + 1
tile_from_open_list.set_route_cost(
self.get_estimated_cost_to_finish(
tile_from_open_list.position) +
tile_work_cost_from_start)
self.open_list.insert(tile_from_open_list)
continue
else:
if surrounding_tile.position == tile_work.pre_tile:
# Wenn der umliegende Tile der vorherige vom tile_work ist, kann er ignoriert werden
continue
# Wenn bis hierher nichts dagegen spricht, ist der Tile legitim, um ihn in nem navistep zu bearbeiten
# pre-tile festlegen
surrounding_tile.pre_tile = tile_work.position
# Den pre-tile auch in der tiles.Liste festlegen
self.tiles[surrounding_tile.position[0]][
surrounding_tile.position[1]].pre_tile = tile_work.position
# In die open-list einfügen
self.open_list.insert(surrounding_tile)
# Entsprechenden Tile als open markieren
self.tiles[surrounding_tile.position[0]][
surrounding_tile.position[1]].type = "open"
# print("Open List: ", self.open_list.get_size())
# print("Closed List: ", self.closed_list.get_size())
# print(self.finish_reached)
# if self.finish_reached == False and self.recursion_level < self.max_recursion_level:
# self.navi_step()
self.recursion_level = 0
return (tile_work.position, tile_work.route_cost)
# self.navi_step(tile.position,position)
def route_finished(self, tile):
""" Route wurde gefunden! """
route = []
route.append(tile.position)
next_tile = tile.pre_tile
while True:
route.append(next_tile)
if len(route) > 1000:
print('Finish: Route > 1000')
break
# print(next_tile)
next_tile = self.tiles[next_tile[0]][next_tile[1]].pre_tile
if next_tile == self.position_start:
print('Finish: Start erreicht.')
break
if next_tile == -1:
break
for tile_position in route:
self.tiles[tile_position[0]][tile_position[1]].type = "route"
self.map.add_status_text("Kosten: " + str(tile.get_route_cost()))
print("Kosten: ", tile.get_route_cost())
self.map.add_status_text("Länge Route: " + str(len(route)))
print("Länge Route: ", len(route))
# print(route)
self.navi_active = False
self.position_start = tile.position
def get_next_navi_tile(self, surrounding_tiles, position, last_position):
""" Liefert den nächsten Navi-Tile zurück. Checkt, ob alle Bedingungen eingehalten werden."""
# Bedingungen:
# 1. Tiletype != wand
# 2. Tiletype != navi
# 3. Tiletype != last_position
# 4. Tile ist in self.queue
for tile in surrounding_tiles:
if not tile:
return False
tile_type = self.map.get_tile_type(tile.position)
print(tile.position, tile_type)
if not tile_type == "wall" and not tile_type == "navi" and not tile.position == last_position:
return tile
print("Sackgasse?")
return False
# if tile_surround.position == self.position_finish:
# print("FINISH")
# print("Routenlänge: ",len(self.route))
def get_estimated_cost_to_finish(self, position):
""" Liefert die estimated cost an gegebener Position zurück."""
distance_to_point = float(
sqrt((position[0] - self.position_finish[0])**2 +
(position[1] - self.position_finish[1])**2))
return distance_to_point
def get_pre_tile(self, tile):
""" Liefert den Vorgänger zurück """
# print('get_pre_tile()')
surrounding_tiles = self.get_surrounding_tiles(tile.position,
order='start')
# print('surrounding_tiles: ', surrounding_tiles)
pre_tile = surrounding_tiles.get_and_delete()
# print('pre_tile: ', pre_tile)
return pre_tile.position
def get_surrounding_tiles(self, position, order='finish'):
""" Liefert eine Queue der angrenzenden Tiles zurück."""
tiles = PriorityQueue(order)
# print('Order: ', order)
# self.surroundings
for surround in self.surroundings:
# Ränder abfragen
# y unten
if position[0] == len(self.tiles) - 1 and surround[0] == +1:
continue
# y oben
if position[0] == 0 and surround[0] == -1:
continue
# x rechts
if position[1] == len(self.tiles[0]) - 1 and surround[1] == +1:
continue
# x links
if position[1] == 0 and surround[1] == -1:
continue
x = position[1] + surround[1]
y = position[0] + surround[0]
tiles.insert(self.tiles[y][x])
# Wenn Position am unteren Rande der y-Achse ist
# tiles.sort(key=lambda x: x.estimated_cost_to_finish, reverse=False)
return tiles
def show_open_list(self):
for item in self.open_list.queue:
print(item.position, item.get_estimated_cost_to_finish())
def get_open_list(self):
return self.open_list.queue
def get_closed_list(self):
return self.closed_list.queue
def show_closed_list(self):
for item in self.closed_list.queue:
print(item.position)
def get_finish_tile(self):
return self.tiles[self.position_finish[0]][self.position_finish[1]]
item = QueueItem("A", 1)
assert (item is not None)
assert (hasattr(item, "content"))
assert (hasattr(item, "priority"))
assert (item.content == "A")
assert (item.priority == 1)
q = PriorityQueue()
assert (q is not None)
assert (hasattr(q, "items"))
assert (hasattr(q, "insert"))
assert (hasattr(q, "delete"))
assert (hasattr(q, "is_empty"))
# A,1 B,2 C,3 D,4 E,5, F,6, G,7
q.insert("C", 3)
assert (len(q.items) == 1)
assert (q.items[0].priority == 3)
assert (q.items[0].content == "C")
assert (str(q) == "(C,3)")
item = q.delete()
assert (item.priority == 3)
assert (item.content == "C")
assert (len(q.items) == 0)
assert (str(q) == "<<E>>")
q.insert("C", 3)
assert (len(q.items) == 1)
assert (q.items[0].priority == 3)
assert (q.items[0].content == "C")
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()))
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
def find(mapdata, width, height, start, end):
""" mapdata is a one-dimensional list of values, start and end are vectors of size 2 """
# WRITE THIS FUNCTION
open = PriorityQueue()
closed = []
curTile = MapTile(start, None, None, None, None)
print(width, height, start, end)
while curTile.coords != end:
if onMap(curTile.coords, width, height):
n = north(curTile.coords)
nter = terraintype(mapdata, width, height, n)
ntile = MapTile(n, LAT_COST, mandistance(n, end), nter, curTile)
if nter and (ntile not in closed):
print(ntile)
open.insert(ntile)
s = south(curTile.coords)
ster = terraintype(mapdata, width, height, s)
stile = MapTile(s, LAT_COST, mandistance(s, end), ster, curTile)
if ster and (stile not in closed):
print(stile)
open.insert(stile)
e = east(curTile.coords)
eter = terraintype(mapdata, width, height, e)
etile = MapTile(e, LAT_COST, mandistance(e, end), eter, curTile)
if eter and (etile not in closed):
print(etile)
open.insert(etile)
w = west(curTile.coords)
wter = terraintype(mapdata, width, height, w)
wtile = MapTile(w, LAT_COST, mandistance(w, end), wter, curTile)
if wter and (wtile not in closed):
print(wtile)
open.insert(wtile)
nw = northwest(curTile.coords)
nwter = terraintype(mapdata, width, height, nw)
nwtile = MapTile(nw, DIAG_COST, mandistance(nw, end), nwter, curTile)
if nwter and (nwtile not in closed):
print(nwtile)
open.insert(nwtile)
ne = northeast(curTile.coords)
neter = terraintype(mapdata, width, height, ne)
netile = MapTile(ne, DIAG_COST, mandistance(ne, end), neter, curTile)
if neter and (netile not in closed):
print(netile)
open.insert(netile)
sw = southwest(curTile.coords)
swter = terraintype(mapdata, width, height, sw)
swtile = MapTile(sw, DIAG_COST, mandistance(sw, end), swter, curTile)
if swter and (swtile not in closed):
print(swtile)
open.insert(swtile)
se = southeast(curTile.coords)
seter = terraintype(mapdata, width, height, se)
setile = MapTile(se, DIAG_COST, mandistance(se, end), seter, curTile)
if seter and (setile not in closed):
print(setile)
open.insert(setile)
closed.append(curTile)
print(open)
curTile = open.remove()
path = []
if curTile.coords == end:
while curTile.parent is not None:
path.append(curTile.parent)
curTile = curTile.parent
print(path)