ex07 works

.gitignore

@@ -10,3 +10,5 @@ wheels/
 .venv
 
 .hypothesis
+*.png
+

examples/07_large_scale_routing.py

@@ -6,7 +6,7 @@ from inire.router.astar import AStarRouter
 from inire.router.cost import CostEvaluator
 from inire.router.danger_map import DangerMap
 from inire.router.pathfinder import PathFinder
-from inire.utils.visualization import plot_routing_results, plot_danger_map, plot_expanded_nodes
+from inire.utils.visualization import plot_routing_results, plot_danger_map, plot_expanded_nodes, plot_expansion_density
 from shapely.geometry import box
 
 def main() -> None:
@@ -27,10 +27,10 @@ def main() -> None:
     danger_map = DangerMap(bounds=bounds)
     danger_map.precompute(obstacles)
 
-    evaluator = CostEvaluator(engine, danger_map, greedy_h_weight=1.5, unit_length_cost=0.5, bend_penalty=100.0, sbend_penalty=200.0, congestion_penalty=100.0)
+    evaluator = CostEvaluator(engine, danger_map, greedy_h_weight=2.0, unit_length_cost=0.1, bend_penalty=100.0, sbend_penalty=200.0, congestion_penalty=20.0)
 
-    router = AStarRouter(evaluator, node_limit=1000000, snap_size=5.0)
-    pf = PathFinder(router, evaluator, max_iterations=10, base_congestion_penalty=100.0)
+    router = AStarRouter(evaluator, node_limit=2000000, snap_size=5.0, bend_radii=[50.0], sbend_radii=[50.0], use_analytical_sbends=False)
+    pf = PathFinder(router, evaluator, max_iterations=50, base_congestion_penalty=20.0, congestion_multiplier=1.2)
 
     # 2. Define Netlist
     netlist = {}
@@ -49,36 +49,115 @@ def main() -> None:
 
     net_widths = {nid: 2.0 for nid in netlist}
 
-    def iteration_callback(idx, current_results):
-        print(f"  Iteration {idx} finished. Successes: {sum(1 for r in current_results.values() if r.is_valid)}/{len(netlist)}")
-        print(pf.router.get_metrics_summary())
-        pf.router.reset_metrics()
-        # fig, ax = plot_routing_results(current_results, obstacles, bounds, netlist=netlist)
-        # plot_danger_map(danger_map, ax=ax)
-        # fig.savefig(f"examples/07_iteration_{idx:02d}.png")
-        # import matplotlib.pyplot as plt
-        # plt.close(fig)
-
     # 3. Route
     print(f"Routing {len(netlist)} nets through 200um bottleneck...")
+    
+    iteration_stats = []
+
+    def iteration_callback(idx, current_results):
+        successes = sum(1 for r in current_results.values() if r.is_valid)
+        total_collisions = sum(r.collisions for r in current_results.values())
+        total_nodes = pf.router.metrics['nodes_expanded']
+        
+        # Identify Hotspots
+        hotspots = {}
+        overlap_matrix = {} # (net_a, net_b) -> count
+        
+        for nid, res in current_results.items():
+            if res.path:
+                for comp in res.path:
+                    for poly in comp.geometry:
+                        # Check what it overlaps with
+                        overlaps = engine.dynamic_index.intersection(poly.bounds)
+                        for other_obj_id in overlaps:
+                            other_nid, other_poly = engine.dynamic_geometries[other_obj_id]
+                            if other_nid != nid:
+                                if poly.intersects(other_poly):
+                                    # Record hotspot
+                                    cx, cy = poly.centroid.x, poly.centroid.y
+                                    grid_key = (int(cx/20)*20, int(cy/20)*20)
+                                    hotspots[grid_key] = hotspots.get(grid_key, 0) + 1
+                                    
+                                    # Record pair
+                                    pair = tuple(sorted((nid, other_nid)))
+                                    overlap_matrix[pair] = overlap_matrix.get(pair, 0) + 1
+
+        print(f"  Iteration {idx} finished. Successes: {successes}/{len(netlist)}, Collisions: {total_collisions}")
+        if overlap_matrix:
+            top_pairs = sorted(overlap_matrix.items(), key=lambda x: x[1], reverse=True)[:3]
+            print(f"  Top Conflicts: {top_pairs}")
+        if hotspots:
+            top_hotspots = sorted(hotspots.items(), key=lambda x: x[1], reverse=True)[:3]
+            print(f"  Top Hotspots: {top_hotspots}")
+        
+        # Adaptive Greediness: Decay from 2.0 to 1.1 over 25 iterations
+        new_greedy = max(1.1, 2.0 - ((idx + 1) / 25.0))
+        evaluator.greedy_h_weight = new_greedy
+        print(f"  Adaptive Greedy Weight for Next Iteration: {new_greedy:.3f}")
+
+        iteration_stats.append({
+            'Iteration': idx,
+            'Success': successes,
+            'Congestion': total_collisions,
+            'Nodes': total_nodes
+        })
+        
+        # Save plots only for certain iterations to save time
+        if idx % 20 == 0 or idx == pf.max_iterations - 1:
+            # Save a plot of this iteration's result
+            fig, ax = plot_routing_results(current_results, obstacles, bounds, netlist=netlist)
+            plot_danger_map(danger_map, ax=ax)
+            
+            # Overlay failures: show where they stopped
+            for nid, res in current_results.items():
+                if not res.is_valid and res.path:
+                    last_p = res.path[-1].end_port
+                    target_p = netlist[nid][1]
+                    dist = abs(last_p.x - target_p.x) + abs(last_p.y - target_p.y)
+                    ax.scatter(last_p.x, last_p.y, color='red', marker='x', s=100)
+                    ax.text(last_p.x, last_p.y, f" {nid} (rem: {dist:.0f}um)", color='red', fontsize=8)
+            
+            fig.savefig(f"examples/07_iteration_{idx:02d}.png")
+            import matplotlib.pyplot as plt
+            plt.close(fig)
+
+            # Plot Expansion Density if data is available
+            if pf.accumulated_expanded_nodes:
+                 fig_d, ax_d = plot_expansion_density(pf.accumulated_expanded_nodes, bounds)
+                 fig_d.savefig(f"examples/07_iteration_{idx:02d}_density.png")
+                 plt.close(fig_d)
+        
+        pf.router.reset_metrics()
+
     import cProfile, pstats
     profiler = cProfile.Profile()
     profiler.enable()
     t0 = time.perf_counter()
-    results = pf.route_all(netlist, net_widths, store_expanded=True, iteration_callback=iteration_callback)
+    results = pf.route_all(netlist, net_widths, store_expanded=True, iteration_callback=iteration_callback, shuffle_nets=True, seed=42)
     t1 = time.perf_counter()
     profiler.disable()
+    
+    # ... (rest of the code)
     stats = pstats.Stats(profiler).sort_stats('tottime')
     stats.print_stats(20)
     print(f"Routing took {t1-t0:.4f}s")
 
     # 4. Check Results
+    print("\n--- Iteration Summary ---")
+    print(f"{'Iter':<5} | {'Success':<8} | {'Congest':<8} | {'Nodes':<10}")
+    print("-" * 40)
+    for s in iteration_stats:
+        print(f"{s['Iteration']:<5} | {s['Success']:<8} | {s['Congestion']:<8} | {s['Nodes']:<10}")
+    
     success_count = sum(1 for res in results.values() if res.is_valid)
-    print(f"Routed {success_count}/{len(netlist)} nets successfully.")
+    print(f"\nFinal: Routed {success_count}/{len(netlist)} nets successfully.")
     
     for nid, res in results.items():
+        target_p = netlist[nid][1]
         if not res.is_valid:
-            print(f"  FAILED: {nid}")
+            last_p = res.path[-1].end_port if res.path else netlist[nid][0]
+            dist = abs(last_p.x - target_p.x) + abs(last_p.y - target_p.y)
+            print(f"  FAILED: {nid} (Stopped {dist:.1f}um from target)")
         else:
             types = [move.move_type for move in res.path]
             from collections import Counter

inire/geometry/components.py

@@ -242,88 +242,65 @@ def _get_arc_polygons(
 
 
 def _clip_bbox(
-        bbox: Polygon,
         cx: float,
         cy: float,
         radius: float,
         width: float,
-        clip_margin: float,
-        arc_poly: Polygon,
-        t_start: float | None = None,
-        t_end: float | None = None,
+        t_start: float,
+        t_end: float,
         ) -> Polygon:
     """
-    Clips corners of a bounding box for better collision modeling.
+    Generates a rotationally invariant bounding polygon for an arc.
     """
-    r_out_cut = radius + width / 2.0 + clip_margin
-    r_in_cut = radius - width / 2.0 - clip_margin
-
-    # Angular range of the arc
-    if t_start is not None and t_end is not None:
-        ts, te = t_start, t_end
-        if ts > te:
-            ts, te = te, ts
-        # Sweep could cross 2pi boundary
-        sweep = (te - ts) % (2 * numpy.pi)
-        ts_norm = ts % (2 * numpy.pi)
+    sweep = abs(t_end - t_start)
+    if sweep > 2 * numpy.pi:
+        sweep = sweep % (2 * numpy.pi)
+    
+    mid_angle = (t_start + t_end) / 2.0
+    # Handle wrap-around for mid_angle
+    if abs(t_end - t_start) > numpy.pi:
+        mid_angle += numpy.pi
+        
+    r_out = radius + width / 2.0
+    r_in = max(0.0, radius - width / 2.0)
+    
+    half_sweep = sweep / 2.0
+    
+    # Define vertices in local space (center at 0,0, symmetry axis along +X)
+    # 1. Start Inner
+    # 2. Start Outer
+    # 3. Peak Outer (intersection of tangents at start/end)
+    # 4. End Outer
+    # 5. End Inner
+    # 6. Peak Inner (ensures convexity and inner clipping)
+    
+    # Outer tangent intersection point
+    # Tangent at -hs: x*cos(hs) - y*sin(hs) = r_out
+    # Tangent at +hs: x*cos(hs) + y*sin(hs) = r_out
+    # Intersection: y=0, x = r_out / cos(hs)
+    cos_hs = numpy.cos(half_sweep)
+    if cos_hs < 1e-3: # Sweep near 180 deg
+        peak_out_x = r_out * 2.0 # Fallback
     else:
-        # Fallback: assume 90 deg based on centroid quadrant
-        ac = arc_poly.centroid
-        mid_angle = numpy.arctan2(ac.y - cy, ac.x - cx)
-        ts_norm = (mid_angle - numpy.pi/4) % (2 * numpy.pi)
-        sweep = numpy.pi/2
+        peak_out_x = r_out / cos_hs
 
-    minx, miny, maxx, maxy = bbox.bounds
-    verts = [
-        numpy.array([minx, miny]),
-        numpy.array([maxx, miny]),
-        numpy.array([maxx, maxy]),
-        numpy.array([minx, maxy])
+    local_verts = [
+        [r_in * numpy.cos(-half_sweep), r_in * numpy.sin(-half_sweep)],
+        [r_out * numpy.cos(-half_sweep), r_out * numpy.sin(-half_sweep)],
+        [peak_out_x, 0.0],
+        [r_out * numpy.cos(half_sweep), r_out * numpy.sin(half_sweep)],
+        [r_in * numpy.cos(half_sweep), r_in * numpy.sin(half_sweep)],
+        [r_in, 0.0]
     ]
     
-    new_verts = []
-    for p in verts:
-        dx, dy = p[0] - cx, p[1] - cy
-        dist = numpy.sqrt(dx**2 + dy**2)
-        angle = numpy.arctan2(dy, dx)
-        angle_rel = (angle - ts_norm) % (2 * numpy.pi)
-        is_in_sweep = angle_rel <= sweep + 1e-6
-
-        d_line = -1.0
-        if is_in_sweep:
-            # We can clip if outside R_out or inside R_in
-            if dist > radius + width/2.0 - 1e-6:
-                d_line = r_out_cut * numpy.sqrt(2)
-            elif r_in_cut > 1e-3 and dist < radius - width/2.0 + 1e-6:
-                d_line = r_in_cut
-        else:
-            # Corner is outside angular sweep.
-            if dist > radius + width/2.0 - 1e-6:
-                d_line = r_out_cut * numpy.sqrt(2)
-            elif r_in_cut > 1e-3 and dist < radius - width/2.0 + 1e-6:
-                d_line = r_in_cut
-        
-        if d_line > 0:
-            sx = 1.0 if dx > 0 else -1.0
-            sy = 1.0 if dy > 0 else -1.0
-            try:
-                # Intersection of line sx*(x-cx) + sy*(y-cy) = d_line with box edges
-                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]])
-                
-                # Check if intersection points are on the box boundary
-                if (minx - 1e-6 <= p_edge_y[0] <= maxx + 1e-6 and 
-                    miny - 1e-6 <= p_edge_x[1] <= maxy + 1e-6):
-                     new_verts.append(p_edge_x)
-                     new_verts.append(p_edge_y)
-                else:
-                     new_verts.append(p)
-            except ZeroDivisionError:
-                new_verts.append(p)
-        else:
-            new_verts.append(p)
-
-    return Polygon(new_verts).convex_hull
+    # Rotate and translate to world space
+    cos_m = numpy.cos(mid_angle)
+    sin_m = numpy.sin(mid_angle)
+    rot = numpy.array([[cos_m, -sin_m], [sin_m, cos_m]])
+    
+    world_verts = (numpy.array(local_verts) @ rot.T) + [cx, cy]
+    
+    return Polygon(world_verts)
 
 
 def _apply_collision_model(
@@ -346,16 +323,16 @@ def _apply_collision_model(
     if collision_type == "arc":
         return [arc_poly]
     
-    # Bounding box of the high-fidelity arc
+    if collision_type == "clipped_bbox" and t_start is not None and t_end is not None:
+        return [_clip_bbox(cx, cy, radius, width, t_start, t_end)]
+
+    # Bounding box of the high-fidelity arc (fallback for bbox or missing angles)
     minx, miny, maxx, maxy = arc_poly.bounds
     bbox_poly = box(minx, miny, maxx, maxy)
 
     if collision_type == "bbox":
         return [bbox_poly]
     
-    if collision_type == "clipped_bbox":
-        return [_clip_bbox(bbox_poly, cx, cy, radius, width, clip_margin, arc_poly, t_start, t_end)]
-
     return [arc_poly]
 
 
@@ -405,8 +382,23 @@ class Bend90:
         else:
             ex, ey = ex_raw, ey_raw
         
-        # Slightly adjust radius to hit snapped point exactly
+        # Slightly adjust radius and t_end to hit snapped point exactly
         actual_radius = numpy.sqrt((ex - cx)**2 + (ey - cy)**2)
+        
+        t_end_snapped = numpy.arctan2(ey - cy, ex - cx)
+        # Ensure directionality and approx 90 degree sweep
+        if direction == "CCW":
+            while t_end_snapped <= t_start:
+                t_end_snapped += 2 * numpy.pi
+            while t_end_snapped > t_start + numpy.pi:
+                t_end_snapped -= 2 * numpy.pi
+        else:
+            while t_end_snapped >= t_start:
+                t_end_snapped -= 2 * numpy.pi
+            while t_end_snapped < t_start - numpy.pi:
+                t_end_snapped += 2 * numpy.pi
+        t_end = t_end_snapped
+
         end_port = Port(ex, ey, new_ori)
 
         arc_polys = _get_arc_polygons(cx, cy, actual_radius, width, t_start, t_end, sagitta)

inire/router/astar.py

@@ -204,13 +204,20 @@ class AStarRouter:
              if max_reach >= proj_t - 0.01:
                   self._process_move(current, target, net_width, net_id, open_set, closed_set, snap, f'S{proj_t}', 'S', (proj_t,), skip_congestion, skip_static=True, snap_to_grid=False)
 
-        # B. SBend Jump (if oriented correctly but offset)
-        if proj_t > 0 and abs(cp.orientation - target.orientation) < 0.1 and abs(perp_t) > 1e-3:
-             if proj_t < 200.0: # Only lookahead when close
-                  for radius in self.config.sbend_radii:
-                       if abs(perp_t) < 2 * radius:
-                            # Try to generate it
-                            self._process_move(current, target, net_width, net_id, open_set, closed_set, snap, f'SB{perp_t}R{radius}', 'SB', (perp_t, radius), skip_congestion, snap_to_grid=False)
+        # B. SBend Jump (Direct to Target)
+        if self.config.use_analytical_sbends and proj_t > 0 and abs(cp.orientation - target.orientation) < 0.1 and abs(perp_t) > 1e-3:
+             # Calculate required radius to hit target exactly: R = (dx^2 + dy^2) / (4*|dy|)
+             req_radius = (proj_t**2 + perp_t**2) / (4.0 * abs(perp_t))
+             
+             min_radius = min(self.config.sbend_radii) if self.config.sbend_radii else 50.0
+             
+             if req_radius >= min_radius:
+                  # We can hit it exactly!
+                  self._process_move(current, target, net_width, net_id, open_set, closed_set, snap, f'SB_Direct_R{req_radius:.1f}', 'SB', (perp_t, req_radius), skip_congestion, snap_to_grid=False)
+             else:
+                  # Required radius is too small. We must use a larger radius and some straight segments.
+                  # A* will handle this through Priority 3 SBends + Priority 2 Straights.
+                  pass
 
                   # In super sparse mode, we can return here, but A* needs other options for optimality.
                   # return 
@@ -406,6 +413,10 @@ class AStarRouter:
         if 'SB' in move_type: penalty = self.config.sbend_penalty
         elif 'B' in move_type: penalty = self.config.bend_penalty
 
+        # Scale penalty by radius (larger radius = smoother = lower penalty)
+        if move_radius is not None and move_radius > 1e-6:
+            penalty *= (10.0 / move_radius)**0.5
+
         move_cost = self.cost_evaluator.evaluate_move(
             result.geometry, result.end_port, net_width, net_id,
             start_port=parent_p, length=result.length,
@@ -418,9 +429,6 @@ class AStarRouter:
             self.metrics['pruned_cost'] += 1
             return
 
-        if 'B' in move_type and move_radius is not None and move_radius > 1e-6:
-            move_cost *= (10.0 / move_radius)**0.5
-
         g_cost = parent.g_cost + move_cost
         if state in closed_set and closed_set[state] <= g_cost + 1e-6:
             self.metrics['pruned_closed_set'] += 1

inire/router/config.py

@@ -25,6 +25,7 @@ class RouterConfig:
     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
+    use_analytical_sbends: bool = True
     bend_penalty: float = 250.0
     sbend_penalty: float = 500.0
     bend_collision_type: Literal["arc", "bbox", "clipped_bbox"] | Any = "arc"

inire/router/pathfinder.py

@@ -2,6 +2,7 @@ from __future__ import annotations
 
 import logging
 import time
+import random
 from dataclasses import dataclass
 from typing import TYPE_CHECKING, Callable
 
@@ -39,7 +40,7 @@ class PathFinder:
     """
     Multi-net router using Negotiated Congestion.
     """
-    __slots__ = ('router', 'cost_evaluator', 'max_iterations', 'base_congestion_penalty', 'use_tiered_strategy')
+    __slots__ = ('router', 'cost_evaluator', 'max_iterations', 'base_congestion_penalty', 'use_tiered_strategy', 'congestion_multiplier', 'accumulated_expanded_nodes')
 
     router: AStarRouter
     """ The A* search engine """
@@ -53,6 +54,9 @@ class PathFinder:
     base_congestion_penalty: float
     """ Starting penalty for overlaps """
 
+    congestion_multiplier: float
+    """ Multiplier for congestion penalty per iteration """
+
     use_tiered_strategy: bool
     """ If True, use simpler collision models in early iterations for speed """
 
@@ -62,6 +66,7 @@ class PathFinder:
             cost_evaluator: CostEvaluator,
             max_iterations: int = 10,
             base_congestion_penalty: float = 100.0,
+            congestion_multiplier: float = 1.5,
             use_tiered_strategy: bool = True,
             ) -> None:
         """
@@ -72,13 +77,16 @@ class PathFinder:
             cost_evaluator: The evaluator for path costs.
             max_iterations: Maximum number of rip-up and reroute iterations.
             base_congestion_penalty: Starting penalty for overlaps.
+            congestion_multiplier: Multiplier for congestion penalty per iteration.
             use_tiered_strategy: Whether to use simplified collision models in early iterations.
         """
         self.router = router
         self.cost_evaluator = cost_evaluator
         self.max_iterations = max_iterations
         self.base_congestion_penalty = base_congestion_penalty
+        self.congestion_multiplier = congestion_multiplier
         self.use_tiered_strategy = use_tiered_strategy
+        self.accumulated_expanded_nodes: list[tuple[float, float, float]] = []
 
     def route_all(
             self,
@@ -86,6 +94,8 @@ class PathFinder:
             net_widths: dict[str, float],
             store_expanded: bool = False,
             iteration_callback: Callable[[int, dict[str, RoutingResult]], None] | None = None,
+            shuffle_nets: bool = False,
+            seed: int | None = None,
             ) -> dict[str, RoutingResult]:
         """
         Route all nets in the netlist using Negotiated Congestion.
@@ -93,25 +103,40 @@ class PathFinder:
         Args:
             netlist: Mapping of net_id to (start_port, target_port).
             net_widths: Mapping of net_id to waveguide width.
-            store_expanded: Whether to store expanded nodes for the last iteration.
+            store_expanded: Whether to store expanded nodes for ALL iterations and nets.
             iteration_callback: Optional callback(iteration_idx, current_results).
+            shuffle_nets: Whether to randomize the order of nets each iteration.
+            seed: Optional seed for randomization (enables reproducibility).
 
         Returns:
             Mapping of net_id to RoutingResult.
         """
         results: dict[str, RoutingResult] = {}
         self.cost_evaluator.congestion_penalty = self.base_congestion_penalty
+        self.accumulated_expanded_nodes = []
 
         start_time = time.monotonic()
         num_nets = len(netlist)
         session_timeout = max(60.0, 10.0 * num_nets * self.max_iterations)
 
+        all_net_ids = list(netlist.keys())
+
         for iteration in range(self.max_iterations):
             any_congestion = False
+            # Clear accumulation for this iteration so callback gets fresh data
+            self.accumulated_expanded_nodes = []
+            
             logger.info(f'PathFinder Iteration {iteration}...')
 
+            # 0. Shuffle nets if requested
+            if shuffle_nets:
+                # Use a new seed based on iteration for deterministic different orders
+                it_seed = (seed + iteration) if seed is not None else None
+                random.Random(it_seed).shuffle(all_net_ids)
+
             # Sequence through nets
-            for net_id, (start, target) in netlist.items():
+            for net_id in all_net_ids:
+                start, target = netlist[net_id]
                 # Timeout check
                 elapsed = time.monotonic() - start_time
                 if elapsed > session_timeout:
@@ -139,15 +164,16 @@ class PathFinder:
                      current_node_limit = base_node_limit * (iteration + 1)
                 
                 net_start = time.monotonic()
-                # Store expanded only in the last potential iteration or if specifically requested
-                do_store = store_expanded and (iteration == self.max_iterations - 1)
                 
                 # Temporarily override node_limit
                 original_limit = self.router.node_limit
                 self.router.node_limit = current_node_limit
                 
-                path = self.router.route(start, target, width, net_id=net_id, bend_collision_type=coll_model, return_partial=True, store_expanded=do_store, skip_congestion=skip_cong)
+                path = self.router.route(start, target, width, net_id=net_id, bend_collision_type=coll_model, return_partial=True, store_expanded=store_expanded, skip_congestion=skip_cong)
                 
+                if store_expanded and self.router.last_expanded_nodes:
+                    self.accumulated_expanded_nodes.extend(self.router.last_expanded_nodes)
+
                 # Restore
                 self.router.node_limit = original_limit
                 
@@ -205,7 +231,7 @@ class PathFinder:
                     break
 
             # 4. Inflate congestion penalty
-            self.cost_evaluator.congestion_penalty *= 1.5
+            self.cost_evaluator.congestion_penalty *= self.congestion_multiplier
 
         return self._finalize_results(results, netlist)
 

inire/utils/visualization.py

@@ -142,6 +142,7 @@ def plot_danger_map(
     plt.colorbar(im, ax=ax, label='Danger Cost')
     ax.set_title("Danger Map (Proximity Costs)")
     return fig, ax
+
 def plot_expanded_nodes(
     nodes: list[tuple[float, float, float]],
     ax: Axes | None = None,
@@ -162,3 +163,56 @@ def plot_expanded_nodes(
     x, y, _ = zip(*nodes)
     ax.scatter(x, y, s=1, c=color, alpha=alpha, zorder=0)
     return fig, ax
+
+def plot_expansion_density(
+    nodes: list[tuple[float, float, float]],
+    bounds: tuple[float, float, float, float],
+    ax: Axes | None = None,
+    bins: int | tuple[int, int] = 50,
+) -> tuple[Figure, Axes]:
+    """
+    Plot a density heatmap (2D histogram) of expanded nodes.
+
+    Args:
+        nodes: List of (x, y, orientation) tuples.
+        bounds: (minx, miny, maxx, maxy) for the plot range.
+        ax: Optional existing axes to plot on.
+        bins: Number of bins for the histogram (int or (nx, ny)).
+
+    Returns:
+        Figure and Axes objects.
+    """
+    if ax is None:
+        fig, ax = plt.subplots(figsize=(12, 12))
+    else:
+        fig = ax.get_figure()
+
+    if not nodes:
+        ax.text(0.5, 0.5, "No Expansion Data", ha='center', va='center', transform=ax.transAxes)
+        return fig, ax
+
+    x, y, _ = zip(*nodes)
+    
+    # Create 2D histogram
+    h, xedges, yedges = numpy.histogram2d(
+        x, y, 
+        bins=bins, 
+        range=[[bounds[0], bounds[2]], [bounds[1], bounds[3]]]
+    )
+    
+    # Plot as image
+    im = ax.imshow(
+        h.T, 
+        origin='lower', 
+        extent=[bounds[0], bounds[2], bounds[1], bounds[3]],
+        cmap='plasma',
+        interpolation='nearest',
+        alpha=0.7
+    )
+    
+    plt.colorbar(im, ax=ax, label='Expansion Count')
+    ax.set_title("Search Expansion Density")
+    ax.set_xlim(bounds[0], bounds[2])
+    ax.set_ylim(bounds[1], bounds[3])
+    
+    return fig, ax