def reset_navgraph(self):
''' Create and store a new nav graph for this box world configuration.
The graph is build by adding NavNode to the graph for each of the
boxes in box world. Then edges are created (4-sided).
'''
self.path = None
# invalid so remove if present.
self.graph = SparseGraph()
# Set a heuristic cost function for the search to use.
self.graph.cost_h = self._manhattan
#self.graph.cost_h = self._hypot.
#self.graph.cost_h = self._max.
nx, ny = self.nx, self.ny
# add all the nodes required.
for i, box in enumerate(self.boxes):
box.pos = (i % nx, i // nx)
#tuple position.
box.node = self.graph.add_node(Node(idx=i))
# build all the edges required for this world.
for i, box in enumerate(self.boxes):
# four sided N-S-E-W connections.
if box.kind in no_edge:
continue
# UP (i + nx).
if (i + nx) < len(self.boxes):
self._add_edge(i, i + nx)
# DOWN (i - nx).
if (i - nx) >= 0:
self._add_edge(i, i - nx)
# RIGHT (i + 1).
if (i % nx + 1) < nx:
self._add_edge(i, i + 1)
# LEFT (i - 1).
if (i % nx - 1) >= 0:
self._add_edge(i, i - 1)
def reset_navgraph(self):
''' Create and store a new nav graph for this box world configuration.
The graph is build by adding NavNode to the graph for each of the
boxes in box world. Then edges are created (4-sided).
'''
self.path = None # invalid so remove if present
self.graph = SparseGraph()
# Set a heuristic cost function for the search to use
self.graph.cost_h = self._manhattan
#self.graph.cost_h = self._hypot
#self.graph.cost_h = self._max
nx, ny = self.nx, self.ny
# add all the nodes required
for i, box in enumerate(self.boxes):
box.pos = (i % nx, i // nx) #tuple position
box.node = self.graph.add_node(Node(idx=i))
# build all the edges required for this world
for i, box in enumerate(self.boxes):
# four sided N-S-E-W connections
if box.kind in no_edge:
continue
# UP (i + nx)
if (i+nx) < len(self.boxes):
self._add_edge(i, i+nx)
# DOWN (i - nx)
if (i-nx) >= 0:
self._add_edge(i, i-nx)
# RIGHT (i + 1)
if (i%nx + 1) < nx:
self._add_edge(i, i+1)
# LEFT (i - 1)
if (i%nx - 1) >= 0:
self._add_edge(i, i-1)
# # Diagonal connections
# # UP LEFT(i + nx - 1)
j = i + nx
if (j-1) < len(self.boxes) and (j%nx - 1) >= 0:
self._add_edge(i, j-1, 1.4142) # sqrt(1+1)
# # UP RIGHT (i + nx + 1)
j = i + nx
if (j+1) < len(self.boxes) and (j%nx + 1) < nx:
self._add_edge(i, j+1, 1.4142)
# # DOWN LEFT(i - nx - 1)
j = i - nx
if (j-1) >= 0 and (j%nx - 1) >= 0:
print i, j, j%nx
self._add_edge(i, j-1, 1.4142)
# # DOWN RIGHT (i - nx + 1)
j = i - nx
if (j+1) >= 0 and (j%nx +1) < nx:
self._add_edge(i, j+1, 1.4142)
class BoxWorld(object):
'''A world made up of boxes. '''
def __init__(self, nx, ny, cx, cy):
self.boxes = [None] * nx * ny
self.nx, self.ny = nx, ny # number of box (squares)
for i in range(len(self.boxes)):
self.boxes[i] = Box()
self.boxes[i].idx = i
# use resize to set all the positions correctly
self.cx = self.cy = self.wx = self.wy = None
self.resize(cx, cy)
# create nav_graph
self.path = None
self.graph = None
self.reset_navgraph()
self.start = None
self.target = None
self.agent_pos = None
self.items_and_points_done = False
def get_box_by_index(self, ix, iy):
idx = (self.nx * iy) + ix
return self.boxes[idx] if idx < len(self.boxes) else None
def get_box_by_pos(self, x, y):
idx = (self.nx * (y // self.wy)) + (x // self.wx)
return self.boxes[idx] if idx < len(self.boxes) else None
def update(self, delta):
pass
def draw(self, agent_pos):
for box in self.boxes:
box.draw()
if cfg['EDGES_ON']:
egi.set_pen_color(name='LIGHT_BLUE')
for node, edges in self.graph.edgelist.items():
# print node, edges
for dest in edges:
egi.line_by_pos(self.boxes[node]._vc, self.boxes[dest]._vc)
if self.path:
# put a circle in the visited boxes?
if cfg['BOXUSED_ON']:
egi.set_pen_color(name="GREEN")
for i in self.path.closed:
egi.circle(self.boxes[i]._vc, 10)
if cfg['TREE_ON']:
egi.set_stroke(3)
# Show open edges
route = self.path.route
egi.set_pen_color(name='GREEN')
for i in self.path.open_nodes:
egi.circle(self.boxes[i]._vc, 10)
# show the partial paths considered
egi.set_pen_color(name='ORANGE')
for i, j in route.items():
egi.line_by_pos(self.boxes[i]._vc, self.boxes[j]._vc)
egi.set_stroke(1)
if cfg['PATH_ON']:
# show the final path delivered
egi.set_pen_color(name='RED')
egi.set_stroke(2)
path = self.path.path
for i in range(1, len(path)):
egi.line_by_pos(self.boxes[path[i - 1]]._vc,
self.boxes[path[i]]._vc)
egi.set_stroke(1)
if agent_pos:
egi.set_stroke(4)
egi.set_pen_color(name="BLUE")
egi.circle(agent_pos, 10)
def resize(self, cx, cy):
self.cx, self.cy = cx, cy # world size
self.wx = (cx - 1) // self.nx
self.wy = (cy - 1) // self.ny # int div - box width/height
for i in range(len(self.boxes)):
# basic positions (bottom left to top right)
x = (i % self.nx) * self.wx
y = (i // self.nx) * self.wy
# top, right, bottom, left
coords = (y + self.wy - 1, x + self.wx - 1, y, x)
self.boxes[i].reposition(coords)
def _add_edge(self, from_idx, to_idx, distance=1.0):
b = self.boxes
if b[to_idx].kind not in no_edge: # stone wall
cost = edge_cost(b[from_idx].kind, b[to_idx].kind)
self.graph.add_edge(Edge(from_idx, to_idx, cost * distance))
def _manhattan(self, idx1, idx2):
''' Manhattan distance between two nodes in boxworld, assuming the
minimal edge cost so that we don't overestimate the cost). '''
x1, y1 = self.boxes[idx1].pos
x2, y2 = self.boxes[idx2].pos
return (abs(x1 - x2) + abs(y1 - y2)) * min_edge_cost
def _hypot(self, idx1, idx2):
'''Return the straight line distance between two points on a 2-D
Cartesian plane. Argh, Pythagoras... trouble maker. '''
x1, y1 = self.boxes[idx1].pos
x2, y2 = self.boxes[idx2].pos
return hypot(x1 - x2, y1 - y2) * min_edge_cost
def _max(self, idx1, idx2):
'''Return the straight line distance between two points on a 2-D
Cartesian plane. Argh, Pythagoras... trouble maker. '''
x1, y1 = self.boxes[idx1].pos
x2, y2 = self.boxes[idx2].pos
return max(abs(x1 - x2), abs(y1 - y2)) * min_edge_cost
def reset_navgraph(self):
''' Create and store a new nav graph for this box world configuration.
The graph is build by adding NavNode to the graph for each of the
boxes in box world. Then edges are created (4-sided).
'''
self.path = None # invalid so remove if present
self.graph = SparseGraph()
# Set a heuristic cost function for the search to use
#self.graph.cost_h = self._manhattan
self.graph.cost_h = self._hypot
#self.graph.cost_h = self._max
nx, ny = self.nx, self.ny
# add all the nodes required
for i, box in enumerate(self.boxes):
box.pos = (i % nx, i // nx) #tuple position
box.node = self.graph.add_node(Node(idx=i))
# build all the edges required for this world
for i, box in enumerate(self.boxes):
# four sided N-S-E-W connections
if box.kind in no_edge:
continue
# UP (i + nx)
if (i + nx) < len(self.boxes):
self._add_edge(i, i + nx)
# DOWN (i - nx)
if (i - nx) >= 0:
self._add_edge(i, i - nx)
# RIGHT (i + 1)
if (i % nx + 1) < nx:
self._add_edge(i, i + 1)
# LEFT (i - 1)
if (i % nx - 1) >= 0:
self._add_edge(i, i - 1)
# # Diagonal connections
# # UP LEFT(i + nx - 1)
j = i + nx
if (j - 1) < len(self.boxes) and (j % nx - 1) >= 0:
self._add_edge(i, j - 1, 1.4142) # sqrt(1+1)
# # UP RIGHT (i + nx + 1)
j = i + nx
if (j + 1) < len(self.boxes) and (j % nx + 1) < nx:
self._add_edge(i, j + 1, 1.4142)
# # DOWN LEFT(i - nx - 1)
j = i - nx
if (j - 1) >= 0 and (j % nx - 1) >= 0:
print(i, j, j % nx)
self._add_edge(i, j - 1, 1.4142)
# # DOWN RIGHT (i - nx + 1)
j = i - nx
if (j + 1) >= 0 and (j % nx + 1) < nx:
self._add_edge(i, j + 1, 1.4142)
def set_start(self, idx):
'''Set the start box based on its index idx value. '''
# remove any existing start node, set new start node
if self.target == self.boxes[idx]:
print("Can't have the same start and end boxes!")
return
if self.start:
self.start.marker = None
self.start = self.boxes[idx]
self.start.marker = 'S'
def set_target(self, idx):
'''Set the target box based on its index idx value. '''
# remove any existing target node, set new target node
if self.start == self.boxes[idx]:
print("Can't have the same start and end boxes!")
return
if self.target is not None:
self.target.marker = None
self.target = self.boxes[idx]
self.target.marker = 'T'
def plan_path(self, search, limit):
'''Conduct a nav-graph search from the current world start node to the
current target node, using a search method that matches the string
specified in `search`.
'''
cls = SEARCHES[search]
self.path = cls(self.graph, self.start.idx, self.target.idx,
self.item_idx, self.point_idx, limit)
@classmethod
def FromFile(cls, filename, pixels=(500, 500)):
'''Support a the construction of a BoxWorld map from a simple text file.
See the module doc details at the top of this file for format details.
'''
# open and read the file
f = open(filename)
lines = []
for line in f.readlines():
line = line.strip()
if line and not line.startswith('#'):
lines.append(line)
f.close()
# first line is the number of boxes width, height
nx, ny = [int(bit) for bit in lines.pop(0).split()]
# Create a new BoxWorld to store all the new boxes in...
cx, cy = pixels
world = BoxWorld(nx, ny, cx, cy)
# Get and set the Start and Target tiles
s_idx, t_idx = [int(bit) for bit in lines.pop(0).split()]
world.set_start(s_idx)
world.set_target(t_idx)
# Ready to process each line
assert len(lines) == ny, "Number of rows doesn't match data."
# read each line
idx = 0
for line in reversed(lines): # in reverse order
bits = line.split()
assert len(bits) == nx, "Number of columns doesn't match data."
for bit in bits:
bit = bit.strip()
assert bit in box_kind, "Not a known box type: " + bit
world.boxes[idx].set_kind(bit)
idx += 1
return world
def items_and_points(self):
self.items_and_points_done = True
clear_boxes = []
for box in self.boxes:
if box.kind == '.':
clear_boxes.append(box)
box_change = clear_boxes[randrange(len(clear_boxes))]
box_selected = next(
(box for box in self.boxes if box.pos == box_change.pos), None)
box_selected.kind = 'I'
self.item_idx = box_selected.idx
self.item_found = False
clear_boxes.remove(box_change)
box_change = clear_boxes[randrange(len(clear_boxes))]
box_selected = next(
(box for box in self.boxes if box.pos == box_change.pos), None)
box_selected.kind = 'P'
self.point_idx = box_selected.idx
self.point_found = False
class BoxWorld(object):
'''A world made up of boxes. '''
def __init__(self, nx, ny, cx, cy):
self.boxes = [None]*nx*ny
self.nx, self.ny = nx, ny # number of box (squares)
for i in range(len(self.boxes)):
self.boxes[i] = Box()
self.boxes[i].idx = i
# use resize to set all the positions correctly
self.cx = self.cy = self.wx = self.wy = None
self.resize(cx, cy)
# create nav_graph
self.path = None
self.graph = None
self.reset_navgraph()
self.start = None
self.target = None
def get_box_by_index(self, ix, iy):
idx = (self.nx * iy) + ix
return self.boxes[idx] if idx < len(self.boxes) else None
def get_box_by_pos(self, x, y):
idx = (self.nx * (y // self.wy)) + (x // self.wx)
return self.boxes[idx] if idx < len(self.boxes) else None
def update(self, delta):
pass
def draw(self):
for box in self.boxes:
box.draw()
if cfg['EDGES_ON']:
egi.set_pen_color(name='LIGHT_BLUE')
for node, edges in self.graph.edgelist.items():
# print node, edges
for dest in edges:
egi.line_by_pos(self.boxes[node]._vc, self.boxes[dest]._vc)
if self.path:
# put a circle in the visited boxes?
if cfg['BOXUSED_ON']:
egi.set_pen_color(name="GREEN")
for i in self.path.closed:
egi.circle(self.boxes[i]._vc, 10)
if cfg['TREE_ON']:
egi.set_stroke(3)
# Show open edges
route = self.path.route
egi.set_pen_color(name='GREEN')
for i in self.path.open:
egi.circle(self.boxes[i]._vc, 10)
# show the partial paths considered
egi.set_pen_color(name='ORANGE')
for i,j in route.items():
egi.line_by_pos(self.boxes[i]._vc, self.boxes[j]._vc)
egi.set_stroke(1)
if cfg['PATH_ON']:
# show the final path delivered
egi.set_pen_color(name='RED')
egi.set_stroke(2)
path = self.path.path
for i in range(1,len(path)):
egi.line_by_pos(self.boxes[path[i-1]]._vc, self.boxes[path[i]]._vc)
egi.set_stroke(1)
def resize(self, cx, cy):
self.cx, self.cy = cx, cy # world size
self.wx = (cx-1) // self.nx
self.wy = (cy-1) // self.ny # int div - box width/height
for i in range(len(self.boxes)):
# basic positions (bottom left to top right)
x = (i % self.nx) * self.wx
y = (i // self.nx) * self.wy
# top, right, bottom, left
coords = (y + self.wy -1, x + self.wx -1, y, x)
self.boxes[i].reposition(coords)
def _add_edge(self, from_idx, to_idx, distance=1.0):
b = self.boxes
if b[to_idx].kind not in no_edge: # stone wall
cost = edge_cost(b[from_idx].kind, b[to_idx].kind)
self.graph.add_edge(Edge(from_idx, to_idx, cost*distance))
def _manhattan(self, idx1, idx2):
''' Manhattan distance between two nodes in boxworld, assuming the
minimal edge cost so that we don't overestimate the cost). '''
x1, y1 = self.boxes[idx1].pos
x2, y2 = self.boxes[idx2].pos
return (abs(x1-x2) + abs(y1-y2)) * min_edge_cost
def _hypot(self, idx1, idx2):
'''Return the straight line distance between two points on a 2-D
Cartesian plane. Argh, Pythagoras... trouble maker. '''
x1, y1 = self.boxes[idx1].pos
x2, y2 = self.boxes[idx2].pos
return hypot(x1-x2, y1-y2) * min_edge_cost
def _max(self, idx1, idx2):
'''Return the straight line distance between two points on a 2-D
Cartesian plane. Argh, Pythagoras... trouble maker. '''
x1, y1 = self.boxes[idx1].pos
x2, y2 = self.boxes[idx2].pos
return max(abs(x1-x2),abs(y1-y2)) * min_edge_cost
def reset_navgraph(self):
''' Create and store a new nav graph for this box world configuration.
The graph is build by adding NavNode to the graph for each of the
boxes in box world. Then edges are created (4-sided).
'''
self.path = None # invalid so remove if present
self.graph = SparseGraph()
# Set a heuristic cost function for the search to use
self.graph.cost_h = self._manhattan
#self.graph.cost_h = self._hypot
#self.graph.cost_h = self._max
nx, ny = self.nx, self.ny
# add all the nodes required
for i, box in enumerate(self.boxes):
box.pos = (i % nx, i // nx) #tuple position
box.node = self.graph.add_node(Node(idx=i))
# build all the edges required for this world
for i, box in enumerate(self.boxes):
# four sided N-S-E-W connections
if box.kind in no_edge:
continue
# UP (i + nx)
if (i+nx) < len(self.boxes):
self._add_edge(i, i+nx)
# DOWN (i - nx)
if (i-nx) >= 0:
self._add_edge(i, i-nx)
# RIGHT (i + 1)
if (i%nx + 1) < nx:
self._add_edge(i, i+1)
# LEFT (i - 1)
if (i%nx - 1) >= 0:
self._add_edge(i, i-1)
# # Diagonal connections
# # UP LEFT(i + nx - 1)
j = i + nx
if (j-1) < len(self.boxes) and (j%nx - 1) >= 0:
self._add_edge(i, j-1, 1.4142) # sqrt(1+1)
# # UP RIGHT (i + nx + 1)
j = i + nx
if (j+1) < len(self.boxes) and (j%nx + 1) < nx:
self._add_edge(i, j+1, 1.4142)
# # DOWN LEFT(i - nx - 1)
j = i - nx
if (j-1) >= 0 and (j%nx - 1) >= 0:
print i, j, j%nx
self._add_edge(i, j-1, 1.4142)
# # DOWN RIGHT (i - nx + 1)
j = i - nx
if (j+1) >= 0 and (j%nx +1) < nx:
self._add_edge(i, j+1, 1.4142)
def set_start(self, idx):
'''Set the start box based on its index idx value. '''
# remove any existing start node, set new start node
if self.target == self.boxes[idx]:
print("Can't have the same start and end boxes!")
return
if self.start:
self.start.marker = None
self.start = self.boxes[idx]
self.start.marker = 'S'
def set_target(self, idx):
'''Set the target box based on its index idx value. '''
# remove any existing target node, set new target node
if self.start == self.boxes[idx]:
print("Can't have the same start and end boxes!")
return
if self.target is not None:
self.target.marker = None
self.target = self.boxes[idx]
self.target.marker = 'T'
def plan_path(self, search, limit):
'''Conduct a nav-graph search from the current world start node to the
current target node, using a search method that matches the string
specified in `search`.
'''
cls = SEARCHES[search]
self.path = cls(self.graph, self.start.idx, self.target.idx, limit)
@classmethod
def FromFile(cls, filename, pixels=(500,500) ):
'''Support a the construction of a BoxWorld map from a simple text file.
See the module doc details at the top of this file for format details.
'''
# open and read the file
f = file(filename)
lines = []
for line in f.readlines():
line = line.strip()
if line and not line.startswith('#'):
lines.append(line)
f.close()
# first line is the number of boxes width, height
nx, ny = [int(bit) for bit in lines.pop(0).split()]
# Create a new BoxWorld to store all the new boxes in...
cx, cy = pixels
world = BoxWorld(nx, ny, cx, cy)
# Get and set the Start and Target tiles
s_idx, t_idx = [int(bit) for bit in lines.pop(0).split()]
world.set_start(s_idx)
world.set_target(t_idx)
# Ready to process each line
assert len(lines) == ny, "Number of rows doesn't match data."
# read each line
idx = 0
for line in reversed(lines): # in reverse order
bits = line.split()
assert len(bits) == nx, "Number of columns doesn't match data."
for bit in bits:
bit = bit.strip()
assert bit in box_kind, "Not a known box type: "+bit
world.boxes[idx].set_kind(bit)
idx += 1
return world