Find the shortest path in a graph (distance and nodes)

shortest_path.py

# Shortest Path
# G = [][]
# G[i][j] is the distance from i to  j, and if G[i][j] < 0 means they are not connected.
# node ids ranging from 0 to len(G)-1
# src, dst, return the shortest distance.
# revise it so that we can get one shortest path [src, node1, node2.... dst]

G=[
[0, 2, -1 , 1, 7, -1],
[-1, 0, 3,  5, -1, -1],
[3, -1, 0,  6,  2,  -1],
[4, -1, 1, 0, 8, 1],
[2, 10, 2, -2, 0, 3],
[-1, -1, -1, 4, 6, 0]
]


def getSP(src, dst):
    queue = [[src, 0, set(), [src]]]
    cur_shortest_path_dist = None
    cur_shortest_path = None
    
    while len(queue) > 0:
        cur_node = queue.pop()

        if cur_shortest_path_dist is not None and cur_node[1] > cur_shortest_path_dist:
            continue
                
        if cur_node[0] == dst:
            cur_shortest_path_dist = cur_node[1]
            cur_shortest_path = cur_node[3]
        
        cur_node[2].add(cur_node[0])
        
        for i in xrange(len(G)):
            if i in cur_node[2] or G[cur_node[0]][i] <= 0:
                continue
            cur_path = cur_node[3] + [i]
            queue.append([i, cur_node[1] + G[cur_node[0]][i], cur_node[2], cur_path])
    
    return cur_shortest_path_dist, cur_shortest_path


assert getSP(0, 1) == (2, [0, 1])
assert getSP(1, 5) == (6, [1, 3, 5])
assert getSP(2, 5) == (5, [2, 4, 5])