warm start and some perf

inire/geometry/collision.py

@@ -96,7 +96,7 @@ 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) -> None:
+    def add_static_obstacle(self, polygon: Polygon) -> int:
         obj_id = self._static_id_counter
         self._static_id_counter += 1
         
@@ -115,6 +115,26 @@ class CollisionEngine:
         b = dilated.bounds
         area = (b[2] - b[0]) * (b[3] - b[1])
         self.static_is_rect[obj_id] = (abs(dilated.area - area) < 1e-4)
+        return obj_id
+
+    def remove_static_obstacle(self, obj_id: int) -> None:
+        """
+        Remove a static obstacle by ID.
+        """
+        if obj_id not in self.static_geometries:
+            return
+            
+        bounds = self.static_dilated[obj_id].bounds
+        self.static_index.delete(obj_id, bounds)
+        
+        del self.static_geometries[obj_id]
+        del self.static_dilated[obj_id]
+        del self.static_prepared[obj_id]
+        del self.static_is_rect[obj_id]
+        
+        self.static_tree = None
+        self._static_raw_tree = None
+        self.static_grid = {}
 
     def _ensure_static_tree(self) -> None:
         if self.static_tree is None and self.static_dilated:
@@ -229,26 +249,43 @@ class CollisionEngine:
         tb = result.total_dilated_bounds
         if tb is None: return 0
         self._ensure_dynamic_grid()
-        if not self.dynamic_grid: return 0
+        dynamic_grid = self.dynamic_grid
+        if not dynamic_grid: return 0
         
         cs_inv = self._inv_grid_cell_size
-        gx_min, gy_min = int(tb[0] * cs_inv), int(tb[1] * cs_inv)
-        gx_max, gy_max = int(tb[2] * cs_inv), int(tb[3] * cs_inv)
+        gx_min = int(tb[0] * cs_inv)
+        gy_min = int(tb[1] * cs_inv)
+        gx_max = int(tb[2] * cs_inv)
+        gy_max = int(tb[3] * cs_inv)
         
-        any_possible = False
-        dynamic_grid = self.dynamic_grid
         dynamic_geometries = self.dynamic_geometries
+        
+        # Fast path for single cell
+        if gx_min == gx_max and gy_min == gy_max:
+            cell = (gx_min, gy_min)
+            if cell in dynamic_grid:
+                for obj_id in dynamic_grid[cell]:
+                    if dynamic_geometries[obj_id][0] != net_id:
+                        return self._check_real_congestion(result, net_id)
+            return 0
+
+        # General case
+        any_possible = False
         for gx in range(gx_min, gx_max + 1):
             for gy in range(gy_min, gy_max + 1):
                 cell = (gx, gy)
                 if cell in dynamic_grid:
                     for obj_id in dynamic_grid[cell]:
                         if dynamic_geometries[obj_id][0] != net_id:
-                            any_possible = True; break
+                            any_possible = True
+                            break
                 if any_possible: break
             if any_possible: break
+            
         if not any_possible: return 0
-        
+        return self._check_real_congestion(result, net_id)
+
+    def _check_real_congestion(self, result: ComponentResult, net_id: str) -> int:
         self.metrics['congestion_tree_queries'] += 1
         self._ensure_dynamic_tree()
         if self.dynamic_tree is None: return 0
@@ -259,28 +296,19 @@ class CollisionEngine:
         possible_total = (tb[0] < d_bounds[:, 2]) & (tb[2] > d_bounds[:, 0]) & \
                          (tb[1] < d_bounds[:, 3]) & (tb[3] > d_bounds[:, 1])
         
-        # Filter by net_id (important for negotiated congestion)
         valid_hits = (self._dynamic_net_ids_array != net_id)
         if not numpy.any(possible_total & valid_hits):
              return 0
 
         # 2. Per-polygon AABB check using query on geometries (LAZY triggering)
-        # We only trigger evaluation if total bounds intersect with other nets.
         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')
         
         if tree_indices.size == 0:
              return 0
              
-        # Filter out self-overlaps (from same net)
         hit_net_ids = numpy.take(self._dynamic_net_ids_array, tree_indices)
         valid_geoms_hits = (hit_net_ids != net_id)
-        if not numpy.any(valid_geoms_hits):
-             return 0
-
-        # 3. Real geometry check (Only if AABBs intersect with other nets)
-        # We already have hits from STRtree which are accurate for polygons too.
-        # But wait, query(..., predicate='intersects') ALREADY does real check!
         return int(numpy.sum(valid_geoms_hits))
 
     def _is_in_safety_zone(self, geometry: Polygon, obj_id: int, start_port: Port | None, end_port: Port | None) -> bool: