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)