navi-offroute: vectorize _cap_candidates (O2a, perf) (#49)

Co-authored-by: mj <mj@k7zvx.com>
Co-authored-by: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

backend/services/navi_offroute/tests/test_offroute.py

@@ -978,6 +978,39 @@ def test_transition_cap_closest_15(monkeypatch):
     assert len(capped) == 15 * 6                      # 6 directed tuples per lot
 
 
+def test_cap_candidates_vectorized_matches_scalar_oracle():
+    """O2a safety net: the vectorized _cap_candidates must select the SAME set as the scalar
+    reference. Oracle = the original group/score/filter/sort/take-K loop using the retained
+    scalar _cross_track_distance_m. 200 points on an east-west line at strictly increasing
+    perpendicular offsets (no boundary ties), straddling the 5 km radius; 2 tuples/point."""
+    line = ((40.0, -111.0), (40.0, -110.0))           # east-west at lat 40
+    raw = []
+    for i in range(200):
+        lat = 40.0 + (i + 1) * 0.0008                 # perp dist ~ (i+1)*89 m -> ~56 within 5 km
+        raw.append((lat, -110.5, 0, 3, 60.0))
+        raw.append((lat, -110.5, 3, 0, 60.0))
+
+    def oracle(raw, line):
+        groups = {}
+        for t in raw:
+            groups.setdefault((t[0], t[1]), []).append(t)
+        scored = []
+        for (la, lo), tuples in groups.items():
+            d = _trans._cross_track_distance_m(la, lo, line)
+            if d <= _trans._CAP_RADIUS_M:
+                scored.append((d, tuples))
+        scored.sort(key=lambda x: x[0])
+        out = []
+        for _d, tuples in scored[:_trans._CAP_PER_TYPE]:
+            out.extend(tuples)
+        return out
+
+    vec = _trans._cap_candidates(raw, line)
+    orc = oracle(raw, line)
+    assert set(vec) == set(orc)
+    assert len(vec) == len(orc) == _trans._CAP_PER_TYPE * 2   # 15 closest points x 2 tuples
+
+
 def test_compute_unified_cost_layers_perf():
     """≤1 s to build 4 cost layers + transition cells for a ~50 km bbox (spec §5 gate).
     Requires the real parking/trailhead DBs + a reachable Valhalla; skips otherwise."""

backend/services/navi_offroute/transitions.py

@@ -77,24 +77,41 @@ def _cross_track_distance_m(lat, lon, line):
 def _cap_candidates(raw, line):
     """§5 cap for one transition type: group by (lat, lon) so a point's several directed
     tuples count as ONE candidate, keep the closest _CAP_PER_TYPE points within
-    _CAP_RADIUS_M of `line`, flatten. line=None -> uncapped (test convenience)."""
+    _CAP_RADIUS_M of `line`, flatten. line=None -> uncapped (test convenience).
+
+    Vectorised (O2a perf): the per-unique-point cross-track distance is computed in one numpy
+    pass instead of ~1M pure-Python great-circle calls (the dominant Route B cost). The math
+    is identical to the scalar _cross_track_distance_m / _bearing (retained as the test
+    oracle); the result is set-equivalent — the kernel consumes the cells order-independently."""
     if not raw:
         return []
     if line is None:
         return list(raw)
-    groups = {}
-    for t in raw:
-        groups.setdefault((t[0], t[1]), []).append(t)
-    scored = []
-    for (lat, lon), tuples in groups.items():
-        d = _cross_track_distance_m(lat, lon, line)
-        if d <= _CAP_RADIUS_M:
-            scored.append((d, tuples))
-    scored.sort(key=lambda x: x[0])
-    out = []
-    for _d, tuples in scored[:_CAP_PER_TYPE]:
-        out.extend(tuples)
-    return out
+    (lat1, lon1), (lat2, lon2) = line
+    pts = np.array([(t[0], t[1]) for t in raw], dtype=np.float64)   # (N, 2) lat/lon
+    uniq, inv = np.unique(pts, axis=0, return_inverse=True)         # one row per physical point
+    inv = inv.reshape(-1)
+
+    # Cross-track distance per unique point -- vectorised twin of _cross_track_distance_m.
+    phi1, lam1 = math.radians(lat1), math.radians(lon1)
+    phi3, lam3 = np.radians(uniq[:, 0]), np.radians(uniq[:, 1])
+    h = (np.sin((phi3 - phi1) / 2) ** 2
+         + math.cos(phi1) * np.cos(phi3) * np.sin((lam3 - lam1) / 2) ** 2)
+    d13 = 2 * np.arcsin(np.minimum(1.0, np.sqrt(h)))               # angular distance (radians)
+    if lat1 == lat2 and lon1 == lon2:
+        dxt = d13 * _EARTH_R_M                                     # degenerate line -> point dist
+    else:
+        theta12 = _bearing(phi1, lam1, math.radians(lat2), math.radians(lon2))
+        theta13 = np.arctan2(
+            np.sin(lam3 - lam1) * np.cos(phi3),
+            math.cos(phi1) * np.sin(phi3) - math.sin(phi1) * np.cos(phi3) * np.cos(lam3 - lam1))
+        dxt = np.abs(np.arcsin(np.clip(np.sin(d13) * np.sin(theta13 - theta12), -1.0, 1.0))) * _EARTH_R_M
+
+    within = np.nonzero(dxt <= _CAP_RADIUS_M)[0]                   # unique points within 5 km
+    if within.size > _CAP_PER_TYPE:                               # keep the closest _CAP_PER_TYPE
+        within = within[np.argpartition(dxt[within], _CAP_PER_TYPE)[:_CAP_PER_TYPE]]
+    keep = np.isin(inv, within)                                   # raw entries whose point survives
+    return [raw[i] for i in np.nonzero(keep)[0].tolist()]
 
 
 def parking_transitions_near_line(line, buffer_m=5000):