Source code for scenic.core.geometry

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

import itertools
import math
import warnings

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

from scenic.core.distributions import (
    distributionFunction,
    monotonicDistributionFunction,
    needsSampling,
)
from scenic.core.lazy_eval import isLazy
from scenic.core.utils import cached_property


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


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


@monotonicDistributionFunction
def hypot(*args) -> float:
    return math.hypot(*args)


@monotonicDistributionFunction
def max(*args, **kwargs):
    return __builtins__["max"](*args, **kwargs)


@monotonicDistributionFunction
def min(*args, **kwargs):
    return __builtins__["min"](*args, **kwargs)


@distributionFunction
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 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 headingOfSegment(pointA, pointB):
    ax, ay = pointA[:2]
    bx, by = pointB[:2]
    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[:2]
    ox, oy = point[:2]
    a = (heading + (math.pi / 2.0)) - math.atan2(oy - y, ox - x)
    return normalizeAngle(a)


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


# Fastest known way to make a Shapely Point from a list/tuple/Vector
makeShapelyPoint = shapely.lib.points


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 https://github.com/Toblerity/Shapely/issues/1230)
    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
    return union


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:
                kept.append(poly)
        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:
                ints.append(interior)
    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:
            newChain.append(b)
            a = b
    if closed:
        b = chain[0]
        dx, dy = b[0] - a[0], b[1] - a[1]
        if (dx * dx) + (dy * dy) <= tol2:
            newChain.pop()
        newChain.append(chain[0])
    if len(newChain) <= minLength:
        if not closed:
            if len(newChain) > 1:
                newChain.pop()
            newChain.append(chain[-1])
        return newChain
    # collapse collinear points
    chain = newChain
    if closed:
        a = chain[-2]
        b = chain[0]
        ci = 1
        newChain = []
    else:
        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:
            newChain.append(b)
            a = b
            b = c
        else:
            b = c
    newChain.append(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
    else:
        return shapely.geometry.Polygon(polygon.exterior)


[docs]class TriangulationError(RuntimeError): """Signals that the installed triangulation libraries are insufficient.""" 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). 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 raise RuntimeError( "no triangulation libraries installed " "(did you uninstall mapbox_earcut?)" )
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)[:, :2] rings = np.array(rings) result = mapbox_earcut.triangulate_float64(vertices, rings) triangles = [] points = vertices[result] coords = np.split(points, len(points) / 3) triangles = shapely.polygons(coords) 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)