训练新版

Env/utils.py

@@ -5,6 +5,86 @@ import random
 from metadrive.type import MetaDriveType
 
 
+def _static_obbs_overlap(car_a, car_b):
+    """
+    Check if two static vehicle OBBs overlap (2D SAT).
+    car_a, car_b: dicts with "begin" (x, y), "heading" (rad), "length", "width".
+    begin is center; half-extents are length/2, width/2.
+    """
+    def _get_corners(car):
+        cx, cy = car["begin"][0], car["begin"][1]
+        h = float(car["heading"])
+        L2 = float(car["length"]) / 2.0
+        W2 = float(car["width"]) / 2.0
+        ux, uy = np.cos(h), np.sin(h)
+        vx, vy = -np.sin(h), np.cos(h)
+        return np.array([
+            [cx + L2 * ux + W2 * vx, cy + L2 * uy + W2 * vy],
+            [cx + L2 * ux - W2 * vx, cy + L2 * uy - W2 * vy],
+            [cx - L2 * ux - W2 * vx, cy - L2 * uy - W2 * vy],
+            [cx - L2 * ux + W2 * vx, cy - L2 * uy + W2 * vy],
+        ])
+
+    def _get_axes(car):
+        h = float(car["heading"])
+        return [
+            np.array([np.cos(h), np.sin(h)]),
+            np.array([-np.sin(h), np.cos(h)]),
+        ]
+
+    corners_a = _get_corners(car_a)
+    corners_b = _get_corners(car_b)
+    axes = _get_axes(car_a) + _get_axes(car_b)
+
+    for axis in axes:
+        proj_a = corners_a @ axis
+        proj_b = corners_b @ axis
+        min_a, max_a = proj_a.min(), proj_a.max()
+        min_b, max_b = proj_b.min(), proj_b.max()
+        if max_a < min_b or max_b < min_a:
+            return False
+    return True
+
+
+def _deduplicate_background_by_collision(background_vehicles):
+    """
+    Merge static tracks that collide (same physical vehicle). Build collision graph,
+    find connected components, keep one representative per component (min show_time, then scenario_id).
+    """
+    if not background_vehicles:
+        return background_vehicles
+    items = list(background_vehicles.items())
+    n = len(items)
+    # Build adjacency by index
+    parent = list(range(n))
+
+    def find(i):
+        if parent[i] != i:
+            parent[i] = find(parent[i])
+        return parent[i]
+
+    def union(i, j):
+        pi, pj = find(i), find(j)
+        if pi != pj:
+            parent[pi] = pj
+
+    for i in range(n):
+        for j in range(i + 1, n):
+            if _static_obbs_overlap(items[i][1], items[j][1]):
+                union(i, j)
+
+    # Representative per component: index with min (show_time, scenario_id)
+    comp_rep = {}
+    for i in range(n):
+        r = find(i)
+        sid, car = items[i][0], items[i][1]
+        key = (car.get("show_time", 0), sid)
+        if r not in comp_rep or key < comp_rep[r][0]:
+            comp_rep[r] = (key, sid, car)
+
+    return {sid: car for (_, sid, car) in comp_rep.values()}
+
+
 def is_on_lane(pos, map_manager, threshold=2.0):
     """Check if a position is on a valid lane (within lateral tolerance)."""
     if map_manager is None or map_manager.current_map is None:
@@ -31,6 +111,7 @@ def filter_traffic_tracks_to_birth_lists(
     static_displacement_threshold=5.0,
     static_speed_threshold=1.0,
     return_stats=False,
+    deduplicate_static_by_collision=True,
 ):
     """
     Filter traffic tracks into controlled (car_birth_info_list) and background lists.
@@ -127,6 +208,9 @@ def filter_traffic_tracks_to_birth_lists(
             "width": track["state"]["width"][first_show],
         })
 
+    if deduplicate_static_by_collision and background_vehicles:
+        background_vehicles = _deduplicate_background_by_collision(background_vehicles)
+
     if return_stats:
         stats = {
             "n_total": n_total,