def cdecomp(region, obsts): # Obstacles in region robsts = inters_to_union(region, obsts) # Region and obstacle vertices, sorted, no repetitions vs = np.vstack([cdd.vrep_pts(obs) for obs in robsts + [region]]) vs = vs[vs[:,0].argsort()] vs = vs[~np.isclose(np.r_[1, np.diff(vs, axis=0)[:,0]], 0)] # Cilinder lines, y-axis aligned (dimension index 1) lines = [inters(region, Polytope(np.array([[-v[0], 1, 0], [v[0], -1, 0]]))) for v in vs] free = [] for i in range(len(lines) - 1): # Cilinder cil = conv(lines[i:i+2]) # Obstacles in cilinder cobsts = inters_to_union(cil, obsts) # Exteriors of the obstacles in the cilinder exts = [exterior(obs, cil) for obs in cobsts if len(obs.lin_set) == 0] if len(exts) == 0: # Cilinder is all free space free.append(cil) elif len(exts) == 1: # Only one obstacle contributing free.extend(exts[0]) else: # Intersections of all combinations of exteriors frees = [inters(*tup) for tup in it.product(*exts)] # Avoid including lines or points (pfulldim) free.extend(x for x in frees if cdd.pfulldim(x)) return free
def contains(self, x): if isinstance(x, Polytope): vrep = cdd.vrep_pts(x) if vrep.shape == (2, 2): z = vrep[0] - vrep[1] w = vrep[1] - self.v # logger.debug(vrep) # logger.debug(z) # logger.debug(w) # logger.debug(self.A) return (z.dot(self.A).dot(w))**2 >= \ (z.dot(self.A).dot(z))*(w.dot(self.A).dot(w) - 1) else: raise Exception("Not implemented") elif x.shape == (2,): return (x[0] - self.v[0])**2 / self.a2 + \ (x[1] - self.v[1])**2 / self.b2 <= 1 elif len(x.shape) > 1 and x.shape[1] == 2: return (x[:,0] - self.v[0])**2 / self.a2 + \ (x[:,1] - self.v[1])**2 / self.b2 <= 1 else: return False
def plot_poly(ax, poly): vs = cdd.vrep_pts(poly) c = centroid(vs) vs = sorted(vs, key=lambda p: math.atan2(p[1]-c[1],p[0]-c[0])) ax.add_patch(patches.Polygon(vs, facecolor="red"))
def conv(pols): return conv_pts(np.vstack([cdd.vrep_pts(p) for p in pols]))