trim down

inire/geometry/collision.py

@@ -23,12 +23,13 @@ class CollisionEngine:
         'clearance', 'max_net_width', 'safety_zone_radius',
         'static_index', 'static_geometries', 'static_dilated', 'static_prepared', 
         'static_is_rect', 'static_tree', 'static_obj_ids', 'static_safe_cache', 
-        'static_grid', 'grid_cell_size', '_static_id_counter',
+        'static_grid', 'grid_cell_size', '_static_id_counter', '_net_specific_trees',
+        '_net_specific_is_rect', '_net_specific_bounds',
         'dynamic_index', 'dynamic_geometries', 'dynamic_dilated', 'dynamic_prepared', 
         'dynamic_tree', 'dynamic_obj_ids', 'dynamic_grid', '_dynamic_id_counter',
         'metrics', '_dynamic_tree_dirty', '_dynamic_net_ids_array', '_inv_grid_cell_size',
         '_static_bounds_array', '_static_is_rect_array', '_locked_nets',
-        '_static_raw_tree', '_static_raw_obj_ids', '_dynamic_bounds_array'
+        '_static_raw_tree', '_static_raw_obj_ids', '_dynamic_bounds_array', '_static_version'
     )
 
     def __init__(
@@ -53,6 +54,10 @@ class CollisionEngine:
         self._static_is_rect_array: numpy.ndarray | None = None
         self._static_raw_tree: STRtree | None = None
         self._static_raw_obj_ids: list[int] = []
+        self._net_specific_trees: dict[tuple[float, float], STRtree] = {}
+        self._net_specific_is_rect: dict[tuple[float, float], numpy.ndarray] = {}
+        self._net_specific_bounds: dict[tuple[float, float], numpy.ndarray] = {}
+        self._static_version = 0
         
         self.static_safe_cache: set[tuple] = set()
         self.static_grid: dict[tuple[int, int], list[int]] = {}
@@ -96,22 +101,21 @@ class CollisionEngine:
                 f"  Congestion: {m['congestion_tree_queries']} checks\n"
                 f"  Safety Zone: {m['safety_zone_checks']} full intersections performed")
 
-    def add_static_obstacle(self, polygon: Polygon) -> int:
+    def add_static_obstacle(self, polygon: Polygon, dilated_geometry: Polygon | None = None) -> int:
         obj_id = self._static_id_counter
         self._static_id_counter += 1
         
-        # Consistent with Wi/2 + C/2 separation:
-        # Buffer static obstacles by half clearance.
-        # Checkers must also buffer waveguide by Wi/2 + C/2.
-        dilated = polygon.buffer(self.clearance / 2.0, join_style=2)
+        # Preserve existing dilation if provided, else use default C/2
+        if dilated_geometry is not None:
+             dilated = dilated_geometry
+        else:
+             dilated = polygon.buffer(self.clearance / 2.0, join_style=2)
         
         self.static_geometries[obj_id] = polygon
         self.static_dilated[obj_id] = dilated
         self.static_prepared[obj_id] = prep(dilated)
         self.static_index.insert(obj_id, dilated.bounds)
-        self.static_tree = None
-        self._static_raw_tree = None
-        self.static_grid = {}
+        self._invalidate_static_caches()
         b = dilated.bounds
         area = (b[2] - b[0]) * (b[3] - b[1])
         self.static_is_rect[obj_id] = (abs(dilated.area - area) < 1e-4)
@@ -131,10 +135,21 @@ class CollisionEngine:
         del self.static_dilated[obj_id]
         del self.static_prepared[obj_id]
         del self.static_is_rect[obj_id]
-        
+        self._invalidate_static_caches()
+
+    def _invalidate_static_caches(self) -> None:
         self.static_tree = None
+        self._static_bounds_array = None
+        self._static_is_rect_array = None
+        self.static_obj_ids = []
         self._static_raw_tree = None
+        self._static_raw_obj_ids = []
         self.static_grid = {}
+        self._net_specific_trees.clear()
+        self._net_specific_is_rect.clear()
+        self._net_specific_bounds.clear()
+        self.static_safe_cache.clear()
+        self._static_version += 1
 
     def _ensure_static_tree(self) -> None:
         if self.static_tree is None and self.static_dilated:
@@ -144,6 +159,37 @@ class CollisionEngine:
             self._static_bounds_array = numpy.array([g.bounds for g in geoms])
             self._static_is_rect_array = numpy.array([self.static_is_rect[i] for i in self.static_obj_ids])
 
+    def _ensure_net_static_tree(self, net_width: float) -> STRtree:
+        """
+        Lazily generate a tree where obstacles are dilated by (net_width/2 + clearance).
+        """
+        key = (round(net_width, 4), round(self.clearance, 4))
+        if key in self._net_specific_trees:
+            return self._net_specific_trees[key]
+        
+        # Physical separation must be >= clearance.
+        # Centerline to raw obstacle edge must be >= net_width/2 + clearance.
+        total_dilation = net_width / 2.0 + self.clearance
+        geoms = []
+        is_rect_list = []
+        bounds_list = []
+        
+        for obj_id in sorted(self.static_geometries.keys()):
+            poly = self.static_geometries[obj_id]
+            dilated = poly.buffer(total_dilation, join_style=2)
+            geoms.append(dilated)
+            
+            b = dilated.bounds
+            bounds_list.append(b)
+            area = (b[2] - b[0]) * (b[3] - b[1])
+            is_rect_list.append(abs(dilated.area - area) < 1e-4)
+        
+        tree = STRtree(geoms)
+        self._net_specific_trees[key] = tree
+        self._net_specific_is_rect[key] = numpy.array(is_rect_list, dtype=bool)
+        self._net_specific_bounds[key] = numpy.array(bounds_list)
+        return tree
+
     def _ensure_static_raw_tree(self) -> None:
         if self._static_raw_tree is None and self.static_geometries:
             self._static_raw_obj_ids = sorted(self.static_geometries.keys())
@@ -205,7 +251,9 @@ class CollisionEngine:
         to_move = [obj_id for obj_id, (nid, _) in self.dynamic_geometries.items() if nid == net_id]
         for obj_id in to_move:
             poly = self.dynamic_geometries[obj_id][1]
-            self.add_static_obstacle(poly)
+            dilated = self.dynamic_dilated[obj_id]
+            # Preserve dilation for perfect consistency
+            self.add_static_obstacle(poly, dilated_geometry=dilated)
             
         # Remove from dynamic index (without triggering the locked-net guard)
         self.dynamic_tree = None
@@ -219,9 +267,9 @@ class CollisionEngine:
     def unlock_net(self, net_id: str) -> None:
         self._locked_nets.discard(net_id)
 
-    def check_move_straight_static(self, start_port: Port, length: float) -> bool:
+    def check_move_straight_static(self, start_port: Port, length: float, net_width: float) -> bool:
         self.metrics['static_straight_fast'] += 1
-        reach = self.ray_cast(start_port, start_port.orientation, max_dist=length + 0.01)
+        reach = self.ray_cast(start_port, start_port.orientation, max_dist=length + 0.01, net_width=net_width)
         return reach < length - 0.001
 
     def _is_in_safety_zone_fast(self, idx: int, start_port: Port | None, end_port: Port | None) -> bool:
@@ -236,19 +284,19 @@ class CollisionEngine:
                  b[1]-sz <= end_port.y <= b[3]+sz): return True
         return False
 
-    def check_move_static(self, result: ComponentResult, start_port: Port | None = None, end_port: Port | None = None) -> bool:
+    def check_move_static(self, result: ComponentResult, start_port: Port | None = None, end_port: Port | None = None, net_width: float | None = None) -> bool:
         if not self.static_dilated: return False
         self.metrics['static_tree_queries'] += 1
         self._ensure_static_tree()
         
-        # 1. Fast total bounds check
-        tb = result.total_bounds
+        # 1. Fast total bounds check (Use dilated bounds to ensure clearance is caught)
+        tb = result.total_dilated_bounds if result.total_dilated_bounds else result.total_bounds
         hits = self.static_tree.query(box(*tb))
         if hits.size == 0: return False
              
         # 2. Per-hit check
         s_bounds = self._static_bounds_array
-        move_poly_bounds = result.bounds
+        move_poly_bounds = result.dilated_bounds if result.dilated_bounds else result.bounds
         for hit_idx in hits:
             obs_b = s_bounds[hit_idx]
             
@@ -266,9 +314,6 @@ class CollisionEngine:
             if self._is_in_safety_zone_fast(hit_idx, start_port, end_port):
                  # If near port, we must use the high-precision check
                  obj_id = self.static_obj_ids[hit_idx]
-                 # Triggers lazy evaluation of geometry only if needed
-                 poly_move = result.geometry[0] # Simplification: assume 1 poly for now or loop
-                 # Actually, better loop over move polygons for high-fidelity
                  collision_found = False
                  for p_move in result.geometry:
                       if not self._is_in_safety_zone(p_move, obj_id, start_port, end_port):
@@ -277,13 +322,14 @@ class CollisionEngine:
                  return True
             
             # Not in safety zone and AABBs overlap - check real intersection
-            # This is the most common path for real collisions or near misses
             obj_id = self.static_obj_ids[hit_idx]
-            raw_obstacle = self.static_geometries[obj_id]
+            # Use dilated geometry (Wi/2 + C/2) against static_dilated (C/2) to get Wi/2 + C.
+            # Touching means gap is exactly C. Intersection without touches means gap < C.
             test_geoms = result.dilated_geometry if result.dilated_geometry else result.geometry
+            static_obs_dilated = self.static_dilated[obj_id]
             
             for i, p_test in enumerate(test_geoms):
-                if p_test.intersects(raw_obstacle):
+                if p_test.intersects(static_obs_dilated) and not p_test.touches(static_obs_dilated):
                     return True
         return False
 
@@ -339,11 +385,11 @@ class CollisionEngine:
         possible_total = (tb[0] < d_bounds[:, 2]) & (tb[2] > d_bounds[:, 0]) & \
                          (tb[1] < d_bounds[:, 3]) & (tb[3] > d_bounds[:, 1])
         
-        valid_hits = (self._dynamic_net_ids_array != net_id)
-        if not numpy.any(possible_total & valid_hits):
+        valid_hits_mask = (self._dynamic_net_ids_array != net_id)
+        if not numpy.any(possible_total & valid_hits_mask):
              return 0
 
-        # 2. Per-polygon AABB check using query on geometries (LAZY triggering)
+        # 2. Per-polygon check using query
         geoms_to_test = result.dilated_geometry if result.dilated_geometry else result.geometry
         res_indices, tree_indices = self.dynamic_tree.query(geoms_to_test, predicate='intersects')
         
@@ -351,8 +397,35 @@ class CollisionEngine:
              return 0
              
         hit_net_ids = numpy.take(self._dynamic_net_ids_array, tree_indices)
-        valid_geoms_hits = (hit_net_ids != net_id)
-        return int(numpy.sum(valid_geoms_hits))
+        
+        # Group by other net_id to minimize 'touches' calls
+        unique_other_nets = numpy.unique(hit_net_ids[hit_net_ids != net_id])
+        if unique_other_nets.size == 0:
+             return 0
+             
+        tree_geoms = self.dynamic_tree.geometries
+        real_hits_count = 0
+        
+        for other_nid in unique_other_nets:
+             other_mask = (hit_net_ids == other_nid)
+             sub_tree_indices = tree_indices[other_mask]
+             sub_res_indices = res_indices[other_mask]
+             
+             # Check if ANY hit for THIS other net is a real collision
+             found_real = False
+             for j in range(len(sub_tree_indices)):
+                  p_test = geoms_to_test[sub_res_indices[j]]
+                  p_tree = tree_geoms[sub_tree_indices[j]]
+                  if not p_test.touches(p_tree):
+                       # Add small area tolerance for numerical precision
+                       if p_test.intersection(p_tree).area > 1e-7:
+                            found_real = True
+                            break
+             
+             if found_real:
+                  real_hits_count += 1
+                  
+        return real_hits_count
 
     def _is_in_safety_zone(self, geometry: Polygon, obj_id: int, start_port: Port | None, end_port: Port | None) -> bool:
         """ 
@@ -392,17 +465,21 @@ class CollisionEngine:
             self._ensure_static_tree()
             if self.static_tree is None: return False
             
-            # Separation needed: (Wi + C)/2.
-            # static_dilated is buffered by C/2.
-            # So we need geometry buffered by Wi/2.
-            if dilated_geometry: 
+            # Separation needed: Centerline-to-WallEdge >= Wi/2 + C.
+            # static_tree has obstacles buffered by C/2.
+            # geometry is physical waveguide (Wi/2 from centerline).
+            # So we buffer geometry by C/2 to get Wi/2 + C/2.
+            # Intersection means separation < (Wi/2 + C/2) + C/2 = Wi/2 + C.
+            if dilated_geometry is not None: 
                  test_geom = dilated_geometry
             else:
-                 dist = (net_width / 2.0) if net_width is not None else 0.0
-                 test_geom = geometry.buffer(dist + 1e-7, join_style=2) if dist >= 0 else geometry
+                 dist = self.clearance / 2.0
+                 test_geom = geometry.buffer(dist + 1e-7, join_style=2) if dist > 0 else geometry
             
             hits = self.static_tree.query(test_geom, predicate='intersects')
+            tree_geoms = self.static_tree.geometries
             for hit_idx in hits:
+                if test_geom.touches(tree_geoms[hit_idx]): continue
                 obj_id = self.static_obj_ids[hit_idx]
                 if self._is_in_safety_zone(geometry, obj_id, start_port, end_port): continue
                 return True
@@ -412,60 +489,166 @@ class CollisionEngine:
         if self.dynamic_tree is None: return 0
         test_poly = dilated_geometry if dilated_geometry else geometry.buffer(self.clearance / 2.0)
         hits = self.dynamic_tree.query(test_poly, predicate='intersects')
-        count = 0
+        tree_geoms = self.dynamic_tree.geometries
+        hit_net_ids = []
         for hit_idx in hits:
+            if test_poly.touches(tree_geoms[hit_idx]): continue
             obj_id = self.dynamic_obj_ids[hit_idx]
-            if self.dynamic_geometries[obj_id][0] != net_id: count += 1
-        return count
+            other_id = self.dynamic_geometries[obj_id][0]
+            if other_id != net_id:
+                 hit_net_ids.append(other_id)
+        return len(numpy.unique(hit_net_ids)) if hit_net_ids else 0
 
     def is_collision(self, geometry: Polygon, net_id: str = 'default', net_width: float | None = None, start_port: Port | None = None, end_port: Port | None = None) -> bool:
         """ Unified entry point for static collision checks. """
         result = self.check_collision(geometry, net_id, buffer_mode='static', start_port=start_port, end_port=end_port, net_width=net_width)
         return bool(result)
 
-    def ray_cast(self, origin: Port, angle_deg: float, max_dist: float = 2000.0) -> float:
+    def verify_path(self, net_id: str, components: list[ComponentResult]) -> tuple[bool, int]:
+        """
+        Non-approximated, full-polygon intersection check of a path against all
+        static obstacles and other nets.
+        """
+        collision_count = 0
+        
+        # 1. Check against static obstacles
+        self._ensure_static_raw_tree()
+        if self._static_raw_tree is not None:
+             raw_geoms = self._static_raw_tree.geometries
+             for comp in components:
+                  # Use ACTUAL geometry, not dilated/proxy
+                  actual_geoms = comp.actual_geometry if comp.actual_geometry is not None else comp.geometry
+                  for p_actual in actual_geoms:
+                       # Physical separation must be >= clearance.
+                       p_verify = p_actual.buffer(self.clearance, join_style=2)
+                       hits = self._static_raw_tree.query(p_verify, predicate='intersects')
+                       for hit_idx in hits:
+                            p_obs = raw_geoms[hit_idx]
+                            # If they ONLY touch, gap is exactly clearance. Valid.
+                            if p_verify.touches(p_obs): continue
+                            
+                            obj_id = self._static_raw_obj_ids[hit_idx]
+                            if not self._is_in_safety_zone(p_actual, obj_id, None, None):
+                                 collision_count += 1
+        
+        # 2. Check against other nets
+        self._ensure_dynamic_tree()
+        if self.dynamic_tree is not None:
+             tree_geoms = self.dynamic_tree.geometries
+             for comp in components:
+                  # Robust fallback chain to ensure crossings are caught even with zero clearance
+                  d_geoms = comp.dilated_actual_geometry or comp.dilated_geometry or comp.actual_geometry or comp.geometry
+                  if not d_geoms: continue
+                  
+                  # Ensure d_geoms is a list/array for STRtree.query
+                  if not isinstance(d_geoms, (list, tuple, numpy.ndarray)):
+                       d_geoms = [d_geoms]
+
+                  res_indices, tree_indices = self.dynamic_tree.query(d_geoms, predicate='intersects')
+                  if tree_indices.size > 0:
+                       hit_net_ids = numpy.take(self._dynamic_net_ids_array, tree_indices)
+                       net_id_str = str(net_id)
+                       
+                       comp_hits = []
+                       for i in range(len(tree_indices)):
+                            if hit_net_ids[i] == net_id_str: continue
+                            
+                            p_new = d_geoms[res_indices[i]]
+                            p_tree = tree_geoms[tree_indices[i]]
+                            if not p_new.touches(p_tree):
+                                 # Numerical tolerance for area overlap
+                                 if p_new.intersection(p_tree).area > 1e-7:
+                                      comp_hits.append(hit_net_ids[i])
+                       
+                       if comp_hits:
+                            collision_count += len(numpy.unique(comp_hits))
+
+        return (collision_count == 0), collision_count
+
+    def ray_cast(self, origin: Port, angle_deg: float, max_dist: float = 2000.0, net_width: float | None = None) -> float:
         rad = numpy.radians(angle_deg)
         cos_v, sin_v = numpy.cos(rad), numpy.sin(rad)
         dx, dy = max_dist * cos_v, max_dist * sin_v
         min_x, max_x = sorted([origin.x, origin.x + dx])
         min_y, max_y = sorted([origin.y, origin.y + dy])
-        self._ensure_static_tree()
-        if self.static_tree is None: return max_dist
-        candidates = self.static_tree.query(box(min_x, min_y, max_x, max_y))
+        
+        key = None
+        if net_width is not None:
+             tree = self._ensure_net_static_tree(net_width)
+             key = (round(net_width, 4), round(self.clearance, 4))
+             is_rect_arr = self._net_specific_is_rect[key]
+             bounds_arr = self._net_specific_bounds[key]
+        else:
+             self._ensure_static_tree()
+             tree = self.static_tree
+             is_rect_arr = self._static_is_rect_array
+             bounds_arr = self._static_bounds_array
+
+        if tree is None: return max_dist
+        candidates = tree.query(box(min_x, min_y, max_x, max_y))
         if candidates.size == 0: return max_dist
+        
         min_dist = max_dist
         inv_dx = 1.0 / dx if abs(dx) > 1e-12 else 1e30
         inv_dy = 1.0 / dy if abs(dy) > 1e-12 else 1e30
-        b_arr = self._static_bounds_array[candidates]
-        dist_sq = (b_arr[:, 0] - origin.x)**2 + (b_arr[:, 1] - origin.y)**2
-        sorted_indices = numpy.argsort(dist_sq)
+        
+        tree_geoms = tree.geometries
         ray_line = None
-        for i in sorted_indices:
-            c = candidates[i]; b = self._static_bounds_array[c]
-            if abs(dx) < 1e-12:
+        
+        # Fast AABB-based pre-sort
+        candidates_bounds = bounds_arr[candidates]
+        # Distance to AABB min corner as heuristic
+        dist_sq = (candidates_bounds[:, 0] - origin.x)**2 + (candidates_bounds[:, 1] - origin.y)**2
+        sorted_indices = numpy.argsort(dist_sq)
+        
+        for idx in sorted_indices:
+            c = candidates[idx]
+            b = bounds_arr[c]
+            
+            # Fast axis-aligned ray-AABB intersection
+            # (Standard Slab method)
+            if abs(dx) < 1e-12: # Vertical ray
                 if origin.x < b[0] or origin.x > b[2]: tx_min, tx_max = 1e30, -1e30
                 else: tx_min, tx_max = -1e30, 1e30
             else:
                 t1, t2 = (b[0] - origin.x) * inv_dx, (b[2] - origin.x) * inv_dx
                 tx_min, tx_max = min(t1, t2), max(t1, t2)
-            if abs(dy) < 1e-12:
+                
+            if abs(dy) < 1e-12: # Horizontal ray
                 if origin.y < b[1] or origin.y > b[3]: ty_min, ty_max = 1e30, -1e30
                 else: ty_min, ty_max = -1e30, 1e30
             else:
                 t1, t2 = (b[1] - origin.y) * inv_dy, (b[3] - origin.y) * inv_dy
                 ty_min, ty_max = min(t1, t2), max(t1, t2)
+                
             t_min, t_max = max(tx_min, ty_min), min(tx_max, ty_max)
-            if t_max < 0 or t_min > t_max or t_min > 1.0 or t_min >= min_dist / max_dist: continue
-            if self._static_is_rect_array[c]:
-                min_dist = max(0.0, t_min * max_dist); continue
-            if ray_line is None: ray_line = LineString([(origin.x, origin.y), (origin.x + dx, origin.y + dy)])
-            obj_id = self.static_obj_ids[c]
-            if self.static_prepared[obj_id].intersects(ray_line):
-                intersection = ray_line.intersection(self.static_dilated[obj_id])
+            
+            # Intersection conditions
+            if t_max < 0 or t_min > t_max or t_min > 1.0: continue
+            
+            # If hit is further than current min_dist, skip
+            if t_min * max_dist >= min_dist: continue
+            
+            # HIGH PRECISION CHECK
+            if is_rect_arr[c]:
+                # Rectangles are perfectly described by their AABB
+                min_dist = max(0.0, t_min * max_dist)
+                continue
+            
+            # Fallback to full geometry check for non-rectangles (arcs, etc.)
+            if ray_line is None: 
+                ray_line = LineString([(origin.x, origin.y), (origin.x + dx, origin.y + dy)])
+            
+            obs_dilated = tree_geoms[c]
+            if obs_dilated.intersects(ray_line):
+                intersection = ray_line.intersection(obs_dilated)
                 if intersection.is_empty: continue
+                
                 def get_dist(geom):
                     if hasattr(geom, 'geoms'): return min(get_dist(g) for g in geom.geoms)
                     return numpy.sqrt((geom.coords[0][0] - origin.x)**2 + (geom.coords[0][1] - origin.y)**2)
+                
                 d = get_dist(intersection)
                 if d < min_dist: min_dist = d
+                
         return min_dist