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]))
