This post describes how to generate and solve a navigation mesh - a structure used to figure out how to move an entity around a game playing field in a sensible manner (ie. not running into things).
We start with a playing field with some rectangles in it that we'll call "blockers". These block movement in some way:
Having blockers with other shapes is quite possible - you'd just need to implement the intersects() method as used below.
We generate a quad tree structure by recursively:
- dividing the play field in quarter-sized nodes,
- look to see whether there are any blockers present in the new nodes,
- if there are:
- if the node size is larger than some minimum size we subdivide into quarters again as per step 1, else
- we discard it from the tree.
Assuming a square play field, the code for this is (using the Rect base class from cocos2d for convenience):
class QuadTreeNode(Rect):
def __init__(self, x, y, size, parent, minimum_size=None):
super(QuadTreeNode, self).__init__(x, y, size, size)
self.parent = parent
self.minimum_size = minimum_size or parent.minimum_size
def check_node(self, blockers):
for blocker in blockers:
if blocker.intersects(self):
if self.width <= self.minimum_size:
return False
self.subdivide(blockers)
if not self.nodes:
# my subdivision came up empty; remove me entirely
return False
break
return True
def subdivide(self, blockers):
size = self.width // 2
# bottom left
node = QuadTreeNode(self.x, self.y, size, self)
if node.check_node(blockers):
self.nodes.append(node)
# bottom right
node = QuadTreeNode(self.x + size, self.y, size, self)
if node.check_node(blockers):
self.nodes.append(node)
# top right
node = QuadTreeNode(self.x + size, self.y + size, size, self)
if node.check_node(blockers):
self.nodes.append(node)
# top left
node = QuadTreeNode(self.x, self.y + size, size, self)
if node.check_node(blockers):
self.nodes.append(node)
class QuadTree(QuadTreeNode):
def __init__(self, x, y, size, blockers, minimum_size):
super(QuadTree, self).__init__(x, y, size, None, minimum_size)
self.check_node(blockers)
This gives us a quad tree:
To reduce the number of nodes in the tree we can collapse adjacent nodes which share the same dimension on their shared side. To keep subsequent path generation sensible (as implemented here) we restrict the aspect ratio of the nodes so they're tend to be more square. The following fairly inelegant code achieves this:
def optimise(self, limit_aspect=True):
all = self.list_nodes()
all.sort(key=lambda x:x[1].y)
all.sort(key=lambda x:x[1].x)
changed = True
for i in range(2):
while changed:
changed = False
for i, (parent_one, one) in enumerate(all):
try:
parent_two, two = all[i+1]
except IndexError:
break
if one.bottomleft == two.bottomright and one.height == two.height:
if limit_aspect and two.width > 2 * two.height:
continue
parent_one.remove(one)
del all[i]
two.width += one.width
elif two.topleft == one.bottomleft and one.width == two.width:
if limit_aspect and two.height > 2 * two.width:
continue
parent_one.remove(one)
del all[i]
two.height += one.height
elif two.bottomleft == one.bottomright and one.height == two.height:
if limit_aspect and one.width > 2 * one.height:
continue
parent_two.remove(two)
del all[i+1]
one.width += two.width
elif one.topleft == two.bottomleft and one.width == two.width:
if limit_aspect and one.height > 2 * one.width:
continue
parent_two.remove(two)
del all[i+1]
one.height += two.height
else:
continue
changed = True
break
all.sort(key=lambda x:x[1].x)
all.sort(key=lambda x:x[1].y)
changed = True
This reduces the quad tree to:
Now that we have a quad tree of nodes we can analyse it to determine the neighbor lists for the various nodes, given the addition of the various n_ variables on the QuadTreeNode class.
def find_neighbors(self):
all = self.list_nodes()
for op, one in all:
for tp, two in all:
if one is two: continue
if one.left == two.right and (one.bottom < two.top and one.top > two.bottom):
one.n_left.append(two)
elif one.right == two.left and (one.bottom < two.top and one.top > two.bottom):
one.n_right.append(two)
elif one.top == two.bottom and (one.left < two.right and one.right > two.left):
one.n_top.append(two)
elif one.bottom == two.top and (one.left < two.right and one.right > two.left):
one.n_bottom.append(two)
Finally we may now solve a path to take us from one node to another. This uses the A* algorithm (the Python code below is almost identical to the pseudocode from Wikipedia.) as a method on the QuadTree class:
def astar(self, start, goal):
# The set of nodes already evaluated.
closed = set()
# Distance from start along optimal path.
g_score = {start: 0}
# Heuristic estimate of distance to goal
h_score = {start: distance(start.center, goal.center)}
# Open nodes; estimated total distance from start to goal through y
f_score = {start: h_score[start]}
came_from = {}
def reconstruct_path(node):
if node in came_from:
return reconstruct_path(came_from[node]) + [node]
else:
return [node]
while f_score:
l = [(v, k) for k, v in f_score.items()]
l.sort()
x = l[0][1]
if x is goal:
return reconstruct_path(came_from[goal])
del f_score[x]
closed.add(x)
for y in x.neighbors():
if y in closed:
continue
tentative_g_score = g_score[x] + distance(x.center, y.center)
if y not in f_score:
tentative_is_better = True
elif tentative_g_score < g_score[y]:
tentative_is_better = True
else:
tentative_is_better = False
if tentative_is_better:
came_from[y] = x
g_score[y] = tentative_g_score
h_score[y] = distance(y.center, goal.center)
f_score[y] = g_score[y] + h_score[y]
raise ValueError('unable to solve map')
And finally we can create a nice wrapper method which takes an arbitrary source and destination, uses the A* code to determine the nodes to travel along and extracts the relevant edge information to create a nice path:
def find_path(self, source, destination):
# solve the path from source -> destination using A*
source = self.quadtree.cell_at(*source)
destination = self.quadtree.cell_at(*destination)
path = self.quadtree.astar(source, destination) + [destination]
# now find the best points along the path rects to use
points = [source.center]
for i, cell in enumerate(path):
try:
next = path[i+1]
except IndexError:
points.append(cell.center)
break
if cell.top == next.bottom:
if cell.width < next.width:
points.append(cell.midtop)
else:
points.append(next.midbottom)
elif cell.bottom == next.top:
if cell.width < next.width:
points.append(cell.midbottom)
else:
points.append(next.midtop)
elif cell.right == next.left:
if cell.height < next.height:
points.append(cell.midright)
else:
points.append(next.midleft)
elif cell.left == next.right:
if cell.height < next.height:
points.append(cell.midleft)
else:
points.append(next.midright)
return points
The result is a path like this:
Note that it might be simpler to just have the A* solver work using the various major points around the QuadTreeNode rectangles; that's just not how I implemented it here :-)
There is an optimisation of the path left to peform that I ran out of time to implement:
- for each pair of lines in the path:
- join their ultimate endpoints to create a new line,
- determine whether the new line crosses out of the quad tree at any point,
- if it doesn't then replace the original lines from #1 with the new line at #2, and
- repeat until no lines are replaced.
If you're interested in the full working code, including the visualisation above, feel free to grab it from my PyWeek entry "Catch the Cootie" - the interesting file is in gamelib/navigation.py.