477 lines
18 KiB
Python
477 lines
18 KiB
Python
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass, field
|
|
from typing import Literal
|
|
|
|
import numpy
|
|
from shapely.affinity import rotate as shapely_rotate
|
|
from shapely.affinity import scale as shapely_scale
|
|
from shapely.affinity import translate as shapely_translate
|
|
from shapely.geometry import Polygon, box
|
|
|
|
from inire.constants import TOLERANCE_ANGULAR
|
|
from inire.seeds import Bend90Seed, PathSegmentSeed, SBendSeed, StraightSeed
|
|
from .primitives import Port, rotation_matrix2
|
|
|
|
|
|
MoveKind = Literal["straight", "bend90", "sbend"]
|
|
BendCollisionModelName = Literal["arc", "bbox", "clipped_bbox"]
|
|
BendCollisionModel = BendCollisionModelName | Polygon
|
|
BendPhysicalGeometry = Literal["arc"] | Polygon
|
|
|
|
|
|
def _normalize_length(value: float) -> float:
|
|
return float(value)
|
|
|
|
|
|
@dataclass(frozen=True, slots=True)
|
|
class ComponentResult:
|
|
start_port: Port
|
|
collision_geometry: tuple[Polygon, ...]
|
|
end_port: Port
|
|
length: float
|
|
move_type: MoveKind
|
|
move_spec: PathSegmentSeed
|
|
physical_geometry: tuple[Polygon, ...]
|
|
dilated_collision_geometry: tuple[Polygon, ...]
|
|
dilated_physical_geometry: tuple[Polygon, ...]
|
|
_bounds: tuple[tuple[float, float, float, float], ...] = field(init=False, repr=False)
|
|
_total_bounds: tuple[float, float, float, float] = field(init=False, repr=False)
|
|
_dilated_bounds: tuple[tuple[float, float, float, float], ...] = field(init=False, repr=False)
|
|
_total_dilated_bounds: tuple[float, float, float, float] = field(init=False, repr=False)
|
|
|
|
def __post_init__(self) -> None:
|
|
collision_geometry = tuple(self.collision_geometry)
|
|
physical_geometry = tuple(self.physical_geometry)
|
|
dilated_collision_geometry = tuple(self.dilated_collision_geometry)
|
|
dilated_physical_geometry = tuple(self.dilated_physical_geometry)
|
|
|
|
object.__setattr__(self, "collision_geometry", collision_geometry)
|
|
object.__setattr__(self, "physical_geometry", physical_geometry)
|
|
object.__setattr__(self, "dilated_collision_geometry", dilated_collision_geometry)
|
|
object.__setattr__(self, "dilated_physical_geometry", dilated_physical_geometry)
|
|
object.__setattr__(self, "length", float(self.length))
|
|
|
|
bounds = tuple(poly.bounds for poly in collision_geometry)
|
|
object.__setattr__(self, "_bounds", bounds)
|
|
object.__setattr__(self, "_total_bounds", _combine_bounds(list(bounds)))
|
|
|
|
dilated_bounds = tuple(poly.bounds for poly in dilated_collision_geometry)
|
|
object.__setattr__(self, "_dilated_bounds", dilated_bounds)
|
|
object.__setattr__(self, "_total_dilated_bounds", _combine_bounds(list(dilated_bounds)))
|
|
|
|
@property
|
|
def bounds(self) -> tuple[tuple[float, float, float, float], ...]:
|
|
return self._bounds
|
|
|
|
@property
|
|
def total_bounds(self) -> tuple[float, float, float, float]:
|
|
return self._total_bounds
|
|
|
|
@property
|
|
def dilated_bounds(self) -> tuple[tuple[float, float, float, float], ...]:
|
|
return self._dilated_bounds
|
|
|
|
@property
|
|
def total_dilated_bounds(self) -> tuple[float, float, float, float]:
|
|
return self._total_dilated_bounds
|
|
|
|
def translate(self, dx: int | float, dy: int | float) -> ComponentResult:
|
|
return ComponentResult(
|
|
start_port=self.start_port.translate(dx, dy),
|
|
collision_geometry=[shapely_translate(poly, dx, dy) for poly in self.collision_geometry],
|
|
end_port=self.end_port.translate(dx, dy),
|
|
length=self.length,
|
|
move_type=self.move_type,
|
|
move_spec=self.move_spec,
|
|
physical_geometry=[shapely_translate(poly, dx, dy) for poly in self.physical_geometry],
|
|
dilated_collision_geometry=[shapely_translate(poly, dx, dy) for poly in self.dilated_collision_geometry],
|
|
dilated_physical_geometry=[shapely_translate(poly, dx, dy) for poly in self.dilated_physical_geometry],
|
|
)
|
|
|
|
|
|
def _combine_bounds(bounds_list: list[tuple[float, float, float, float]]) -> tuple[float, float, float, float]:
|
|
arr = numpy.asarray(bounds_list, dtype=numpy.float64)
|
|
return (
|
|
float(arr[:, 0].min()),
|
|
float(arr[:, 1].min()),
|
|
float(arr[:, 2].max()),
|
|
float(arr[:, 3].max()),
|
|
)
|
|
|
|
|
|
def _get_num_segments(radius: float, angle_deg: float, sagitta: float = 0.01) -> int:
|
|
if radius <= 0:
|
|
return 1
|
|
ratio = max(0.0, min(1.0, 1.0 - sagitta / radius))
|
|
theta_max = 2.0 * numpy.arccos(ratio)
|
|
if theta_max < TOLERANCE_ANGULAR:
|
|
return 16
|
|
num = int(numpy.ceil(numpy.radians(abs(angle_deg)) / theta_max))
|
|
return max(8, num)
|
|
|
|
|
|
def _get_arc_polygons(
|
|
cxy: tuple[float, float],
|
|
radius: float,
|
|
width: float,
|
|
ts: tuple[float, float],
|
|
sagitta: float = 0.01,
|
|
dilation: float = 0.0,
|
|
) -> list[Polygon]:
|
|
t_start, t_end = numpy.radians(ts[0]), numpy.radians(ts[1])
|
|
num_segments = _get_num_segments(radius, abs(ts[1] - ts[0]), sagitta)
|
|
angles = numpy.linspace(t_start, t_end, num_segments + 1)
|
|
|
|
cx, cy = cxy
|
|
inner_radius = radius - width / 2.0 - dilation
|
|
outer_radius = radius + width / 2.0 + dilation
|
|
|
|
cos_a = numpy.cos(angles)
|
|
sin_a = numpy.sin(angles)
|
|
|
|
inner_points = numpy.column_stack((cx + inner_radius * cos_a, cy + inner_radius * sin_a))
|
|
outer_points = numpy.column_stack((cx + outer_radius * cos_a[::-1], cy + outer_radius * sin_a[::-1]))
|
|
return [Polygon(numpy.concatenate((inner_points, outer_points), axis=0))]
|
|
|
|
|
|
def _clip_bbox_legacy(
|
|
cxy: tuple[float, float],
|
|
radius: float,
|
|
width: float,
|
|
ts: tuple[float, float],
|
|
clip_margin: float,
|
|
) -> Polygon:
|
|
arc_poly = _get_arc_polygons(cxy, radius, width, ts)[0]
|
|
minx, miny, maxx, maxy = arc_poly.bounds
|
|
bbox_poly = box(minx, miny, maxx, maxy)
|
|
shrink = min(clip_margin, max(radius, width))
|
|
return bbox_poly.buffer(-shrink, join_style=2) if shrink > 0 else bbox_poly
|
|
|
|
|
|
def _clip_bbox_polygonal(cxy: tuple[float, float], radius: float, width: float, ts: tuple[float, float]) -> Polygon:
|
|
"""Return a conservative 8-point polygonal proxy for the arc.
|
|
|
|
The polygon uses 4 points along the outer edge and 4 along the inner edge.
|
|
The outer edge is a circumscribed polyline and the inner edge is an
|
|
inscribed polyline, so the result conservatively contains the true arc.
|
|
"""
|
|
cx, cy = cxy
|
|
sample_count = 4
|
|
angle_span = abs(float(ts[1]) - float(ts[0]))
|
|
if angle_span < TOLERANCE_ANGULAR:
|
|
return box(*_get_arc_polygons(cxy, radius, width, ts)[0].bounds)
|
|
|
|
segment_half_angle = numpy.radians(angle_span / (2.0 * (sample_count - 1)))
|
|
cos_half = max(float(numpy.cos(segment_half_angle)), 1e-9)
|
|
|
|
inner_radius = max(0.0, radius - width / 2.0)
|
|
outer_radius = radius + width / 2.0
|
|
tolerance = max(1e-3, radius * 1e-4)
|
|
conservative_inner_radius = max(0.0, inner_radius * cos_half - tolerance)
|
|
conservative_outer_radius = outer_radius / cos_half + tolerance
|
|
|
|
angles = numpy.radians(numpy.linspace(ts[0], ts[1], sample_count))
|
|
cos_a = numpy.cos(angles)
|
|
sin_a = numpy.sin(angles)
|
|
|
|
outer_points = numpy.column_stack((cx + conservative_outer_radius * cos_a, cy + conservative_outer_radius * sin_a))
|
|
inner_points = numpy.column_stack((cx + conservative_inner_radius * cos_a[::-1], cy + conservative_inner_radius * sin_a[::-1]))
|
|
return Polygon(numpy.concatenate((outer_points, inner_points), axis=0))
|
|
|
|
|
|
def _clip_bbox(
|
|
cxy: tuple[float, float],
|
|
radius: float,
|
|
width: float,
|
|
ts: tuple[float, float],
|
|
clip_margin: float | None,
|
|
) -> Polygon:
|
|
if clip_margin is not None:
|
|
return _clip_bbox_legacy(cxy, radius, width, ts, clip_margin)
|
|
return _clip_bbox_polygonal(cxy, radius, width, ts)
|
|
|
|
|
|
def _transform_custom_collision_polygon(
|
|
collision_poly: Polygon,
|
|
cxy: tuple[float, float],
|
|
rotation_deg: float,
|
|
mirror_y: bool,
|
|
) -> Polygon:
|
|
poly = collision_poly
|
|
if mirror_y:
|
|
poly = shapely_scale(poly, xfact=1.0, yfact=-1.0, origin=(0.0, 0.0))
|
|
if rotation_deg % 360:
|
|
poly = shapely_rotate(poly, rotation_deg, origin=(0.0, 0.0), use_radians=False)
|
|
return shapely_translate(poly, cxy[0], cxy[1])
|
|
|
|
|
|
def _apply_collision_model(
|
|
arc_poly: Polygon,
|
|
collision_type: BendCollisionModel,
|
|
radius: float,
|
|
width: float,
|
|
cxy: tuple[float, float],
|
|
ts: tuple[float, float],
|
|
clip_margin: float | None = None,
|
|
rotation_deg: float = 0.0,
|
|
mirror_y: bool = False,
|
|
) -> list[Polygon]:
|
|
if isinstance(collision_type, Polygon):
|
|
return [_transform_custom_collision_polygon(collision_type, cxy, rotation_deg, mirror_y)]
|
|
if collision_type == "arc":
|
|
return [arc_poly]
|
|
if collision_type == "clipped_bbox":
|
|
clipped = _clip_bbox(cxy, radius, width, ts, clip_margin)
|
|
return [clipped if not clipped.is_empty else box(*arc_poly.bounds)]
|
|
return [box(*arc_poly.bounds)]
|
|
|
|
|
|
class Straight:
|
|
@staticmethod
|
|
def generate(
|
|
start_port: Port,
|
|
length: float,
|
|
width: float,
|
|
dilation: float = 0.0,
|
|
) -> ComponentResult:
|
|
rot2 = rotation_matrix2(start_port.r)
|
|
length_f = _normalize_length(length)
|
|
disp = rot2 @ numpy.array((length_f, 0.0))
|
|
end_port = Port(start_port.x + disp[0], start_port.y + disp[1], start_port.r)
|
|
|
|
half_w = width / 2.0
|
|
pts = numpy.array(((0.0, half_w), (length_f, half_w), (length_f, -half_w), (0.0, -half_w)))
|
|
poly_points = (pts @ rot2.T) + numpy.array((start_port.x, start_port.y))
|
|
geometry = [Polygon(poly_points)]
|
|
|
|
if dilation > 0:
|
|
half_w_d = half_w + dilation
|
|
pts_d = numpy.array(
|
|
(
|
|
(-dilation, half_w_d),
|
|
(length_f + dilation, half_w_d),
|
|
(length_f + dilation, -half_w_d),
|
|
(-dilation, -half_w_d),
|
|
)
|
|
)
|
|
poly_points_d = (pts_d @ rot2.T) + numpy.array((start_port.x, start_port.y))
|
|
dilated_geometry = [Polygon(poly_points_d)]
|
|
else:
|
|
dilated_geometry = geometry
|
|
|
|
return ComponentResult(
|
|
start_port=start_port,
|
|
collision_geometry=geometry,
|
|
end_port=end_port,
|
|
length=abs(length_f),
|
|
move_type="straight",
|
|
move_spec=StraightSeed(length=length_f),
|
|
physical_geometry=geometry,
|
|
dilated_collision_geometry=dilated_geometry,
|
|
dilated_physical_geometry=dilated_geometry,
|
|
)
|
|
|
|
|
|
class Bend90:
|
|
@staticmethod
|
|
def generate(
|
|
start_port: Port,
|
|
radius: float,
|
|
width: float,
|
|
direction: Literal["CW", "CCW"],
|
|
sagitta: float = 0.01,
|
|
collision_type: BendCollisionModel = "arc",
|
|
physical_geometry_type: BendPhysicalGeometry = "arc",
|
|
clip_margin: float | None = None,
|
|
dilation: float = 0.0,
|
|
) -> ComponentResult:
|
|
rot2 = rotation_matrix2(start_port.r)
|
|
sign = 1 if direction == "CCW" else -1
|
|
|
|
center_local = numpy.array((0.0, sign * radius))
|
|
end_local = numpy.array((radius, sign * radius))
|
|
center_xy = (rot2 @ center_local) + numpy.array((start_port.x, start_port.y))
|
|
end_xy = (rot2 @ end_local) + numpy.array((start_port.x, start_port.y))
|
|
end_port = Port(end_xy[0], end_xy[1], start_port.r + sign * 90)
|
|
|
|
start_theta = start_port.r - sign * 90
|
|
end_theta = start_port.r
|
|
ts = (float(start_theta), float(end_theta))
|
|
|
|
arc_polys = _get_arc_polygons((float(center_xy[0]), float(center_xy[1])), radius, width, ts, sagitta)
|
|
collision_polys = _apply_collision_model(
|
|
arc_polys[0],
|
|
collision_type,
|
|
radius,
|
|
width,
|
|
(float(center_xy[0]), float(center_xy[1])),
|
|
ts,
|
|
clip_margin=clip_margin,
|
|
rotation_deg=float(start_port.r),
|
|
mirror_y=(sign < 0),
|
|
)
|
|
|
|
if isinstance(physical_geometry_type, Polygon):
|
|
physical_geometry = _apply_collision_model(
|
|
arc_polys[0],
|
|
physical_geometry_type,
|
|
radius,
|
|
width,
|
|
(float(center_xy[0]), float(center_xy[1])),
|
|
ts,
|
|
rotation_deg=float(start_port.r),
|
|
mirror_y=(sign < 0),
|
|
)
|
|
uses_physical_custom_geometry = True
|
|
else:
|
|
physical_geometry = arc_polys
|
|
uses_physical_custom_geometry = False
|
|
if dilation > 0:
|
|
if uses_physical_custom_geometry:
|
|
dilated_physical_geometry = [poly.buffer(dilation) for poly in physical_geometry]
|
|
else:
|
|
dilated_physical_geometry = _get_arc_polygons(
|
|
(float(center_xy[0]), float(center_xy[1])),
|
|
radius,
|
|
width,
|
|
ts,
|
|
sagitta,
|
|
dilation=dilation,
|
|
)
|
|
dilated_collision_geometry = (
|
|
dilated_physical_geometry if collision_type == "arc" else [poly.buffer(dilation) for poly in collision_polys]
|
|
)
|
|
else:
|
|
dilated_physical_geometry = physical_geometry
|
|
dilated_collision_geometry = collision_polys
|
|
|
|
return ComponentResult(
|
|
start_port=start_port,
|
|
collision_geometry=collision_polys,
|
|
end_port=end_port,
|
|
length=abs(radius) * numpy.pi / 2.0,
|
|
move_type="bend90",
|
|
move_spec=Bend90Seed(radius=radius, direction=direction),
|
|
physical_geometry=physical_geometry,
|
|
dilated_collision_geometry=dilated_collision_geometry,
|
|
dilated_physical_geometry=dilated_physical_geometry,
|
|
)
|
|
|
|
|
|
class SBend:
|
|
@staticmethod
|
|
def generate(
|
|
start_port: Port,
|
|
offset: float,
|
|
radius: float,
|
|
width: float,
|
|
sagitta: float = 0.01,
|
|
collision_type: BendCollisionModel = "arc",
|
|
physical_geometry_type: BendPhysicalGeometry = "arc",
|
|
clip_margin: float | None = None,
|
|
dilation: float = 0.0,
|
|
) -> ComponentResult:
|
|
if abs(offset) >= 2 * radius:
|
|
raise ValueError(f"SBend offset {offset} must be less than 2*radius {2 * radius}")
|
|
|
|
sign = 1 if offset >= 0 else -1
|
|
theta = numpy.arccos(1.0 - abs(offset) / (2.0 * radius))
|
|
dx = 2.0 * radius * numpy.sin(theta)
|
|
theta_deg = float(numpy.degrees(theta))
|
|
|
|
rot2 = rotation_matrix2(start_port.r)
|
|
end_local = numpy.array((dx, offset))
|
|
end_xy = (rot2 @ end_local) + numpy.array((start_port.x, start_port.y))
|
|
end_port = Port(end_xy[0], end_xy[1], start_port.r)
|
|
|
|
c1_local = numpy.array((0.0, sign * radius))
|
|
c2_local = numpy.array((dx, offset - sign * radius))
|
|
c1_xy = (rot2 @ c1_local) + numpy.array((start_port.x, start_port.y))
|
|
c2_xy = (rot2 @ c2_local) + numpy.array((start_port.x, start_port.y))
|
|
|
|
ts1 = (float(start_port.r - sign * 90), float(start_port.r - sign * 90 + sign * theta_deg))
|
|
second_base = start_port.r + (90 if sign > 0 else 270)
|
|
ts2 = (float(second_base + sign * theta_deg), float(second_base))
|
|
|
|
arc1 = _get_arc_polygons((float(c1_xy[0]), float(c1_xy[1])), radius, width, ts1, sagitta)[0]
|
|
arc2 = _get_arc_polygons((float(c2_xy[0]), float(c2_xy[1])), radius, width, ts2, sagitta)[0]
|
|
actual_geometry = [arc1, arc2]
|
|
geometry = [
|
|
_apply_collision_model(
|
|
arc1,
|
|
collision_type,
|
|
radius,
|
|
width,
|
|
(float(c1_xy[0]), float(c1_xy[1])),
|
|
ts1,
|
|
clip_margin=clip_margin,
|
|
rotation_deg=float(start_port.r),
|
|
mirror_y=(sign < 0),
|
|
)[0],
|
|
_apply_collision_model(
|
|
arc2,
|
|
collision_type,
|
|
radius,
|
|
width,
|
|
(float(c2_xy[0]), float(c2_xy[1])),
|
|
ts2,
|
|
clip_margin=clip_margin,
|
|
rotation_deg=float(start_port.r),
|
|
mirror_y=(sign > 0),
|
|
)[0],
|
|
]
|
|
|
|
if isinstance(physical_geometry_type, Polygon):
|
|
physical_geometry = [
|
|
_apply_collision_model(
|
|
arc1,
|
|
physical_geometry_type,
|
|
radius,
|
|
width,
|
|
(float(c1_xy[0]), float(c1_xy[1])),
|
|
ts1,
|
|
rotation_deg=float(start_port.r),
|
|
mirror_y=(sign < 0),
|
|
)[0],
|
|
_apply_collision_model(
|
|
arc2,
|
|
physical_geometry_type,
|
|
radius,
|
|
width,
|
|
(float(c2_xy[0]), float(c2_xy[1])),
|
|
ts2,
|
|
rotation_deg=float(start_port.r),
|
|
mirror_y=(sign > 0),
|
|
)[0],
|
|
]
|
|
uses_physical_custom_geometry = True
|
|
else:
|
|
physical_geometry = actual_geometry
|
|
uses_physical_custom_geometry = False
|
|
if dilation > 0:
|
|
if uses_physical_custom_geometry:
|
|
dilated_physical_geometry = [poly.buffer(dilation) for poly in physical_geometry]
|
|
else:
|
|
dilated_physical_geometry = [
|
|
_get_arc_polygons((float(c1_xy[0]), float(c1_xy[1])), radius, width, ts1, sagitta, dilation=dilation)[0],
|
|
_get_arc_polygons((float(c2_xy[0]), float(c2_xy[1])), radius, width, ts2, sagitta, dilation=dilation)[0],
|
|
]
|
|
dilated_collision_geometry = (
|
|
dilated_physical_geometry if collision_type == "arc" else [poly.buffer(dilation) for poly in geometry]
|
|
)
|
|
else:
|
|
dilated_physical_geometry = physical_geometry
|
|
dilated_collision_geometry = geometry
|
|
|
|
return ComponentResult(
|
|
start_port=start_port,
|
|
collision_geometry=geometry,
|
|
end_port=end_port,
|
|
length=2.0 * radius * theta,
|
|
move_type="sbend",
|
|
move_spec=SBendSeed(offset=offset, radius=radius),
|
|
physical_geometry=physical_geometry,
|
|
dilated_collision_geometry=dilated_collision_geometry,
|
|
dilated_physical_geometry=dilated_physical_geometry,
|
|
)
|