diff --git a/examples/03_locked_paths.png b/examples/03_locked_paths.png index 099ffc2..45db494 100644 Binary files a/examples/03_locked_paths.png and b/examples/03_locked_paths.png differ diff --git a/examples/04_sbends_and_radii.png b/examples/04_sbends_and_radii.png index ff14c26..5f23318 100644 Binary files a/examples/04_sbends_and_radii.png and b/examples/04_sbends_and_radii.png differ diff --git a/inire/geometry/collision.py b/inire/geometry/collision.py index 5fd5531..887bf66 100644 --- a/inire/geometry/collision.py +++ b/inire/geometry/collision.py @@ -178,26 +178,28 @@ class CollisionEngine: for obj_id in candidates: if self.static_prepared[obj_id].intersects(geometry): if start_port or end_port: - # Safety zone check: requires intersection of DILATED query and RAW obstacle. - # Always re-buffer here because static check needs full clearance dilation, - # whereas the provided dilated_geometry is usually clearance/2. - dilation = self.clearance - test_poly = geometry.buffer(dilation) + # Optimization: Instead of expensive buffer + intersection, + # use distance() and check if it's within clearance only near ports. + raw_obstacle = self.static_geometries[obj_id] + # If the intersection is within clearance, distance will be < clearance. + # We already know it intersects the dilated obstacle, so distance < clearance. - intersection = test_poly.intersection(self.static_geometries[obj_id]) - if intersection.is_empty: - continue - - ix_minx, ix_miny, ix_maxx, ix_maxy = intersection.bounds - is_safe = False - for p in [start_port, end_port]: - if p and (abs(ix_minx - p.x) < self.safety_zone_radius and - abs(ix_maxx - p.x) < self.safety_zone_radius and - abs(ix_miny - p.y) < self.safety_zone_radius and - abs(ix_maxy - p.y) < self.safety_zone_radius): - is_safe = True - break + sz = self.safety_zone_radius + + # Use intersection bounds to check proximity to ports + # We need the intersection of the geometry and the RAW obstacle + intersection = geometry.intersection(raw_obstacle) + if not intersection.is_empty: + ix_minx, ix_miny, ix_maxx, ix_maxy = intersection.bounds + for p in [start_port, end_port]: + if p and (abs(ix_minx - p.x) < sz and + abs(ix_maxx - p.x) < sz and + abs(ix_miny - p.y) < sz and + abs(ix_maxy - p.y) < sz): + is_safe = True + break + if is_safe: continue return True diff --git a/inire/geometry/components.py b/inire/geometry/components.py index e81102d..6dd0c9b 100644 --- a/inire/geometry/components.py +++ b/inire/geometry/components.py @@ -30,7 +30,7 @@ class ComponentResult: """ The result of a component generation: geometry, final port, and physical length. """ - __slots__ = ('geometry', 'dilated_geometry', 'end_port', 'length') + __slots__ = ('geometry', 'dilated_geometry', 'end_port', 'length', 'bounds', 'dilated_bounds') geometry: list[Polygon] """ List of polygons representing the component geometry """ @@ -44,6 +44,12 @@ class ComponentResult: length: float """ Physical length of the component path """ + bounds: list[tuple[float, float, float, float]] + """ Pre-calculated bounds for each polygon in geometry """ + + dilated_bounds: list[tuple[float, float, float, float]] | None + """ Pre-calculated bounds for each polygon in dilated_geometry """ + def __init__( self, geometry: list[Polygon], @@ -55,6 +61,22 @@ class ComponentResult: self.dilated_geometry = dilated_geometry self.end_port = end_port self.length = length + self.bounds = [p.bounds for p in geometry] + self.dilated_bounds = [p.bounds for p in dilated_geometry] if dilated_geometry else None + + def translate(self, dx: float, dy: float) -> ComponentResult: + """ + Create a new ComponentResult translated by (dx, dy). + """ + from shapely.affinity import translate + new_geom = [translate(p, dx, dy) for p in self.geometry] + new_dil = [translate(p, dx, dy) for p in self.dilated_geometry] if self.dilated_geometry else None + new_port = Port(self.end_port.x + dx, self.end_port.y + dy, self.end_port.orientation) + return ComponentResult(new_geom, new_port, self.length, new_dil) + + + + class Straight: @@ -158,6 +180,7 @@ def _get_arc_polygons( t_start: float, t_end: float, sagitta: float = 0.01, + dilation: float = 0.0, ) -> list[Polygon]: """ Helper to generate arc-shaped polygons using vectorized NumPy operations. @@ -168,6 +191,7 @@ def _get_arc_polygons( width: Waveguide width. t_start, t_end: Start and end angles (radians). sagitta: Geometric fidelity. + dilation: Optional dilation to apply directly to the arc. Returns: List containing the arc polygon. @@ -178,8 +202,8 @@ def _get_arc_polygons( cos_a = numpy.cos(angles) sin_a = numpy.sin(angles) - inner_radius = radius - width / 2.0 - outer_radius = radius + width / 2.0 + inner_radius = radius - width / 2.0 - dilation + outer_radius = radius + width / 2.0 + dilation inner_points = numpy.stack([cx + inner_radius * cos_a, cy + inner_radius * sin_a], axis=1) outer_points = numpy.stack([cx + outer_radius * cos_a[::-1], cy + outer_radius * sin_a[::-1]], axis=1) @@ -200,21 +224,9 @@ def _clip_bbox( arc_poly: Polygon, ) -> Polygon: """ - Clips corners of a bounding box for better collision modeling. - - Args: - bbox: Initial bounding box. - cx, cy: Arc center. - radius: Arc radius. - width: Waveguide width. - clip_margin: Minimum distance from waveguide. - arc_poly: The original arc polygon. - - Returns: - The clipped polygon. + Clips corners of a bounding box for better collision modeling using direct vertex manipulation. """ - res_poly = bbox - # Determine quadrant signs from arc centroid relative to center + # Determination of which corners to clip ac = arc_poly.centroid qsx = 1.0 if ac.x >= cx else -1.0 qsy = 1.0 if ac.y >= cy else -1.0 @@ -223,46 +235,54 @@ def _clip_bbox( r_in_cut = radius - width / 2.0 - clip_margin minx, miny, maxx, maxy = bbox.bounds - corners = [(minx, miny), (minx, maxy), (maxx, miny), (maxx, maxy)] - for px, py in corners: - dx, dy = px - cx, py - cy + # Initial vertices: [minx,miny], [maxx,miny], [maxx,maxy], [minx,maxy] + verts = [ + numpy.array([minx, miny]), + numpy.array([maxx, miny]), + numpy.array([maxx, maxy]), + numpy.array([minx, maxy]) + ] + + new_verts = [] + for p in verts: + dx, dy = p[0] - cx, p[1] - cy dist = numpy.sqrt(dx**2 + dy**2) - # Check if corner is far enough to be clipped - if dist > r_out_cut: - # Outer corner: remove part furthest from center - # To be conservative, line is at distance r_out_cut from center. - # Equation: sx*x + sy*y = sx*cx + sy*cy + r_out_cut * sqrt(2) - d_line = r_out_cut * numpy.sqrt(2) - elif r_in_cut > 0 and dist < r_in_cut: - # Inner corner: remove part closest to center - # To be safe, line intercept must not exceed r_in_cut. - # Equation: sx*x + sy*y = sx*cx + sy*cy + r_in_cut - d_line = r_in_cut - else: - continue - # Normal vector components from center to corner - # Using rounded signs for stability sx = 1.0 if dx > 1e-6 else (-1.0 if dx < -1e-6 else qsx) sy = 1.0 if dy > 1e-6 else (-1.0 if dy < -1e-6 else qsy) - # val calculation based on d_line - val = sx * cx + sy * cy + d_line + d_line = -1.0 + if dist > r_out_cut: + d_line = r_out_cut * numpy.sqrt(2) + elif r_in_cut > 0 and dist < r_in_cut: + d_line = r_in_cut + + if d_line > 0: + # This corner needs clipping. Replace one vertex with two at intersection of line and edges. + # Line: sx*(x-cx) + sy*(y-cy) = d_line + # Edge x=px: y = cy + (d_line - sx*(px-cx))/sy + # Edge y=py: x = cx + (d_line - sy*(py-cy))/sx + try: + p_edge_x = numpy.array([p[0], cy + (d_line - sx * (p[0] - cx)) / sy]) + p_edge_y = numpy.array([cx + (d_line - sy * (p[1] - cy)) / sx, p[1]]) + # Order matters for polygon winding. + # If we are at [minx, miny] and moving CCW towards [maxx, miny]: + # If we clip this corner, we should add p_edge_y then p_edge_x (or vice versa depending on orientation) + # For simplicity, we can just add both and let Polygon sort it out if it's convex, + # but better to be precise. + # Since we know the bounding box orientation, we can determine order. + # BUT: Difference was safer. Let's try a simpler approach: + # Just collect all possible vertices and use convex_hull if it's guaranteed convex. + # A clipped bbox is always convex. + new_verts.append(p_edge_x) + new_verts.append(p_edge_y) + except ZeroDivisionError: + new_verts.append(p) + else: + new_verts.append(p) - try: - # Create a triangle to remove. - # Vertices: corner, intersection with x=px edge, intersection with y=py edge - p1 = (px, py) - p2 = (px, (val - sx * px) / sy) - p3 = ((val - sy * py) / sx, py) - - triangle = Polygon([p1, p2, p3]) - if triangle.is_valid and triangle.area > 1e-9: - res_poly = cast('Polygon', res_poly.difference(triangle)) - except ZeroDivisionError: - continue - return res_poly + return Polygon(new_verts).convex_hull def _apply_collision_model( @@ -355,7 +375,13 @@ class Bend90: arc_polys[0], collision_type, radius, width, cx, cy, clip_margin ) - dilated_geom = [p.buffer(dilation) for p in collision_polys] if dilation > 0 else None + dilated_geom = None + if dilation > 0: + if collision_type == "arc": + dilated_geom = _get_arc_polygons(cx, cy, radius, width, t_start, t_end, sagitta, dilation=dilation) + else: + # For bbox or clipped_bbox, buffer the model itself (which is simpler than buffering the high-fidelity arc) + dilated_geom = [p.buffer(dilation) for p in collision_polys] return ComponentResult( geometry=collision_polys, @@ -436,7 +462,14 @@ class SBend: col2 = _apply_collision_model(arc2, collision_type, radius, width, cx2, cy2, clip_margin)[0] collision_polys = [col1, col2] - dilated_geom = [p.buffer(dilation) for p in collision_polys] if dilation > 0 else None + dilated_geom = None + if dilation > 0: + if collision_type == "arc": + d1 = _get_arc_polygons(cx1, cy1, radius, width, ts1, te1, sagitta, dilation=dilation)[0] + d2 = _get_arc_polygons(cx2, cy2, radius, width, ts2, te2, sagitta, dilation=dilation)[0] + dilated_geom = [d1, d2] + else: + dilated_geom = [p.buffer(dilation) for p in collision_polys] return ComponentResult( geometry=collision_polys, diff --git a/inire/router/astar.py b/inire/router/astar.py index 74ef3b1..dc45382 100644 --- a/inire/router/astar.py +++ b/inire/router/astar.py @@ -3,16 +3,17 @@ from __future__ import annotations import heapq import logging import functools -from typing import TYPE_CHECKING, Literal +from typing import TYPE_CHECKING, Literal, Any +import rtree import numpy from inire.geometry.components import Bend90, SBend, Straight +from inire.geometry.primitives import Port from inire.router.config import RouterConfig if TYPE_CHECKING: from inire.geometry.components import ComponentResult - from inire.geometry.primitives import Port from inire.router.cost import CostEvaluator logger = logging.getLogger(__name__) @@ -65,6 +66,8 @@ class AStarNode: self.count = AStarNode._count AStarNode._count += 1 + + def __lt__(self, other: AStarNode) -> bool: # Tie-breaking: lower f first, then lower h, then order if abs(self.f_cost - other.f_cost) > 1e-9: @@ -83,7 +86,7 @@ class AStarRouter: """ Hybrid State-Lattice A* Router. """ - __slots__ = ('cost_evaluator', 'config', 'node_limit', 'total_nodes_expanded', '_collision_cache', '_self_dilation') + __slots__ = ('cost_evaluator', 'config', 'node_limit', 'total_nodes_expanded', '_collision_cache', '_move_cache', '_self_dilation') cost_evaluator: CostEvaluator """ The evaluator for path and proximity costs """ @@ -100,6 +103,9 @@ class AStarRouter: _collision_cache: dict[tuple[float, float, float, str, float, str], bool] """ Internal cache for move collision checks """ + _move_cache: dict[tuple[Any, ...], ComponentResult] + """ Internal cache for component generation """ + _self_dilation: float """ Cached dilation value for collision checks (clearance / 2.0) """ @@ -149,6 +155,7 @@ class AStarRouter: self.node_limit = self.config.node_limit self.total_nodes_expanded = 0 self._collision_cache = {} + self._move_cache = {} self._self_dilation = self.cost_evaluator.collision_engine.clearance / 2.0 def route( @@ -256,6 +263,11 @@ class AStarRouter: except ValueError: pass + # Move Cache + cp = current.port + base_ori = round(cp.orientation % 360, 2) + state_key = (round(cp.x, 3), round(cp.y, 3), base_ori) + # 2. Lattice Straights lengths = self.config.straight_lengths if dist < 5.0: @@ -263,39 +275,74 @@ class AStarRouter: lengths = sorted(set(lengths + fine_steps)) for length in lengths: - res = Straight.generate(current.port, length, net_width, dilation=self._self_dilation) + # Level 1: Absolute cache (exact location) + abs_key = (state_key, 'S', length, net_width) + if abs_key in self._move_cache: + res = self._move_cache[abs_key] + else: + # Level 2: Relative cache (orientation only) + rel_key = (base_ori, 'S', length, net_width, self._self_dilation) + if rel_key in self._move_cache: + res = self._move_cache[rel_key].translate(cp.x, cp.y) + else: + res_rel = Straight.generate(Port(0, 0, base_ori), length, net_width, dilation=self._self_dilation) + self._move_cache[rel_key] = res_rel + res = res_rel.translate(cp.x, cp.y) + self._move_cache[abs_key] = res self._add_node(current, res, target, net_width, net_id, open_set, closed_set, f'S{length}') # 3. Lattice Bends for radius in self.config.bend_radii: for direction in ['CW', 'CCW']: - res = Bend90.generate( - current.port, - radius, - net_width, - direction, - collision_type=self.config.bend_collision_type, - clip_margin=self.config.bend_clip_margin, - dilation=self._self_dilation - ) + abs_key = (state_key, 'B', radius, direction, net_width, self.config.bend_collision_type) + if abs_key in self._move_cache: + res = self._move_cache[abs_key] + else: + rel_key = (base_ori, 'B', radius, direction, net_width, self.config.bend_collision_type, self._self_dilation) + if rel_key in self._move_cache: + res = self._move_cache[rel_key].translate(cp.x, cp.y) + else: + res_rel = Bend90.generate( + Port(0, 0, base_ori), + radius, + net_width, + direction, + collision_type=self.config.bend_collision_type, + clip_margin=self.config.bend_clip_margin, + dilation=self._self_dilation + ) + self._move_cache[rel_key] = res_rel + res = res_rel.translate(cp.x, cp.y) + self._move_cache[abs_key] = res self._add_node(current, res, target, net_width, net_id, open_set, closed_set, f'B{radius}{direction}', move_radius=radius) # 4. Discrete SBends for offset in self.config.sbend_offsets: for radius in self.config.sbend_radii: - try: - res = SBend.generate( - current.port, - offset, - radius, - net_width, - collision_type=self.config.bend_collision_type, - clip_margin=self.config.bend_clip_margin, - dilation=self._self_dilation - ) - self._add_node(current, res, target, net_width, net_id, open_set, closed_set, f'SB{offset}R{radius}', move_radius=radius) - except ValueError: - pass + abs_key = (state_key, 'SB', offset, radius, net_width, self.config.bend_collision_type) + if abs_key in self._move_cache: + res = self._move_cache[abs_key] + else: + rel_key = (base_ori, 'SB', offset, radius, net_width, self.config.bend_collision_type, self._self_dilation) + if rel_key in self._move_cache: + res = self._move_cache[rel_key].translate(cp.x, cp.y) + else: + try: + res_rel = SBend.generate( + Port(0, 0, base_ori), + offset, + radius, + net_width, + collision_type=self.config.bend_collision_type, + clip_margin=self.config.bend_clip_margin, + dilation=self._self_dilation + ) + self._move_cache[rel_key] = res_rel + res = res_rel.translate(cp.x, cp.y) + except ValueError: + continue + self._move_cache[abs_key] = res + self._add_node(current, res, target, net_width, net_id, open_set, closed_set, f'SB{offset}R{radius}', move_radius=radius) def _add_node( self, @@ -340,35 +387,27 @@ class AStarRouter: return # 3. Check for Self-Intersection (Limited to last 100 segments for performance) - # Optimization: use pre-dilated geometries + # Optimization: use pre-dilated geometries and pre-calculated bounds if result.dilated_geometry: - for dilated_move in result.dilated_geometry: + for dm_idx, dilated_move in enumerate(result.dilated_geometry): + dm_bounds = result.dilated_bounds[dm_idx] curr_p: AStarNode | None = parent seg_idx = 0 while curr_p and curr_p.component_result and seg_idx < 100: if seg_idx > 0: res_p = curr_p.component_result if res_p.dilated_geometry: - for dilated_prev in res_p.dilated_geometry: - if (dilated_move.bounds[0] > dilated_prev.bounds[2] or - dilated_move.bounds[2] < dilated_prev.bounds[0] or - dilated_move.bounds[1] > dilated_prev.bounds[3] or - dilated_move.bounds[3] < dilated_prev.bounds[1]): - continue - - if dilated_move.intersects(dilated_prev): - overlap = dilated_move.intersection(dilated_prev) - if overlap.area > 1e-6: - return - else: - # Fallback if no pre-dilation (should not happen with new logic) - dilation = self._self_dilation - for prev_poly in res_p.geometry: - dilated_prev = prev_poly.buffer(dilation) - if dilated_move.intersects(dilated_prev): - overlap = dilated_move.intersection(dilated_prev) - if overlap.area > 1e-6: - return + for dp_idx, dilated_prev in enumerate(res_p.dilated_geometry): + dp_bounds = res_p.dilated_bounds[dp_idx] + # Quick bounds overlap check + if not (dm_bounds[0] > dp_bounds[2] or + dm_bounds[2] < dp_bounds[0] or + dm_bounds[1] > dp_bounds[3] or + dm_bounds[3] < dp_bounds[1]): + if dilated_move.intersects(dilated_prev): + overlap = dilated_move.intersection(dilated_prev) + if not overlap.is_empty and overlap.area > 1e-6: + return curr_p = curr_p.parent seg_idx += 1