Python - speed up pathfinding

Question

This is my pathfinding function:

def get_distance(x1,y1,x2,y2):
    neighbors = [(-1,0),(1,0),(0,-1),(0,1)]
    old_nodes = [(square_pos[x1,y1],0)]
    new_nodes = []
    for i in range(50):
        for node in old_nodes:
            if node[0].x == x2 and node[0].y == y2:
                return node[1]
            for neighbor in neighbors:
                try:
                    square = square_pos[node[0].x+neighbor[0],node[0].y+neighbor[1]]
                    if square.lightcycle == None:
                        new_nodes.append((square,node[1]))
                except KeyError:
                    pass
        old_nodes = []
        old_nodes = list(new_nodes)
        new_nodes = []
        nodes = []
    return 50

The problem is that the AI takes to long to respond( response time <= 100ms) This is just a python way of doing https://en.wikipedia.org/wiki/Pathfinding#Sample_algorithm

Answer 1

The biggest issue with your code is that you don't do anything to avoid the same coordinates being visited multiple times. This means that the number of nodes you visit is guaranteed to grow exponentially, since it can keep going back and forth over the first few nodes many times.

The best way to avoid duplication is to maintain a set of the coordinates we've added to the queue (though if your node values are hashable, you might be able to add them directly to the set instead of coordinate tuples). Since we're doing a breadth-first search, we'll always reach a given coordinate by (one of) the shortest path(s), so we never need to worry about finding a better route later on.

Try something like this:

def get_distance(x1,y1,x2,y2):
    neighbors = [(-1,0),(1,0),(0,-1),(0,1)]
    nodes = [(square_pos[x1,y1],0)]
    seen = set([(x1, y1)])
    for node, path_length in nodes:
        if path_length == 50:
            break
        if node.x == x2 and node.y == y2:
            return path_length
        for nx, ny in neighbors:
            try:
                square = square_pos[node.x + nx, node.y + ny]
                if square.lightcycle == None and (square.x, square.y) not in seen:
                    nodes.append((square, path_length + 1))
                    seen.add((square.x, square.y))
            except KeyError:
                pass
    return 50

I've also simplified the loop a bit. Rather than switching out the list after each depth, you can just use one loop and add to its end as you're iterating over the earlier values. I still abort if a path hasn't been found with fewer than 50 steps (using the distance stored in the 2-tuple, rather than the number of passes of the outer loop). A further improvement might be to use a collections.dequeue for the queue, since you could efficiently pop from one end while appending to the other end. It probably won't make a huge difference, but might avoid a little bit of memory usage.

I also avoided most of the indexing by one and zero in favor of unpacking into separate variable names in the for loops. I think this is much easier to read, and it avoids confusion since the two different kinds of 2-tuples had had different meanings (one is a node, distance tuple, the other is x, y).

Answer 2

One reasonably fast solution is to implement the Dijkstra algorithm (that I have already implemented in that question):

Build the original map. It's a masked array where the walker cannot walk on masked element:

%pylab inline
map_size = (20,20)
MAP = np.ma.masked_array(np.zeros(map_size), np.random.choice([0,1], size=map_size))
matshow(MAP)

Below is the Dijkstra algorithm:

def dijkstra(V):
    mask = V.mask
    visit_mask = mask.copy() # mask visited cells
    m = numpy.ones_like(V) * numpy.inf
    connectivity = [(i,j) for i in [-1, 0, 1] for j in [-1, 0, 1] if (not (i == j == 0))]
    cc = unravel_index(V.argmin(), m.shape) # current_cell
    m[cc] = 0
    P = {}  # dictionary of predecessors 
    #while (~visit_mask).sum() > 0:
    for _ in range(V.size):
        #print cc
        neighbors = [tuple(e) for e in asarray(cc) - connectivity 
                     if e[0] > 0 and e[1] > 0 and e[0] < V.shape[0] and e[1] < V.shape[1]]
        neighbors = [ e for e in neighbors if not visit_mask[e] ]
        tentative_distance = [(V[e]-V[cc])**2 for e in neighbors]
        for i,e in enumerate(neighbors):
            d = tentative_distance[i] + m[cc]
            if d < m[e]:
                m[e] = d
                P[e] = cc
        visit_mask[cc] = True
        m_mask = ma.masked_array(m, visit_mask)
        cc = unravel_index(m_mask.argmin(), m.shape)
    return m, P

def shortestPath(start, end, P):
    Path = []
    step = end
    while 1:
        Path.append(step)
        if step == start: break
        if P.has_key(step):
            step = P[step]
        else:
            break
    Path.reverse()
    return asarray(Path)

And the result:

start = (2,8)
stop = (17,19)
D, P = dijkstra(MAP)
path = shortestPath(start, stop, P)
imshow(MAP, interpolation='nearest')
plot(path[:,1], path[:,0], 'ro-', linewidth=2.5)

Below some timing statistics:

%timeit dijkstra(MAP)
#10 loops, best of 3: 32.6 ms per loop

Answer 3

You should replace your algorithm with A*-search with the Manhattan distance as a heuristic.