speedup
This commit is contained in:
parent
8424171946
commit
8bf0ff279f
5 changed files with 191 additions and 117 deletions
Binary file not shown.
|
Before Width: | Height: | Size: 66 KiB After Width: | Height: | Size: 67 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 39 KiB After Width: | Height: | Size: 42 KiB |
|
|
@ -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)
|
||||
|
||||
intersection = test_poly.intersection(self.static_geometries[obj_id])
|
||||
if intersection.is_empty:
|
||||
continue
|
||||
|
||||
ix_minx, ix_miny, ix_maxx, ix_maxy = intersection.bounds
|
||||
# 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.
|
||||
|
||||
is_safe = False
|
||||
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) < 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):
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
# 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))
|
||||
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:
|
||||
continue
|
||||
return res_poly
|
||||
new_verts.append(p)
|
||||
else:
|
||||
new_verts.append(p)
|
||||
|
||||
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,
|
||||
|
|
|
|||
|
|
@ -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,14 +275,35 @@ 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,
|
||||
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,
|
||||
|
|
@ -278,14 +311,25 @@ class AStarRouter:
|
|||
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:
|
||||
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 = SBend.generate(
|
||||
current.port,
|
||||
res_rel = SBend.generate(
|
||||
Port(0, 0, base_ori),
|
||||
offset,
|
||||
radius,
|
||||
net_width,
|
||||
|
|
@ -293,9 +337,12 @@ class AStarRouter:
|
|||
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)
|
||||
self._move_cache[rel_key] = res_rel
|
||||
res = res_rel.translate(cp.x, cp.y)
|
||||
except ValueError:
|
||||
pass
|
||||
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,34 +387,26 @@ 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
|
||||
|
||||
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 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:
|
||||
if not overlap.is_empty and overlap.area > 1e-6:
|
||||
return
|
||||
curr_p = curr_p.parent
|
||||
seg_idx += 1
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue