Richard Jones' Log: Solving Game Entity Navigation Using Python

Mon, 13 Sep 2010

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.