diff --git a/examples/07_large_scale_routing.png b/examples/07_large_scale_routing.png index d1dd4b7..f023100 100644 Binary files a/examples/07_large_scale_routing.png and b/examples/07_large_scale_routing.png differ diff --git a/examples/07_large_scale_routing.py b/examples/07_large_scale_routing.py index 38dfbf7..57a11f6 100644 --- a/examples/07_large_scale_routing.py +++ b/examples/07_large_scale_routing.py @@ -28,8 +28,8 @@ def main() -> None: danger_map.precompute(obstacles) evaluator = CostEvaluator(engine, danger_map, greedy_h_weight=1.5) - - router = AStarRouter(evaluator, node_limit=5000, snap_size=10.0) + + router = AStarRouter(evaluator, node_limit=10000, snap_size=10.0) pf = PathFinder(router, evaluator, max_iterations=20, base_congestion_penalty=500.0) # 2. Define Netlist diff --git a/inire/geometry/collision.py b/inire/geometry/collision.py index a5fbccc..73ba251 100644 --- a/inire/geometry/collision.py +++ b/inire/geometry/collision.py @@ -245,3 +245,67 @@ class CollisionEngine: if other_net_id != net_id and dynamic_prepared[obj_id].intersects(test_poly): count += 1 return count + + def ray_cast(self, origin: Port, angle_deg: float, max_dist: float = 2000.0) -> float: + """ + Cast a ray and find the distance to the nearest static obstacle. + + Args: + origin: Starting port (x, y). + angle_deg: Ray direction in degrees. + max_dist: Maximum lookahead distance. + + Returns: + Distance to first collision, or max_dist if clear. + """ + import numpy + from shapely.geometry import LineString + + rad = numpy.radians(angle_deg) + dx = max_dist * numpy.cos(rad) + dy = max_dist * numpy.sin(rad) + + # Ray geometry + ray_line = LineString([(origin.x, origin.y), (origin.x + dx, origin.y + dy)]) + + # 1. Query R-Tree + candidates = self.static_index.intersection(ray_line.bounds) + + min_dist = max_dist + + # 2. Check Intersections + # Note: We intersect with DILATED obstacles to account for clearance + for obj_id in candidates: + obstacle = self.static_dilated[obj_id] + # Fast check with prepared geom? intersects() is fast, intersection() gives point + if self.static_prepared[obj_id].intersects(ray_line): + # Calculate exact intersection distance + intersection = ray_line.intersection(obstacle) + if intersection.is_empty: + continue + + # Intersection could be MultiLineString or LineString or Point + # We want the point closest to origin + + # Helper to get dist + def get_dist(geom): + if hasattr(geom, 'geoms'): # Multi-part + return min(get_dist(g) for g in geom.geoms) + # For line string, the intersection is the segment INSIDE the obstacle. + # The distance is the distance to the start of that segment. + # Or if it's a touch (Point), distance to point. + coords = geom.coords + # Distance to the first point of the intersection geometry + # (Assuming simple overlap, first point is entry) + p1 = coords[0] + return numpy.sqrt((p1[0] - origin.x)**2 + (p1[1] - origin.y)**2) + + try: + d = get_dist(intersection) + # Subtract safety margin to be safe? No, let higher level handle margins. + if d < min_dist: + min_dist = d + except Exception: + pass # Robustness + + return min_dist diff --git a/inire/geometry/components.py b/inire/geometry/components.py index 3d068d4..4820fbb 100644 --- a/inire/geometry/components.py +++ b/inire/geometry/components.py @@ -445,8 +445,25 @@ class SBend: # tan(theta / 2) = local_dy / local_dx theta = 2 * numpy.arctan2(abs(local_dy), local_dx) + + if abs(theta) < 1e-9: + # Practically straight, but offset implies we need a bend. + # If offset is also tiny, return a straight? + if abs(offset) < 1e-6: + # Degenerate case: effectively straight + return Straight.generate(start_port, numpy.sqrt(local_dx**2 + local_dy**2), width, snap_to_grid=False, dilation=dilation) + raise ValueError("SBend calculation failed: theta close to zero") + # Avoid division by zero if theta is 0 (though unlikely due to offset check) - actual_radius = abs(local_dy) / (2 * (1 - numpy.cos(theta))) if theta > 1e-9 else radius + denom = (2 * (1 - numpy.cos(theta))) + if abs(denom) < 1e-9: + raise ValueError("SBend calculation failed: radius denominator zero") + + actual_radius = abs(local_dy) / denom + + # Limit radius to prevent giant arcs + if actual_radius > 100000.0: + raise ValueError("SBend calculation failed: radius too large") direction = 1 if local_dy > 0 else -1 c1_angle = rad_start + direction * numpy.pi / 2 diff --git a/inire/router/astar.py b/inire/router/astar.py index 8c390ef..e2c4ae8 100644 --- a/inire/router/astar.py +++ b/inire/router/astar.py @@ -154,6 +154,7 @@ class AStarRouter: while open_set: if nodes_expanded >= node_limit: + # logger.warning(f' AStar failed: node limit {node_limit} reached.') return reconstruct_path(best_node) if return_partial else None current = heapq.heappop(open_set) @@ -222,6 +223,7 @@ class AStarRouter: proj = dx_t * cos_r + dy_t * sin_r perp = -dx_t * sin_r + dy_t * cos_r if proj > 0 and 0.5 <= abs(perp) < snap_dist: + # Try a few candidate radii for radius in self.config.sbend_radii: try: res = SBend.generate( @@ -238,15 +240,29 @@ class AStarRouter: except ValueError: pass - # 2. Lattice Straights + # 2. Parametric Straights cp = current.port base_ori = round(cp.orientation, 2) state_key = (int(cp.x / snap), int(cp.y / snap), int(base_ori / 1.0)) - # Backwards pruning - allow_backwards = (dist_sq < 200*200) + # Ray cast to find max length + max_reach = self.cost_evaluator.collision_engine.ray_cast(cp, base_ori, self.config.max_straight_length) + # Subtract buffer for bend radius + margin + effective_max = max(self.config.min_straight_length, max_reach - 50.0) # Assume 50um bend radius + + # Generate samples + lengths = [effective_max] + if self.config.num_straight_samples > 1 and effective_max > self.config.min_straight_length * 2: + # Add intermediate step + lengths.append(effective_max / 2.0) + + # Add min length for maneuvering + lengths.append(self.config.min_straight_length) + + # Deduplicate and sort + lengths = sorted(list(set(lengths)), reverse=True) - for length in self.config.straight_lengths: + for length in lengths: # Level 1: Absolute cache (exact location) abs_key = (state_key, 'S', length, net_width) if abs_key in self._move_cache: @@ -256,7 +272,7 @@ class AStarRouter: # Level 2: Relative cache (orientation only) rel_key = (base_ori, 'S', length, net_width, self._self_dilation) - # OPTIMIZATION: Check hard collision set + # OPTIMIZATION: Check hard collision set BEFORE anything else move_type = f'S{length}' cache_key = (state_key[0], state_key[1], base_ori, move_type, net_width) if cache_key in self._hard_collision_set: @@ -279,7 +295,10 @@ class AStarRouter: self._add_node(current, res, target, net_width, net_id, open_set, closed_set, move_type, snap=snap) # 3. Lattice Bends + # Backwards pruning angle_to_target = numpy.degrees(numpy.arctan2(dy_t, dx_t)) + allow_backwards = (dist_sq < 200*200) + for radius in self.config.bend_radii: for direction in ['CW', 'CCW']: if not allow_backwards: @@ -325,9 +344,27 @@ class AStarRouter: self._move_cache[abs_key] = res self._add_node(current, res, target, net_width, net_id, open_set, closed_set, move_type, move_radius=radius, snap=snap) - # 4. Discrete SBends - for offset in self.config.sbend_offsets: + # 4. Parametric SBends + # Try both positive and negative offsets + offsets = self.config.sbend_offsets + + # Dynamically add target alignment offset if within range + # Project target onto current frame + rad = numpy.radians(cp.orientation) + dx_local = (target.x - cp.x) * numpy.cos(rad) + (target.y - cp.y) * numpy.sin(rad) + dy_local = -(target.x - cp.x) * numpy.sin(rad) + (target.y - cp.y) * numpy.cos(rad) + + if 0 < dx_local < snap_dist: + # If target is ahead, try to align Y + offsets = list(offsets) + [dy_local] + offsets = sorted(list(set(offsets))) # Uniquify + + for offset in offsets: for radius in self.config.sbend_radii: + # Validity check: offset < 2*R + if abs(offset) >= 2 * radius: + continue + move_type = f'SB{offset}R{radius}' abs_key = (state_key, 'SB', offset, radius, net_width, self.config.bend_collision_type) if abs_key in self._move_cache: diff --git a/inire/router/config.py b/inire/router/config.py index b15ab52..d6217b9 100644 --- a/inire/router/config.py +++ b/inire/router/config.py @@ -11,9 +11,18 @@ class RouterConfig: node_limit: int = 1000000 snap_size: float = 5.0 - straight_lengths: list[float] = field(default_factory=lambda: [10.0, 50.0, 100.0, 500.0, 1000.0]) - bend_radii: list[float] = field(default_factory=lambda: [50.0, 100.0]) + # Sparse Sampling Configuration + max_straight_length: float = 2000.0 + num_straight_samples: int = 3 + min_straight_length: float = 10.0 + + # Offsets for SBends (still list-based for now, or could range) sbend_offsets: list[float] = field(default_factory=lambda: [-100.0, -50.0, -10.0, 10.0, 50.0, 100.0]) + + # Deprecated but kept for compatibility during refactor + straight_lengths: list[float] = field(default_factory=list) + + bend_radii: list[float] = field(default_factory=lambda: [50.0, 100.0]) sbend_radii: list[float] = field(default_factory=lambda: [50.0, 100.0, 500.0]) snap_to_target_dist: float = 1000.0 bend_penalty: float = 250.0