Fix corridor-only collision index delaying braking to path end

CHANGELOG.md

@@ -25,6 +25,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Docs: plugin registry field tables list every field with a required column (README, Overview, Plugin Development)
 
 ### Fixed
+- Common: corridor-only side collisions now return the mid-path index — `_find_collision_index` uses centerline distance ≤ ego half-width + margin (same criterion as the buffered corridor) instead of a bare `LineString.intersects` that fell back to `n-1` and delayed braking until path end
 - Common: `TrajectoryTracker` initializes `path_s` from cumulative arc-length instead of re-projecting the reference through KD-tree Frenet conversion — closed tracks with `first==last` (e.g. bundled Yas Marina race line) no longer get non-monotonic `path_s` with `path_s[-1] == 0`
 - Common: Frenet XY→SD picks the better adjacent segment around the nearest waypoint (and SD→XY brackets by arc-length) — on-path points after corners no longer pick up a huge false CTE from the previous segment
 - Common / Planning: lattice sampling, replan end-of-track gates, and race lap detection use `TrajectoryTracker.track_end_s` (`path_s[-1]`) instead of the stale `path_s[-2]` workaround — avoids `IndexError` on 1-point paths and restores the final closed-track segment after the cumulative `path_s` fix

avlite/c50_common/c55_collision_checking.py

@@ -174,7 +174,7 @@ def check_collision(
     min_clearance = _LARGE_CLEARANCE
     for obstacle, agent_velocity in obstacles:
         if trajectory_corridor.intersects(obstacle):
-            idx = _find_collision_index(trajectory_line, obstacle, path_x, path_y)
+            idx = _find_collision_index(trajectory_line, obstacle, path_x, path_y, radius)
             if best_idx is None or idx < best_idx:
                 best_idx, best_vel = idx, agent_velocity
             min_clearance = 0.0
@@ -190,28 +190,40 @@ def check_collision(
     return False, -1, -1, min_clearance
 
 
-def _find_collision_index(trajectory_line: LineString, obstacle_polygon: Polygon,
-                          path_x: np.ndarray, path_y: np.ndarray) -> int:
+def _find_collision_index(
+    trajectory_line: LineString,
+    obstacle_polygon: Polygon,
+    path_x: np.ndarray,
+    path_y: np.ndarray,
+    radius: float = 0.0,
+) -> int:
     """
-    Find the approximate trajectory index where collision with obstacle occurs.
-    Uses binary search for efficiency.
+    Find the approximate trajectory index where the ego corridor first hits the obstacle.
+
+    Detection uses a buffered centerline (``radius`` = ego half-width + safety margin).
+    Index search must use the same criterion: a centerline-only intersect check misses
+    buffer-only side hits and silently falls back to ``n-1``, which delays braking until
+    the path end.
     """
     n = len(path_x)
     if n < 2:
         return 0
 
-    # Binary search to find first collision point
+    # Binary search to find first corridor collision point
     left, right = 1, n - 1  # Start from 1 to ensure at least 2 points
     collision_idx = n - 1  # default to end if can't find
+    # Match corridor.intersects: distance to the obstacle within ego half-width + margin.
+    # Tiny epsilon absorbs shapely flat-cap / float noise at the touch boundary.
+    touch_dist = float(radius) + 1e-9
 
     while left <= right:
         mid = (left + right) // 2
-        # Check if segment from start to mid intersects obstacle
+        # Check if corridor from start to mid touches obstacle
         # Ensure we have at least 2 points for a valid LineString
         end_idx = max(2, mid + 1)
         partial_line = LineString(list(zip(path_x[:end_idx], path_y[:end_idx])))
 
-        if partial_line.intersects(obstacle_polygon):
+        if partial_line.distance(obstacle_polygon) <= touch_dist:
             collision_idx = mid
             right = mid - 1  # Search earlier
         else:

test/c50_common/test_c55_collision_checking.py

@@ -129,3 +129,32 @@ def test_front_corner_side_overlap_detected(self):
             collision_safety_margin=margin,
         )
         assert hit is True
+
+
+class TestBufferOnlyCollisionIndex:
+    """Corridor-only side hits must report the mid-path index, not path end."""
+
+    def test_side_obstacle_index_is_near_agent_not_path_end(self):
+        # Centerline clears the agent polygon, but ego half-width + margin corridor hits.
+        # Old binary search used bare LineString.intersects and fell back to n-1.
+        ego_w = 2.0
+        margin = 0.3
+        n = 31
+        agent_x = 30.0
+        # Agent half-width 1.0 → body gap to y=0 centerline is 0.5 m (< radius 1.3).
+        agent_y = 1.5
+        trajectory = _straight_trajectory(0.0, 100.0, n=n)
+        pm = PerceptionModel(
+            ego_vehicle=EgoState(x=0.0, y=0.0, theta=0.0, velocity=5.0, width=ego_w),
+            agent_vehicles=[
+                AgentState(x=agent_x, y=agent_y, theta=0.0, velocity=0.0, agent_id=1, width=2.0),
+            ],
+        )
+        hit, idx, _, clearance = check_collision(
+            pm, trajectory, collision_safety_margin=margin,
+        )
+        assert hit is True
+        assert clearance == 0.0
+        expected = int(round(agent_x / 100.0 * (n - 1)))
+        assert abs(idx - expected) <= 2, f"collision idx {idx} far from agent wp {expected} (n-1={n - 1})"
+        assert idx < n - 1