# Richard Jones' Log

Mon, 13 Sep 2010
Solving Game Entity Navigation Using Python

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:

1. dividing the play field in quarter-sized nodes,
2. look to see whether there are any blockers present in the new nodes,
3. if there are:
1. if the node size is larger than some minimum size we subdivide into quarters again as per step 1, else
2. 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.y)        all.sort(key=lambda x:x.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.x)            all.sort(key=lambda x:x.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            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:

1. for each pair of lines in the path:
2. join their ultimate endpoints to create a new line,
3. determine whether the new line crosses out of the quad tree at any point,
4. if it doesn't then replace the original lines from #1 with the new line at #2, and
5. 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.