def initialize(points, robotsS, robotV): plt.close('all') t = terrain(points) spList = construct_G(t) s = len(robotsS) vn = spList[0].x.shape[0] Q = [[],[],[],[]] # For each vertex of G intialize a cost array to hold cost for each robot to get to vertex, a parent array to hold previous vertex for each robot, and a local-heap of robots that have not yet reached the vertex for sn, sp in enumerate(spList): sp.costs = np.empty([sp.x.shape[0],s]) sp.parent = np.empty([sp.x.shape[0],s]) sp.localheap = [] for i in range(sp.x.shape[0]): sp.localheap.append([]) # Set each cost initially to infinity and each parent to nan for v in range(sp.x.shape[0]): for i in range(s): sp.costs[v,i] = float("inf") sp.parent[v,i] = float("nan") sp.localheap[v].append(i) # Add vertex, robot-indexed, to min-heap Q Q[0].append(sn) Q[1].append(v) Q[2].append(float("inf")) Q[3].append(i) # Mark each robot's starting vertex as O cost and update min-heap Q accordingly for i,r in enumerate(robotsS): sp = spList[r] sp.costs[robotV[i],i] = 0 minCostInd = i sp.localheap[robotV[i]] = sp.costs[robotV[i]].argsort() Q[2][(vn*r + robotV[i])*s+i] = 0 return spList, Q
def demo(points): t = terrain(points) construct_G(t)