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)