def compute_adjacent(geography, destination="derived/"):
"""Compute the adjacent precincts."""
with open(geography) as file:
data = geojson.load(file)
polygons = []
for i in range(len(data.features)):
n = data[i].properties["GEOID10"]
if data[i].geometry.type == "Polygon":
p = polygon.Polygon(data[i].geometry.coordinates[0])
polygons.append((p, n))
elif data[i].geometry.type == "MultiPolygon":
for coordinates in data[i].geometry.coordinates:
p = polygon.Polygon(coordinates[0])
polygons.append((p, n))
t = len(polygons)
adjacent = {n: [] for p, n in polygons}
try:
for i, (a, na) in enumerate(polygons[:-1]):
print("%i/%i %f%%" % (i, t, i / t * 100))
for (b, nb) in polygons[i + 1:]:
if a.intersects(b):
adjacent[na].append(nb)
adjacent[nb].append(na)
except:
print("cancelled at %i" % i)
name = get_file_name(geography) + ".adjacency.json"
with open(get_write_path(destination, name), "w") as file:
json.dump(adjacent, file)
def calcProportion(self, objList, roomPoints, desRatio=0.45):
"""
Till now on the basis of area not volume
calculating layout-volume-to-room-ratio
objList: List of all points of each object in room
roomVol: Room points
"""
nObj = len(objList)
objListSp = []
# Transform to shapely
for n in range(nObj):
objListSp.append(sgp.Polygon([[p.x, p.y] for p in objList[n]]))
roomSp = sgp.Polygon([p.x, p.y] for p in roomPoints)
objVolSum = 0
for i in range(len(objListSp)):
objVolSum += objListSp[i].area
roomVol = roomSp.area
gP = max(desRatio - 1.0 * objVolSum / roomVol, 0) / (1.0 * desRatio)
return gP
def grow_step(self, verbose=False):
# first and last pt are identical
pts = np.array(self.polygon.exterior.coords)[:-1]
c = self.initial_center
dr = 0.1 * self.r0
f = self.boolfunc
new_pts = pts.copy() # default stay the same
for i, p in enumerate(pts):
if self.stop_growing[i]:
continue
r_unit = (p - c) / np.linalg.norm(p - c)
# test new point
pnew = p + dr * r_unit
if np.linalg.norm(pnew - c) > self.max_radius:
self.stop_growing = [True] * self.nsides
if verbose:
print("All points stopped growing because too big")
break
elif f(pnew):
# accept point
new_pts[i] = pnew
else:
# bisect old p with pnew
# for now, define success here
self.stop_growing[i] = True
if verbose:
print("Point %i stopped growing because success" % i)
self.polygon = P.Polygon(new_pts)
# check that new side lengths are not too long
edge_len_diffs = np.asarray(
(edge_lengths(self.polygon) - self.edge_length_atol) > 0,
dtype='int')
bad_edge_ixs = np.argwhere(edge_len_diffs).T.flatten()
if len(bad_edge_ixs) > 0:
final_pts = new_pts.copy()
for ix in bad_edge_ixs:
# Vertices for edge in polygon object are ix and ix+1 which is guaranteed
# to be in range of indices because last point is repeated.
# IDEAL CASE:
# Scan along radius 1/2 way between the two vertices to find a safe point
# to start a new polygon with same trust radius as this polygon started
# with
# PRACTICAL CASE:
# Revert these indices to their previous values and stop growing them --
# relying on user to start a different polygon and merge
final_pts[ix] = pts[ix]
ix2 = np.mod(ix + 1, self.nsides)
final_pts[ix2] = pts[ix2]
self.stop_growing[ix] = self.stop_growing[ix2] = True
if verbose:
print(
"Points %i and %i stopped growing because edge too long"
% (ix, ix2))
print(" ", pts[ix], pts[ix2])
self.polygon = P.Polygon(final_pts)
def overlap_score(self, points_a, points_b):
region_noisy = polygon.Polygon(points_a)
region_true = polygon.Polygon(points_b)
common_area = region_noisy.intersection(region_true).area
combined_area = region_noisy.union(region_true).area
percent_overlap = 100*(1. - ((region_true.area - common_area) / region_true.area))
return np.array([percent_overlap, np.mean(np.linalg.norm(points_a - points_b, axis=1))])
def Polygon2Shapely(p):
conts = []
sp = sgeometry.Polygon()
holes = []
contours = []
#print(len(p))
#print(p)
for ci, c in enumerate(p):
#print(ci)
shapely_p = spolygon.Polygon(c)
if p.nPoints(ci) > 2 and shapely_p.is_valid:
if p.isHole(ci):
#print('ishole')
holes.append(shapely_p)
else:
contours.append(shapely_p)
#for c in contours:
#sp=sp.union(spolygon.Polygon(c)
sp = shapely.ops.unary_union(contours)
for h in holes:
sp = sp.difference(h)
#sp=sgeometry.asMultiPolygon(conts)
#sp=ops.cascaded_union(sp)
return sp
def shapelyToChunks(p, zlevel): #
chunks = []
# p=sortContours(p)
seq = polygon_utils_cam.shapelyToCoords(p)
i = 0
for s in seq:
# progress(p[i])
if len(s) > 1:
chunk = camPathChunk([])
if len(s) == 2:
sgeometry.LineString(s)
else:
chunk.poly = spolygon.Polygon(
s
) # this should maybe be LineString? but for sorting, we need polygon inside functions.
for v in s:
# progress (v)
# print(v)
if p.has_z:
chunk.points.append((v[0], v[1], v[2]))
else:
chunk.points.append((v[0], v[1], zlevel))
# chunk.points.append((chunk.points[0][0],chunk.points[0][1],chunk.points[0][2]))#last point =first point
if chunk.points[0] == chunk.points[-1] and len(s) > 2:
chunk.closed = True
chunks.append(chunk)
i += 1
chunks.reverse() # this is for smaller shapes first.
#
return chunks
def PolygonGeoJSONToShp_WGS84(json_fp,
shp_fp=None): #面状GeoJSON文件转成SHP（火星坐标 转 WGS84）
import coordinate_conversion
from shapely.geometry import polygon
if not json_fp.endswith("json"): return False
if shp_fp is None:
shp_fp = "{}_wgs84.shp".format(json_fp[:-5])
elif not shp_fp.endswith("shp"):
return False
gdf = geopandas.read_file(json_fp)
# GCJ02转WGS84
for i in range(0, len(gdf)):
poly = gdf.geometry[i] # 获取空间属性，即GeoSeries
old_pnts = poly.exterior.coords #获得坐标串
new_pnts = [] #新坐标
for old_pnt in old_pnts:
lng, lat = old_pnt
lng, lat = coordinate_conversion.gcj02towgs84(lng, lat) #转换
new_pnts.append((lng, lat))
gdf.geometry[i] = polygon.Polygon(new_pnts)
# 设置成WGS84，并保存
gdf.crs = {'init': 'epsg:4326'}
gdf.to_file(shp_fp, encoding="utf-8")
return shp_fp
def process_overlay(input_file, output_file):
fimg = imread(input_file)
gimg = color.colorconv.rgb2grey(fimg)
gimg = gimg[ymin:ymax, xmin:xmax]
contours = measure.find_contours(
gimg, 0.8) # This one line pretty much does everything
fig, ax = plt.subplots()
plt.xlim([0, gimg.shape[1]])
plt.ylim([0, gimg.shape[0]])
plt.gca().invert_yaxis()
ax.axis('off')
fig.set_size_inches(gimg.shape[1] / 100, gimg.shape[0] / 100)
contour_polys = []
# Paint output
for n, contour in enumerate(contours):
inside = False
for poly in contour_polys:
# Check if this contour lies inside one of the previous ones.
if poly.contains(Point(contour[0])):
inside = True
break
if inside:
countour_color = "w" # If so, paint it white
else:
countour_color = "b" # If not, paint it blue
contour_polys.append(polygon.Polygon(contour))
ax.fill(contour[:, 1], contour[:, 0], countour_color, linewidth=0.2)
# Write to file
plt.savefig(output_file, transparent=True)
def from_shp_to_polygon(path_to_shape, buffer_p=0.5):
"""
Read the input shapefile and return a list of polygon objects
:param path_to_shape: absolute path to the input shapefile
:param buffer_p: boundary buffer in degree
:return:
"""
# - open the shapefile using the python fiona package
region_list = list()
# - read the input regional shapefile
pol = shapefile.Reader(path_to_shape)
# - extract polygon-shapes
sub = pol.shapes()
# - Build the regional ice-covered region domain
for ss in range(0, len(sub)):
if len(sub[ss].parts) > 1:
# - the shapefile is composed by multiple parts defining an external ring and one ore more
# - interior holes
limits = sub[ss].parts
holes = [] # - holes boundaries
for x in range(2, len(limits)):
holes.append(sub[ss].points[limits[x - 1]:limits[x]])
# - define polygon with holes
shp_tmp = polygon.Polygon(sub[ss].points[limits[0]:limits[1]],
holes)
else:
# - polygon composed only by an external ring
shp_tmp = shape(sub[ss]).buffer(buffer_p)
region_list.append(shp_tmp)
# -
return region_list
def chunkToShapely(chunk):
# pverts=[]
# for v in chunk.points:
# pverts.append((v[0],v[1]))
p = spolygon.Polygon(chunk.points)
return p
def Circle(r, np):
c = []
v = mathutils.Vector((r, 0, 0))
e = mathutils.Euler((0, 0, 2.0 * math.pi / np))
for a in range(0, np):
c.append((v.x, v.y))
v.rotate(e)
p = spolygon.Polygon(c)
return p
def makeGrid(fileName, buff=.4):
global grid
grid = Grid()
grid.buff = buff
file = open(fileName)
lines = file.readlines()
coords = lines[0].replace('(', '').replace(')', '').split()
xVals = []
yVals = []
for xy in coords:
xVals.append(xy.split(',')[0])
yVals.append(xy.split(',')[1])
grid.minX = (float)(min(xVals))
grid.maxX = (float)(max(xVals))
grid.minY = (float)(min(yVals))
grid.maxY = (float)(max(yVals))
lines = lines[2::] #delete first two lines
#Reading obstacles
# poly = ptc.Polygon(bounds) #create polygon based on vertices
# poly.get_path().contains_point([2,2]) #see if point in polygon
while lines[0] != '---\n':
vertices = []
ln = lines[0]
coords = ln.replace('(', '').replace(')', '').split()
for xy in coords:
xVal = (float)(xy.split(',')[0])
yVal = (float)(xy.split(',')[1])
vertices.append([xVal, yVal])
obs = ptc.Polygon(vertices)
poly = geometry.Polygon(obs.get_xy())
grid.geomPoly.append(poly)
grid.obstacles.append(obs)
if grid.buff != 0:
grid.buffedPoly.append(
pg.Polygon(
poly.buffer(grid.buff,
cap_style=3,
join_style=2,
mitre_limit=2).exterior))
else:
grid.buffedPoly.append(poly)
lines = lines[1::]
# grid.obsLineStrings, grid.obsVertices = createLineSegments()
return grid
def calcGoldenSec(objPos, roomRect, dR):
"""
calculating objects location w.r.t. golden section lines
objPos: objects' center position
roomRect: 4 points of room (or sub-area) rectangle
dR: room diagonal
"""
# make sure the vertices are ordered
tmpRect = sgp.Polygon(roomRect)
tmpRect = tmpRect.convex_hull
t_rect = tmpRect.exterior.coords[0:-1]
# creating golden lines. Assuming gsRatio = 13/21
# go over the 2 consecutive pair of vertices and generate the 4-lines, 2 in each side
gsr = 13.0 / 21.0
line1 = sgls.LineString((t_rect[0], t_rect[1]))
length = npla.norm(np.array(t_rect[0]) - np.array(t_rect[1]))
pt11 = line1.interpolate(length * (1.0 - gsr))
pt12 = line1.interpolate(length * gsr)
line3 = sgls.LineString((t_rect[2], t_rect[3]))
length = npla.norm(np.array(t_rect[2]) - np.array(t_rect[3]))
pt32 = line3.interpolate(length * (1.0 - gsr))
pt31 = line3.interpolate(length * gsr)
line2 = sgls.LineString((t_rect[1], t_rect[2]))
length = npla.norm(np.array(t_rect[1]) - np.array(t_rect[2]))
pt21 = line2.interpolate(length * (1.0 - gsr))
pt22 = line2.interpolate(length * gsr)
line4 = sgls.LineString((t_rect[3], t_rect[0]))
length = npla.norm(np.array(t_rect[3]) - np.array(t_rect[0]))
pt42 = line4.interpolate(length * (1.0 - gsr))
pt41 = line4.interpolate(length * gsr)
gsLines = []
gsLines.append(sgls.LineString((pt11, pt31)))
gsLines.append(sgls.LineString((pt12, pt32)))
gsLines.append(sgls.LineString((pt21, pt41)))
gsLines.append(sgls.LineString((pt22, pt42)))
dObjGs = []
for i in range(len(objPos)):
dd = []
for j in range(len(gsLines)):
dd.append(gsLines[j].distance(spt.Point(objPos[i])))
dObjGs.append(min(dd))
gP = np.sum(dObjGs)
gP /= (1.0 * dR * len(objPos))
return gP
def getHexagon(self, xColumn: int, yRow: int) -> polygon.Polygon:
"""
Gets the hexagon (polygon) for the given integer hex cell coordinates
:param xColumn: the column coordinate
:param yRow: the row coordinate
:return: the hexagon
"""
centreX = xColumn * self.hexagonWidth
centreY = yRow * self.rowStep
if yRow % 2 == 1:
centreX += 0.5 * self.hexagonWidth
centre = np.array([centreX, centreY])
return polygon.Polygon([centre + o for o in self.offsetVectors])
def getSurburbs(self) -> dict:
suburbDict = {}
with open('data/melbourne.geojson') as file:
file_data = json.load(file)
for i in file_data['features']:
_id = i['id']
name = i['properties']['SA2_NAME16']
suburbDict[_id] = {}
suburbDict[_id]['name'] = name
if i['geometry']['type'] == 'Polygon':
suburbDict[_id]['type'] = 'Polygon'
coordinate_list = i['geometry']['coordinates'][0]
suburbDict[_id]['Polygon'] = polygon.Polygon([
(x, y) for x, y in coordinate_list
])
else:
suburbDict[_id]['type'] = 'MultiPolygon'
suburbDict[_id]['polygons'] = []
for coordinates in i['geometry']['coordinates']:
suburbDict[_id]['polygons'].append(
polygon.Polygon([(x, y)
for x, y in coordinates[0]]))
return suburbDict
def __new__(self, polygons=None):
if not polygons:
# allow creation of empty multipolygons, to support unpickling
# TODO better empty constructor
return shapely.from_wkt("MULTIPOLYGON EMPTY")
elif isinstance(polygons, MultiPolygon):
return polygons
polygons = getattr(polygons, "geoms", polygons)
polygons = [
p for p in polygons
if p and not (isinstance(p, polygon.Polygon) and p.is_empty)
]
L = len(polygons)
# Bail immediately if we have no input points.
if L == 0:
return shapely.from_wkt("MULTIPOLYGON EMPTY")
# This function does not accept sequences of MultiPolygons: there is
# no implicit flattening.
if isinstance(polygons[0], MultiPolygon):
raise ValueError(
"Sequences of multi-polygons are not valid arguments")
subs = []
for i in range(L):
ob = polygons[i]
if not isinstance(ob, polygon.Polygon):
shell = ob[0]
holes = ob[1]
p = polygon.Polygon(shell, holes)
else:
p = polygon.Polygon(ob)
subs.append(p)
return shapely.multipolygons(subs)
def rectangle_to_polygon(rect):
resBoxes = np.empty([1, 8], dtype='int32')
resBoxes[0, 0] = int(rect.xmin)
resBoxes[0, 4] = int(rect.ymax)
resBoxes[0, 1] = int(rect.xmin)
resBoxes[0, 5] = int(rect.ymin)
resBoxes[0, 2] = int(rect.xmax)
resBoxes[0, 6] = int(rect.ymin)
resBoxes[0, 3] = int(rect.xmax)
resBoxes[0, 7] = int(rect.ymax)
pointMat = resBoxes[0].reshape([2, 4]).T
return plg.Polygon(pointMat)
def test1():
poly1 = P.Polygon([(1, 2), (0, 0), (0.5, -2), (2.5, -1), (2, 1)])
poly2 = P.Polygon([(1, 1), (2, -3), (6, -1), (5, 1), (3, 3)])
cent1 = poly1.centroid
cent2 = poly2.centroid
assert not poly1.within(poly2)
assert not poly2.within(poly1)
poly3 = poly2.union(poly1)
inter = poly2.intersection(poly1)
plt.figure()
ax = plt.gca()
plot_coords(ax, poly1.exterior, 'r', 'o-', lw=3)
plot_coords(ax, poly2.exterior, 'b', 'o-', lw=3)
ax.plot(cent1.x, cent1.y, 'go')
ax.plot(cent2.x, cent2.y, 'gs')
plot_coords(ax, poly3.exterior, 'k', 'o-')
patch = PolygonPatch(inter, facecolor='#dd3333', alpha=0.5, zorder=0.5)
ax.add_patch(patch)
plt.show()
def fixPolygon(
poly: Union[polygon.Polygon, multipolygon.MultiPolygon],
maxAreaDiff=1e-2) -> Union[polygon.Polygon, multipolygon.MultiPolygon]:
"""
Fix invalid shapely polygons or multipolygons.
Reference:
https://stackoverflow.com/questions/35110632/splitting-self-intersecting-polygon-only-returned-one-polygon-in-shapely
:param poly: the polygon to fix
:param maxAreaDiff: the maximum change in area
:return: the fixed polygon or None if it cannot be fixed given the area change constraint
"""
def _fixPolygonComponent(coords: List[Tuple[float, float]]):
res = list(
polygonize(unary_union(LineString(list(coords) + [coords[0]]))))
return reduce(lambda p1, p2: p1.union(p2), res)
if poly.is_valid:
return poly
else:
if isinstance(poly, polygon.Polygon):
exteriorCoords = poly.exterior.coords[:]
fixedExterior = _fixPolygonComponent(exteriorCoords)
fixedInterior = polygon.Polygon()
for interior in poly.interiors:
coords = interior.coords[:]
fixedInterior = fixedInterior.union(
_fixPolygonComponent(coords))
fixedPolygon = fixedExterior.difference(fixedInterior)
elif isinstance(poly, multipolygon.MultiPolygon):
polys = list(poly)
fixedPolys = [
fixPolygon(p, maxAreaDiff=maxAreaDiff) for p in polys
]
fixedPolygon = reduce(lambda p1, p2: p1.union(p2), fixedPolys)
else:
raise Exception(f"Unsupported type {type(poly)}")
areaDiff = float('Inf') if poly.area == 0 else abs(
poly.area - fixedPolygon.area) / poly.area
#log.info(f"Invalid polygon\n{poly}\nComputed fix:\n{fixedPolygon}.\nArea error: {areaDiff}")
if areaDiff > maxAreaDiff:
return None
else:
return fixedPolygon
def polygon_from_points(points, correctOffset=False):
"""
Returns a Polygon object to use with the Polygon2 class from a list of 8 points: x1,y1,x2,y2,x3,y3,x4,y4
"""
if correctOffset: #this will substract 1 from the coordinates that correspond to the xmax and ymax
points[2] -= 1
points[4] -= 1
points[5] -= 1
points[7] -= 1
resBoxes = np.empty([1, 8], dtype='int32')
resBoxes[0, 0] = int(points[0])
resBoxes[0, 4] = int(points[1])
resBoxes[0, 1] = int(points[2])
resBoxes[0, 5] = int(points[3])
resBoxes[0, 2] = int(points[4])
resBoxes[0, 6] = int(points[5])
resBoxes[0, 3] = int(points[6])
resBoxes[0, 7] = int(points[7])
pointMat = resBoxes[0].reshape([2, 4]).T
return plg.Polygon(pointMat)
def get_voronoy_multipolygon(x, y , edges=None):
"""
Parameters
----------
rect: [xmin, xmax, ymin, ymax] -optional-
Edges of the voronoy map
"""
from scipy.spatial import Voronoi
from itertools import product
# - Define the Grid
# --------------------
flagok = np.isnan(x) * np.isnan(y)
x,y = x[~flagok], y[~flagok]
# define extremum to avoid non finished edges
ext_x,ext_y = np.asarray(list (product([x.min()-x.max()*10,x.mean(), x.max()+x.max()*10],
[y.min()-y.max()*10,y.mean(), y.max()+y.max()*10]))).T
xy = np.asarray([np.concatenate([x,ext_x]),
np.concatenate([y,ext_y])]).T
npoint = np.shape(xy)[0]
vor = Voronoi(xy)
xy_poly = [[vor.vertices[x] for x in vor.regions[vor.point_region[i]]
if x>=0 # this others could be saved
] for i in range(npoint)]
#edges = polygon.Polygon([[0,0],[0,4100],[2100,4100],[2100,0]])
polygons = []
for xy_ in xy_poly:
try:
p_ = edges.intersection(polygon.Polygon(xy_))
except:
continue
if p_.area>0:
polygons.append(p_)
return MultiPolygon(polygons)
def content_within_shape(content: np.ndarray, trans: Affine,
shape: sgp.LinearRing):
"""
:param content: data being displayed on the screen
:param trans: affine transform between content array indices and screen coordinates
:param shape: LinearRing in screen coordinates (e.g. mercator meters)
:return: masked_content:masked_array, (y_index_offset:int, x_index_offset:int) containing minified masked content array
"""
# Get the bounds so we can limit how big our rasterize boolean array actually is
inv_trans = ~trans
# convert bounding box to content coordinates
# (0, 0) image index is upper-left origin of data (needs more work if otherwise)
nx, ny, mx, my = shape.bounds # minx,miny,maxx,maxy
nx, my = inv_trans * (nx, my)
mx, ny = inv_trans * (mx, ny)
nx, my = int(nx), int(my)
mx, ny = int(np.ceil(mx)), int(np.ceil(ny))
# subset the content (ny is the higher *index*, my is the lower *index*)
w = (mx - nx) + 1
h = (ny - my) + 1
# Make our linear ring a properly oriented shapely polygon
shape = sgp.Polygon(shape)
shape = sgp.orient(shape)
# create a transform that is shifted to where the polygon is
offset_trans = trans * Affine.translation(nx, my)
# Get boolean mask for where the polygon is and get an index mask of those positions
index_mask = np.nonzero(
rasterize([shape],
out_shape=(h, w),
transform=offset_trans,
default_value=1).astype(np.bool_))
# translate the mask indexes back to the original data array coordinates (original index mask is read-only)
index_mask = (index_mask[0] + my, index_mask[1] + nx)
return index_mask, content[index_mask]
def calcLayoutClearance(objList, layoutPoly, entList):
"""
calculating layout polygons mean overlap
objList - List of obstacle objects (polygons)
Each object is assumed to represent the EXTENDED-bounding-box, i.e., including the extra gap
required around the object
layoutPoly - Nx2 list of ordered vertices defining a 2D polygon of N vertices - room polygon layout
last point NEQ first point
entList - List of entrance line segments (2D points). Entrances should not be occluded
"""
#
# =>>>>> CURRENTLY constraints are not included, e.g. entrance, window, power-outlet, TV
#
nObj = len(objList)
objListSp = []
# Transform to shapely
for n in range(nObj):
objListSp.append(sgp.Polygon(objList[n]))
ovlpSum = 0
for m in range(nObj - 1):
for n in range(nObj):
if m == n:
continue
ovlp = objListSp[m].intersection(objListSp[n]).area
ovlpSum += ovlp / objListSp[m].area
ovlpSum = ovlpSum / (nObj * (nObj - 1))
# ==> entrance overlap
# if entLine.touches(tmpPolyLayout) or entLine.intersects(tmpPolyLayout):
# ovlp = entLine.intersection(tmpPolyLayout).length / entLine.length
return ovlpSum
def main():
print(__file__ + " start!!")
for i in range(5):
# start and goal position
# sx, sy = random.randrange(-125,125), random.randrange(-125,125) # [m]
# gx, gy = random.randrange(-125,125), random.randrange(-125,125) # [m]
sx, sy = -40.0, -100.0 # [m]
gx, gy = -40.0, 80.0 # [m]
robot_radius = 5.0 # [m]
cnt = 5
obstacles = []
for i in range(cnt):
obstacles.append(genRandomRectangle())
obstacles.append(
ObstaclePolygon([150, -150, -150, 150], [150, 150, -150, -150]))
visible = [False] * (len(obstacles))
if show_animation: # pragma: no cover
plt.plot(sx, sy, "or")
plt.plot(gx, gy, "ob")
for ob in obstacles:
ob.plot()
plt.axis("equal")
plt.pause(0.1)
#create a planner and initalize it with the agent's pose
plan = Planner([sx, sy, 0], [])
world = sp.Polygon()
try:
fov = FieldOfView([sx, sy, 0], 40 / 180.0 * math.pi, obstacles)
#fovChecker = FieldOfView( [sx,sy,0], 40/180.0*math.pi, [obstacles[-1]])
fovChecker = FieldOfView([sx, sy, 0], 355 / 180.0 * math.pi,
obstacles)
for stepSize, heading in plan.closedLoopPlannerFast([gx, gy]):
#needs to be replaced with turning the agent to the appropriate heading in the simulator, then stepping.
#the resulting agent position / heading should be used to set plan.agent* values.
plan.agentH = heading
plan.agentX = plan.agentX + stepSize * math.sin(plan.agentH)
plan.agentY = plan.agentY + stepSize * math.cos(plan.agentH)
#any new obstacles that were observed during the step should be added to the planner
for i in range(len(obstacles)):
if not visible[i] and obstacles[i].minDistanceToVertex(
plan.agentX, plan.agentY) < 30:
plan.addObstacle(obstacles[i])
fovChecker.obstacle.append(obstacles[i])
visible[i] = True
fov.agentX = plan.agentX
fov.agentY = plan.agentY
fov.agentH = plan.agentH
poly = fov.getFoVPolygon(100)
view = sp.Polygon(zip(poly.x_list, poly.y_list))
world = world.union(view)
cx, cy, h = random.randrange(-125, 125), random.randrange(
-125, 125), random.random() * 2 * math.pi # [m]
fovChecker.agentX = cx
fovChecker.agentY = cy
fovChecker.agentH = h
checkPoly = fovChecker.getFoVPolygon(10)
newPoly = sp.Polygon(zip(checkPoly.x_list, checkPoly.y_list))
newPoly = newPoly.difference(world)
if show_animation:
plt.cla()
plt.plot(plan.agentX, plan.agentY, "or")
plt.plot(gx, gy, "ob")
poly.plot("-r")
if world.geom_type == 'MultiPolygon':
for i in range(len(world)):
pts = world[i].exterior.coords
plt.fill([p[0] for p in pts], [p[1] for p in pts],
"-b")
else:
pts = world.exterior.coords
plt.fill([p[0] for p in pts], [p[1] for p in pts],
"-b")
if newPoly.geom_type == 'MultiPolygon':
for i in range(len(newPoly)):
pts = newPoly[i].exterior.coords
plt.fill([p[0] for p in pts], [p[1] for p in pts],
"-g")
else:
pts = newPoly.exterior.coords
plt.fill([p[0] for p in pts], [p[1] for p in pts],
"-g")
plt.text(-125, -125,
"Random View Area: {:0.5}".format(newPoly.area))
for i in range(len(obstacles)):
if visible[i]:
obstacles[i].plot("-g")
else:
obstacles[i].plot("-k")
plt.axis("equal")
plt.pause(1)
except:
continue
def __init__(self,
c,
p1,
condition_func,
rtol=0.1,
nsides=10,
edge_len_rtol=2,
max_radius_fac=500,
reseed_rate_fac=10):
"""
c is the center seed point
p1 is any point that defines the initial radius
relative to c.
(p1 will become one vertex of the polygon.)
condition_func is a function of (x,y) tuple that
returns a scalar, or a sequence of such functions.
The domain is defined by the set of connected points
with the same sign as condition_func(c).
rtol (default 0.1) is the relative tolerance of
the growth stopping condition, as a fraction of the
initial "radius" of the regular polygon defined
by |p1-c|.
nsides (default 10) is the initial number of sides
for the polygons.
edge_len_rtol (default 2) is the factor of |p1-c| that
determines the maximum side length of a polygon edge
before new vertex is added adaptively during domain
growth.
max_radius_fac (default 1000) is the factor of |p1-c|
that determines the largest distance from c that
a grown polygon can be before the iterations stop.
reseed_rate_fac (default 10) is the factor of |p1-c|
that determines how far from the original center
seed point the polygon can grow in any direction
before a new seed polygon is added ???????????????
"""
self.initial_center = np.asarray(c)
self.initial_p = np.asarray(p1)
self.initial_r = self.initial_p - self.initial_center
if callable(condition_func):
# single function
target_fsign = np.sign(condition_func(c))
self.boolfunc = lambda p: np.sign(condition_func(p, fsign = target_fsign)) == \
target_fsign
else:
# list of functions
target_fsigns = [np.sign(f(c)) for f in condition_func]
self.boolfunc = lambda p: [np.sign(f(p)) for f in condition_func] == \
target_fsigns
# check that p1 satifies same sign
if not self.boolfunc(p1):
#raise dst.PyDSTool_ValueError("f(p1) has different sign to f(c)")\
#print("Error: f(p1) has different sign to f(c)")
warnings.warn("Warning: f(p1) has different sign to f(c)")
else:
# distance scale and initial trust radius
self.r0 = np.linalg.norm(p1 - c)
self.max_radius = max_radius_fac * self.r0
self.edge_resolution_atol = rtol * self.r0
self.edge_length_atol = edge_len_rtol * self.r0
# angle of trust radius line to x-axis
theta1 = math.acos(self.initial_r[0] / self.r0)
dtheta = 2 * math.pi / nsides
# distribute angles uniformly over the circle
# and compute nsides-1 new polygon vertices from p1
pt_list = [self.initial_center + \
self.r0*np.array([math.cos(theta1+dtheta*i),
math.sin(theta1+dtheta*i)])
for i in range(nsides)]
self.polygon = P.Polygon(pt_list)
self.nsides = nsides
self.stop_growing = [False] * self.nsides
from shapely.geometry import polygon
bounding_for_maz = polygon.Polygon([(564413, 892531), (618474, 892531),
(564413, 894696), (618474, 894696)])
# -*- coding: utf-8 -*-
# <nbformat>3.0</nbformat>
# <codecell>
import matplotlib.pyplot as plt
x = [1,2,3,4]
y = [1,2,3,4]
m = [[15,14,13,12],[14,12,10,8],[13,10,7,4],[12,8,4,0]]
cs = plt.contour(x,y,m)
# <codecell>
from shapely.geometry import polygon as sp
for i in range(len(cs.collections)):
p = cs.collections[i].get_paths()[0]
v = p.vertices
x = v[:,0]
y = v[:,1]
if len(x)>2:
poly = sp.Polygon([(i[0], i[1]) for i in zip(x,y)])
print i, poly, poly.area
# <codecell>
def path_to_geos(path):
"""
"""
import time
start_time = time.time()
log = []
DEBUG = False
# path_verts, path_codes = zip(*list(path.iter_segments(curves=False)))
path_verts, path_codes = path_segments(path, curves=False)
path_verts = np.array(path_verts)
path_codes = np.array(path_codes)
if DEBUG: print 'codes:', path_codes
verts_split_inds = np.where(path_codes == Path.MOVETO)[0]
verts_split = np.split(path_verts, verts_split_inds, 0)
codes_split = np.split(path_codes, verts_split_inds, 0)
if DEBUG: print 'vs: ', `verts_split`
if DEBUG: print 'cs: ', `codes_split`
log.append('split done %s' % (time.time() - start_time))
collection = []
for path_verts, path_codes in zip(verts_split, codes_split):
if len(path_verts) == 0:
continue
# XXX A path can be given which does not end with close poly, in that situation, we have to guess?
if DEBUG: print 'pv: ', path_verts
# XXX Implement a point
if path_verts.shape[0] > 2 and (path_codes[-1] == Path.CLOSEPOLY or all(path_verts[0, :] == path_verts[-1, :])):
if path_codes[-1] == Path.CLOSEPOLY:
ipath2 = polygon.Polygon(path_verts[:-1, :])
else:
ipath2 = polygon.Polygon(path_verts)
else:
ipath2 = linestring.LineString(path_verts)
if (len(collection) > 0 and
isinstance(collection[-1][0], polygon.Polygon) and
isinstance(ipath2, polygon.Polygon) and
collection[-1][0].contains(ipath2.exterior)):
collection[-1][1].append(ipath2.exterior)
else:
# collection is a list of [exernal_poly, list_of_internal_polys]
collection.append([ipath2, []])
log.append('collection done before while %s.' % (time.time() - start_time))
log.append('Len of collection %s.' % (len(collection)))
res = []
for external_poly, internal_polys in collection:
# print external_poly
if len(internal_polys) > 0:
# print internal_polys
# XXX worry about islands within lakes
poly = polygon.Polygon(external_poly.exterior, internal_polys)
else:
poly = external_poly
res.append(poly)
collection = res
# if len(collection) > 1:
# i = 0
# while i<len(collection)-1:
# poly = collection[i]
# poly2 = collection[i+1]
#
# # TODO Worry about islands within lakes
# if isinstance(poly, polygon.Polygon) and isinstance(poly2, polygon.Polygon):
# if poly.contains(poly2):
# # XXX This is the slow bit!
## collection[i] = polygon.Polygon(poly.exterior, list(poly.interiors) + [poly2.exterior])
# collection.pop(i+1)
# continue
# else:
# res.append([poly])
# i+=1
#
# log.append('Post len of collection %s.' % (len(collection)))
# log.append('collection done after while %s' % (time.time() - start_time))
if len(collection) == 1:
result = collection
else:
if all([isinstance(geom, linestring.LineString) for geom in collection]):
result = [MultiLineString(collection)]
else:
result = collection
# if DEBUG: print 'geom: ', collection, type(collection)
# raise NotImplementedError('The path given was not a collection of line strings, '
# 'nor a single polygon with interiors.')
if (time.time() - start_time) > 1:
print 'geos time %s' % (time.time() - start_time)
print '\n'.join(log)
# remove any zero area polygons
result = filter(lambda geom: (isinstance(geom, polygon.Polygon) and geom.area != 0) or \
not isinstance(geom, polygon.Polygon), result)
return result
def medial_axis(o):
print('operation: Medial Axis')
print('doing highly experimental stuff')
from cam.voronoi import Site, computeVoronoiDiagram
chunks = []
gpoly = spolygon.Polygon()
angle = o.cutter_tip_angle
slope = math.tan(math.pi * (90 - angle / 2) / 180)
if o.cutter_type == 'VCARVE':
angle = o.cutter_tip_angle
# start the max depth calc from the "start depth" of the operation.
maxdepth = o.maxz - math.tan(
math.pi * (90 - angle / 2) / 180) * o.cutter_diameter / 2
# don't cut any deeper than the "end depth" of the operation.
if maxdepth < o.minz:
maxdepth = o.minz
# the effective cutter diameter can be reduced from it's max since we will be cutting shallower than the original maxdepth
# without this, the curve is calculated as if the diameter was at the original maxdepth and we get the bit
# pulling away from the desired cut surface
o.cutter_diameter = (maxdepth - o.maxz) / (
-math.tan(math.pi * (90 - angle / 2) / 180)) * 2
elif o.cutter_type == 'BALLNOSE' or o.cutter_type == 'BALL':
# angle = o.cutter_tip_angle
maxdepth = o.cutter_diameter / 2
else:
o.warnings += 'Only Ballnose, Ball and V-carve cutters\n are supported'
return
# remember resolutions of curves, to refine them,
# otherwise medial axis computation yields too many branches in curved parts
resolutions_before = []
for ob in o.objects:
if ob.type == 'CURVE' or ob.type == 'FONT':
resolutions_before.append(ob.data.resolution_u)
if ob.data.resolution_u < 64:
ob.data.resolution_u = 64
polys = utils.getOperationSilhouete(o)
mpoly = sgeometry.asMultiPolygon(polys)
mpoly_boundary = mpoly.boundary
for poly in polys:
schunks = shapelyToChunks(poly, -1)
schunks = chunksRefineThreshold(
schunks, o.medial_axis_subdivision,
o.medial_axis_threshold) # chunksRefine(schunks,o)
verts = []
for ch in schunks:
for pt in ch.points:
# pvoro = Site(pt[0], pt[1])
verts.append(pt) # (pt[0], pt[1]), pt[2])
# verts= points#[[vert.x, vert.y, vert.z] for vert in vertsPts]
nDupli, nZcolinear = unique(verts)
nVerts = len(verts)
print(str(nDupli) + " duplicates points ignored")
print(str(nZcolinear) + " z colinear points excluded")
if nVerts < 3:
print("Not enough points")
return {'FINISHED'}
# Check colinear
xValues = [pt[0] for pt in verts]
yValues = [pt[1] for pt in verts]
if checkEqual(xValues) or checkEqual(yValues):
print("Points are colinear")
return {'FINISHED'}
# Create diagram
print("Tesselation... (" + str(nVerts) + " points)")
xbuff, ybuff = 5, 5 # %
zPosition = 0
vertsPts = [Point(vert[0], vert[1], vert[2]) for vert in verts]
# vertsPts= [Point(vert[0], vert[1]) for vert in verts]
pts, edgesIdx = computeVoronoiDiagram(vertsPts,
xbuff,
ybuff,
polygonsOutput=False,
formatOutput=True)
#
# pts=[[pt[0], pt[1], zPosition] for pt in pts]
newIdx = 0
vertr = []
filteredPts = []
print('filter points')
for p in pts:
if not poly.contains(sgeometry.Point(p)):
vertr.append((True, -1))
else:
vertr.append((False, newIdx))
if o.cutter_type == 'VCARVE':
# start the z depth calc from the "start depth" of the operation.
z = o.maxz - mpoly.boundary.distance(
sgeometry.Point(p)) * slope
if z < maxdepth:
z = maxdepth
elif o.cutter_type == 'BALL' or o.cutter_type == 'BALLNOSE':
d = mpoly_boundary.distance(sgeometry.Point(p))
r = o.cutter_diameter / 2.0
if d >= r:
z = -r
else:
# print(r, d)
z = -r + sqrt(r * r - d * d)
else:
z = 0 #
# print(mpoly.distance(sgeometry.Point(0,0)))
# if(z!=0):print(z)
filteredPts.append((p[0], p[1], z))
newIdx += 1
print('filter edges')
filteredEdgs = []
ledges = []
for e in edgesIdx:
do = True
p1 = pts[e[0]]
p2 = pts[e[1]]
# print(p1,p2,len(vertr))
if vertr[e[0]][0]: # exclude edges with allready excluded points
do = False
elif vertr[e[1]][0]:
do = False
if do:
filteredEdgs.append(((vertr[e[0]][1], vertr[e[1]][1])))
ledges.append(
sgeometry.LineString((filteredPts[vertr[e[0]][1]],
filteredPts[vertr[e[1]][1]])))
# print(ledges[-1].has_z)
bufpoly = poly.buffer(-o.cutter_diameter / 2, resolution=64)
lines = shapely.ops.linemerge(ledges)
# print(lines.type)
if bufpoly.type == 'Polygon' or bufpoly.type == 'MultiPolygon':
lines = lines.difference(bufpoly)
chunks.extend(shapelyToChunks(bufpoly, maxdepth))
chunks.extend(shapelyToChunks(lines, 0))
# segments=[]
# processEdges=filteredEdgs.copy()
# chunk=camPathChunk([])
# chunk.points.append(filteredEdgs.pop())
# while len(filteredEdgs)>0:
# Create new mesh structure
# print("Create mesh...")
# voronoiDiagram = bpy.data.meshes.new("VoronoiDiagram") #create a new mesh
#
#
#
# voronoiDiagram.from_pydata(filteredPts, filteredEdgs, []) #Fill the mesh with triangles
#
# voronoiDiagram.update(calc_edges=True) #Update mesh with new data
# #create an object with that mesh
# voronoiObj = bpy.data.objects.new("VoronoiDiagram", voronoiDiagram)
# #place object
# #bpy.ops.view3d.snap_cursor_to_selected()#move 3d-cursor
#
# #update scene
# bpy.context.scene.objects.link(voronoiObj) #Link object to scene
# bpy.context.scene.objects.active = voronoiObj
# voronoiObj.select = True
# bpy.ops.object.convert(target='CURVE')
oi = 0
for ob in o.objects:
if ob.type == 'CURVE' or ob.type == 'FONT':
ob.data.resolution_u = resolutions_before[oi]
oi += 1
# bpy.ops.object.join()
chunks = utils.sortChunks(chunks, o)
layers = getLayers(o, o.maxz, o.min.z)
chunklayers = []
for layer in layers:
for chunk in chunks:
if chunk.isbelowZ(layer[0]):
newchunk = chunk.copy()
newchunk.clampZ(layer[1])
chunklayers.append(newchunk)
if o.first_down:
chunklayers = utils.sortChunks(chunklayers, o)
chunksToMesh(chunklayers, o)
def shape_factory(self, *args):
return polygon.Polygon(*args)
