def updatecost(self, range_changed=None, new=None, old=None, mode=False):
# scan graph for changed cost, if cost is changed update it
CHANGED = set()
for xi in self.CLOSED:
if xi in self.CHILDREN:
oldchildren = self.CHILDREN[xi] # A
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
newchildren = set(children(self, xi)) # B
removed = oldchildren.difference(newchildren)
intersection = oldchildren.intersection(newchildren)
added = newchildren.difference(oldchildren)
self.CHILDREN[xi] = newchildren
for xj in removed:
self.COST[xi][xj] = cost(self, xi, xj)
for xj in intersection.union(added):
self.COST[xi][xj] = cost(self, xi, xj)
CHANGED.add(xi)
else:
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
CHANGED.add(xi)
children_added = set(children(self, xi))
self.CHILDREN[xi] = children_added
for xj in children_added:
self.COST[xi][xj] = cost(self, xi, xj)
return CHANGED
def updatecost(self, range_changed=None, new=None, old=None, mode=False):
# scan graph for changed Cost, if Cost is changed update it
CHANGED = set()
for xi in self.CLOSED:
if isinbound(old, xi, mode) or isinbound(new, xi, mode):
newchildren = set(children(self, xi)) # B
self.CHILDREN[xi] = newchildren
for xj in newchildren:
self.COST[xi][xj] = cost(self, xi, xj)
CHANGED.add(xi)
return CHANGED
def updatecost(self, range_changed=None):
# TODO: update cost when the environment is changed
# chaged nodes
CHANGED = set()
for xi in self.CLOSED:
oldchildren = self.CHILDREN[xi] # A
# if you don't know where the change occured:
if range_changed is None:
newchildren = set(children(self, xi)) # B
added = newchildren.difference(oldchildren) # B-A
removed = oldchildren.difference(newchildren) # A-B
self.CHILDREN[xi] = newchildren
if added or removed:
CHANGED.add(xi)
for xj in removed:
self.COST[xi][xj] = cost(self, xi, xj)
for xj in added:
self.COST[xi][xj] = cost(self, xi, xj)
# if you do know where on the map changed, only update those changed around that area
else:
if isinbound(range_changed, xi):
newchildren = set(children(self, xi)) # B
added = newchildren.difference(oldchildren) # B-A
removed = oldchildren.difference(newchildren) # A-B
self.CHILDREN[xi] = newchildren
if added or removed:
CHANGED.add(xi)
for xj in removed:
self.COST[xi][xj] = cost(self, xi, xj)
for xj in added:
self.COST[xi][xj] = cost(self, xi, xj)
return CHANGED
def children(self, x):
allchild = []
resolution = self.env.resolution
for direc in self.Alldirec:
child = tuple(map(np.add, x, np.multiply(direc, resolution)))
if isinbound(self.env.boundary, child):
allchild.append(child)
return allchild
def isCollide(self, x, child):
ray, dist = getRay(x, child), getDist(x, child)
if not isinbound(self.env.boundary, child):
return True, dist
for i in self.env.AABB_pyrr:
shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
if shot is not None:
dist_wall = getDist(x, shot)
if dist_wall <= dist: # collide
return True, dist
for i in self.env.balls:
if isinball(i, child):
return True, dist
shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i)
if shot != []:
dists_ball = [getDist(x, j) for j in shot]
if all(dists_ball <= dist): # collide
return True, dist
return False, dist
def isinobs(self, obs, x, mode):
if mode == 'obb':
return isinobb(obs, x)
elif mode == 'aabb':
return isinbound(obs, x, mode)
