Source code for scenic.core.geometry

"""Utility functions for geometric computation."""

import math
import itertools
import warnings

import numpy as np
import shapely.geometry
import shapely.ops

from scenic.core.distributions import (needsSampling, distributionFunction,
from scenic.core.lazy_eval import needsLazyEvaluation
from scenic.core.utils import cached_property

def sin(x) -> float:
	return math.sin(x)

def cos(x) -> float:
	return math.cos(x)

def hypot(x, y) -> float:
	return math.hypot(x, y)

def max(*args, **kwargs):
	return __builtins__['max'](*args, **kwargs)

def min(*args, **kwargs):
	return __builtins__['min'](*args, **kwargs)

def normalizeAngle(angle) -> float:
	while angle > math.pi:
		angle -= math.tau
	while angle < -math.pi:
		angle += math.tau
	assert -math.pi <= angle <= math.pi
	return angle

def addVectors(a, b):
	ax, ay = a[0], a[1]
	bx, by = b[0], b[1]
	return (ax + bx, ay + by)

def subtractVectors(a, b):
	ax, ay = a[0], a[1]
	bx, by = b[0], b[1]
	return (ax - bx, ay - by)

def averageVectors(a, b, weight=0.5):
	ax, ay = a[0], a[1]
	bx, by = b[0], b[1]
	aw, bw = 1.0 - weight, weight
	return (ax * aw + bx * bw, ay * aw + by * bw)

def rotateVector(vector, angle):
	x, y = vector
	c, s = cos(angle), sin(angle)
	return ((c * x) - (s * y), (s * x) + (c * y))

def findMinMax(iterable):
	minv = float('inf')
	maxv = float('-inf')
	for val in iterable:
		if val < minv:
			minv = val
		if val > maxv:
			maxv = val
	return (minv, maxv)

def radialToCartesian(point, radius, heading):
	angle = heading + (math.pi / 2.0)
	rx, ry = radius * cos(angle), radius * sin(angle)
	return (point[0] + rx, point[1] + ry)

def positionRelativeToPoint(point, heading, offset):
	ro = rotateVector(offset, heading)
	return addVectors(point, ro)

def headingOfSegment(pointA, pointB):
	ax, ay = pointA
	bx, by = pointB
	return normalizeAngle(math.atan2(by - ay, bx - ax) - (math.pi / 2.0))

def viewAngleToPoint(point, base, heading):
	x, y = base
	ox, oy = point
	a = math.atan2(oy - y, ox - x) - (heading + (math.pi / 2.0))
	return normalizeAngle(a)

def apparentHeadingAtPoint(point, heading, base):
	x, y = base
	ox, oy = point
	a = (heading + (math.pi / 2.0)) - math.atan2(oy - y, ox - x)
	return normalizeAngle(a)

def circumcircleOfAnnulus(center, heading, angle, minDist, maxDist):
	m = (minDist + maxDist) / 2.0
	g = (maxDist - minDist) / 2.0
	h = m * math.sin(angle / 2.0)
	h2 = h * h
	d = math.sqrt(h2 + (m * m))
	r = math.sqrt(h2 + (g * g))
	return radialToCartesian(center, d, heading), r

def pointIsInCone(point, base, heading, angle):
	va = viewAngleToPoint(point, base, heading)
	return (abs(va) <= angle / 2.0)

def distanceToLine(point, a, b):
	lx, ly = b[0] - a[0], b[1] - a[1]
	norm = math.hypot(lx, ly)
	nx, ny = -ly/norm, lx/norm
	px, py = point[0] - a[0], point[1] - a[1]
	return abs((px * nx) + (py * ny))

def distanceToSegment(point, a, b):
	lx, ly = b[0] - a[0], b[1] - a[1]
	px, py = point[0] - a[0], point[1] - a[1]
	proj = (px * lx) + (py * ly)
	if proj < 0:
		return math.hypot(px, py)
	lnorm = math.hypot(lx, ly)
	if proj > lnorm * lnorm:
		return math.hypot(px - lx, py - ly)
		return abs((py * lx) - (px * ly)) / lnorm

def polygonUnion(polys, buf=0, tolerance=0, holeTolerance=0.002):
	if not polys:
		return shapely.geometry.Polygon()
	polys = list(polys)
	if len(polys) == 1:
		return polys[0]
	buffered = [poly.buffer(buf) for poly in polys]
	# remove empty polys to avoid triggering segfault in GEOS 3.10
	# (see
	nonempty = [poly for poly in buffered if not poly.is_empty]
	union = shapely.ops.unary_union(nonempty).buffer(-buf)
	assert union.is_valid, union
	if tolerance > 0:
		union = cleanPolygon(union, tolerance, holeTolerance)
		assert union.is_valid, union
		#checkPolygon(union, tolerance)
	return union

def checkPolygon(poly, tolerance):
	def checkPolyline(pl):
		for i, p in enumerate(pl[:-1]):
			q = pl[i+1]
			dx, dy = q[0] - p[0], q[1] - p[1]
			assert math.hypot(dx, dy) >= tolerance
	if isinstance(poly, shapely.geometry.MultiPolygon):
		for p in poly.geoms:
			checkPolygon(p, tolerance)
		for i in poly.interiors:

def cleanPolygon(poly, tolerance, holeTolerance=0, minRelArea=0.05, minHullLenRatio=0.9):
	if holeTolerance == 0:
		holeTolerance = tolerance * tolerance
	if poly.is_empty:
		return poly
	elif isinstance(poly, shapely.geometry.MultiPolygon):
		polys = [cleanPolygon(p, tolerance, holeTolerance) for p in poly.geoms]
		total = sum(poly.area for poly in polys)
		kept = []
		for poly in polys:
			area = poly.area
			if area >= holeTolerance or area >= minRelArea * total:
		poly = shapely.ops.unary_union(kept)
		assert poly.is_valid, poly
		return poly
	exterior = poly.exterior.simplify(tolerance)
	newExterior = cleanChain(exterior.coords, tolerance)
	if len(newExterior) <= 3:
		# attempt to save very thin polygons that would get reduced to a single point
		hull = exterior.convex_hull
		if hull.length >= minHullLenRatio * exterior.length:
			return hull
		return shapely.geometry.Polygon()
	ints = []
	for interior in poly.interiors:
		interior = interior.simplify(tolerance)
		interior = cleanChain(interior.coords, tolerance)
		if len(interior) >= 4:
			hole = shapely.geometry.Polygon(interior)
			if hole.area > holeTolerance:
	newPoly = shapely.geometry.Polygon(newExterior, ints)
	if not newPoly.is_valid:
		# last-ditch attempt to salvage polygon by splitting across self-intersections
		ext = splitSelfIntersections(newPoly.exterior, minArea=holeTolerance,
		                             minRelArea=minRelArea, minHullLenRatio=minHullLenRatio)
		holes = [
			splitSelfIntersections(hole, minArea=holeTolerance,
			                       minRelArea=minRelArea, minHullLenRatio=minHullLenRatio)
			for hole in newPoly.interiors
		newPoly = ext.difference(shapely.ops.unary_union(holes))
	assert newPoly.is_valid, newPoly
	return newPoly

def splitSelfIntersections(chain, minArea, minRelArea, minHullLenRatio):
	ls = shapely.geometry.LineString(chain)
	parts = list(shapely.ops.polygonize(shapely.ops.unary_union(ls)))
	total = sum(part.area for part in parts)
	if total < minArea:
		# attempt to save very thin polygons that could get thrown out by polygonize
		hull = ls.convex_hull
		if hull.length >= minHullLenRatio * ls.length:
			return hull
	kept = [part for part in parts if part.area >= minArea or (part.area / total) >= minRelArea]
	return shapely.ops.unary_union(kept)

def cleanChain(chain, tolerance=1e-6, lineTolerance=1e-6):
	closed = (tuple(chain[0]) == tuple(chain[-1]))
	minLength = 4 if closed else 3
	if len(chain) <= minLength:
		return chain
	tol2 = tolerance * tolerance
	# collapse nearby points (since Shapely's simplify method doesn't always do this)
	a = chain[0]
	newChain = [a]
	for b in chain[1:]:
		dx, dy = b[0] - a[0], b[1] - a[1]
		if (dx * dx) + (dy * dy) > tol2:
			a = b
	if closed:
		b = chain[0]
		dx, dy = b[0] - a[0], b[1] - a[1]
		if (dx * dx) + (dy * dy) <= tol2:
	if len(newChain) <= minLength:
		if not closed:
			if len(newChain) > 1:
		return newChain
	# collapse collinear points
	chain = newChain
	if closed:
		a = chain[-2]
		b = chain[0]
		ci = 1
		newChain = []
		a = chain[0]
		b = chain[1]
		ci = 2
		newChain = [a]
	for c in chain[ci:]:
		dx, dy = c[0] - a[0], c[1] - a[1]
		if dx == dy == 0 or distanceToLine(b, a, c) > lineTolerance:
			a = b
			b = c
			b = c
	return newChain

def removeHoles(polygon):
	if polygon.is_empty:
		return polygon
	elif isinstance(polygon, shapely.geometry.MultiPolygon):
		polys = (removeHoles(poly) for poly in polygon.geoms)
		poly = shapely.geometry.MultiPolygon(polys)
		assert poly.is_valid, poly
		return poly
		return shapely.geometry.Polygon(polygon.exterior)

[docs]class TriangulationError(RuntimeError): """Signals that the installed triangulation libraries are insufficient. Specifically, raised when pypoly2tri hits the recursion limit trying to triangulate a large polygon. """ pass
[docs]def triangulatePolygon(polygon): """Triangulate the given Shapely polygon. Note that we can't use ``shapely.ops.triangulate`` since it triangulates point sets, not polygons (i.e., it doesn't respect edges). We need an algorithm for triangulation of polygons with holes (it doesn't need to be a Delaunay triangulation). We use ``mapbox_earcut`` by default. If it is not installed, we allow fallback to ``pypoly2tri`` for historical reasons (we originally used the GPC library, which is not free for commercial use, falling back to ``pypoly2tri`` if not installed). Args: polygon (shapely.geometry.Polygon): Polygon to triangulate. Returns: A list of disjoint (except for edges) triangles whose union is the original polygon. """ try: return triangulatePolygon_mapbox(polygon) except ImportError: pass try: return triangulatePolygon_pypoly2tri(polygon) except ImportError: pass raise RuntimeError('no triangulation libraries installed ' '(did you uninstall mapbox_earcut?)')
def triangulatePolygon_pypoly2tri(polygon): import pypoly2tri polyline = [] for c in polygon.exterior.coords[:-1]: polyline.append(pypoly2tri.shapes.Point(c[0],c[1])) cdt = pypoly2tri.cdt.CDT(polyline) for i in polygon.interiors: polyline = [] for c in i.coords[:-1]: polyline.append(pypoly2tri.shapes.Point(c[0],c[1])) cdt.AddHole(polyline) try: cdt.Triangulate() except RecursionError: # polygon too big for pypoly2tri raise TriangulationError('pypoly2tri unable to triangulate large polygon; for ' 'non-commercial use, try "pip install Polygon3"') triangles = list() for t in cdt.GetTriangles(): triangles.append(shapely.geometry.Polygon([ t.GetPoint(0).toTuple(), t.GetPoint(1).toTuple(), t.GetPoint(2).toTuple() ])) return triangles def triangulatePolygon_mapbox(polygon): import mapbox_earcut vertices, rings = [], [] ring = polygon.exterior.coords[:-1] # despite 'ring' name, actually need a chain vertices.extend(ring) rings.append(len(vertices)) for interior in polygon.interiors: ring = interior.coords[:-1] vertices.extend(ring) rings.append(len(vertices)) vertices = np.array(vertices, dtype=np.float64) rings = np.array(rings) result = mapbox_earcut.triangulate_float64(vertices, rings) triangles = [] points = vertices[result] its = [iter(points)] * 3 for triple in zip(*its): triangles.append(shapely.geometry.Polygon(triple)) return triangles def allChains(polygon): if polygon.is_empty: return if isinstance(polygon, (shapely.geometry.MultiPolygon, shapely.geometry.MultiLineString, shapely.geometry.MultiPoint, shapely.geometry.collection.GeometryCollection)): polygons = polygon.geoms else: polygons = [polygon] for polygon in polygons: if isinstance(polygon, shapely.geometry.Polygon): yield polygon.exterior for ring in polygon.interiors: yield ring elif isinstance(polygon, (shapely.geometry.LineString, shapely.geometry.LinearRing, shapely.geometry.Point)): yield polygon else: raise RuntimeError(f'unknown kind of shapely geometry {polygon}') def plotPolygon(polygon, plt, style='r-', **kwargs): for chain in allChains(polygon): x, y = chain.xy plt.plot(x, y, style, **kwargs)
[docs]class _RotatedRectangle: """mixin providing collision detection for rectangular objects and regions""" def containsPoint(self, point): pt = shapely.geometry.Point(point) return self.polygon.intersects(pt) def intersects(self, rect): return self.polygon.intersects(rect.polygon) @cached_property def polygon(self): position, heading, hw, hl = self.position, self.heading, self.hw, self.hl if any(needsSampling(c) or needsLazyEvaluation(c) for c in (position, heading, hw, hl)): return None # can only convert fixed Regions to Polygons corners = _RotatedRectangle.makeCorners(position.x, position.y, heading, hw, hl) return shapely.geometry.Polygon(corners) @staticmethod def makeCorners(px, py, heading, hw, hl): s, c = sin(heading), cos(heading) s_hw, c_hw = s*hw, c*hw s_hl, c_hl = s*hl, c*hl corners = ( (px + c_hw - s_hl, py + s_hw + c_hl), (px - c_hw - s_hl, py - s_hw + c_hl), (px - c_hw + s_hl, py - s_hw - c_hl), (px + c_hw + s_hl, py + s_hw - c_hl) ) return corners