navi-offroute: multi-mode A* kernel (Phase 2) (#38)

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

backend/services/navi_offroute/astar.py

@@ -263,3 +263,313 @@ def astar_multigoal(
                                 break
 
     return -1, np.empty((0, 2), dtype=np.int64), INF
+
+
+# ═══════════════════════════════════════════════════════════════════════════════
+# MULTI-MODE A* (unified-graph, spec §2.3 / §10 / §11)
+# ═══════════════════════════════════════════════════════════════════════════════
+#
+# astar_multigoal_multimode extends the single-mode search to a (row, col, mode)
+# state space: mode is part of the state, and mode-switching transition edges
+# (parking lots, trailheads, road termini, surface boundaries) let the optimizer
+# decide WHERE a mode change happens instead of a fixed leg ordering. Single-mode
+# astar_multigoal above is unchanged and still serves explicit-mode requests; this
+# kernel is invoked only by Auto (wired in Phase 4).
+
+
+@njit(cache=True)
+def _movement_edge_time(cr, cc, nr, nc, dr, dc,
+                        elevation, cost_mult, trail_grid, trail_friction_lookup,
+                        barrier_grid, boundary_mode_id,
+                        cell_size_lat_m, cell_size_lon_m,
+                        max_grade, speed_function_id, base_speed_kmh):
+    """Per-edge time (s) for one 8-neighbour grid step in a SINGLE mode, or INF if
+    the edge is impassable / should be skipped. This is exactly the per-edge math
+    inlined in astar_multigoal (smooth slope penalty, trail-takes-both, barrier /
+    boundary rule), factored out for reuse by astar_multigoal_multimode.
+    astar_multigoal itself keeps its own inlined copy and is left unchanged."""
+    elev_cur = elevation[cr, cc]
+    elev_n = elevation[nr, nc]
+    if math.isnan(elev_cur) or math.isnan(elev_n):
+        return INF
+    dlat = dr * cell_size_lat_m
+    dlon = dc * cell_size_lon_m
+    dist = math.sqrt(dlat * dlat + dlon * dlon)
+    signed_grade = (elev_n - elev_cur) / dist
+    overshoot = abs(signed_grade) - max_grade
+    slope_penalty = 1.0
+    if overshoot > 0.0:
+        slope_penalty = math.exp(overshoot * SLOPE_PENALTY_SCALE)
+        if slope_penalty > SLOPE_PENALTY_CAP:
+            return INF
+    spd = _speed_kmh(signed_grade, speed_function_id, base_speed_kmh, max_grade)
+    if spd <= 1e-9:
+        return INF
+    base_time = dist * 3.6 / spd
+    base_time *= slope_penalty
+
+    tv_cur = trail_grid[cr, cc]
+    tv_n = trail_grid[nr, nc]
+    if tv_cur > 0 or tv_n > 0:
+        # Trail-takes-both: pick the lower-friction trail cell.
+        fc = trail_friction_lookup[tv_cur] if tv_cur > 0 else INF
+        fn = trail_friction_lookup[tv_n] if tv_n > 0 else INF
+        tf = fc if fc < fn else fn
+        if not (tf < INF):
+            return INF  # impassable trail for this mode
+        edge = base_time * tf
+    else:
+        mc = cost_mult[cr, cc]
+        mn = cost_mult[nr, nc]
+        if (not (mc < INF)) or (not (mn < INF)):
+            return INF  # impassable terrain (incl. wilderness)
+        edge = base_time * 0.5 * (mc + mn)
+
+    if boundary_mode_id == 0:  # strict
+        if barrier_grid[cr, cc] == 255 or barrier_grid[nr, nc] == 255:
+            return INF
+    elif boundary_mode_id == 1:  # pragmatic
+        if barrier_grid[cr, cc] == 255 or barrier_grid[nr, nc] == 255:
+            edge *= 5.0
+    # emergency (2): ignore barriers
+    return edge
+
+
+@njit(cache=True)
+def astar_multigoal_multimode(
+    cost_mult_stack,        # 3D float64 [rows, cols, n_modes]: per-mode context mult (inf=impassable)
+    elevation,              # 2D float64: metres (NaN = impassable)
+    cell_size_lat_m,        # float
+    cell_size_lon_m,        # float
+    max_grade_arr,          # 1D float64 [n_modes]: tan(max_slope) per mode
+    speed_function_ids,     # 1D int [n_modes]: 0=tobler 1=herzog 2=linear
+    base_speed_kmh_arr,     # 1D float64 [n_modes]
+    trail_grid,             # 2D uint8: 0=none else trail value (5/15/25)
+    trail_friction_stack,   # 2D float64 [n_modes, 256]: friction by trail value per mode
+    barrier_grid,           # 2D uint8: 255=barrier
+    boundary_mode_id,       # int: 0=strict 1=pragmatic 2=emergency
+    origin_row, origin_col,
+    origin_modes,           # 1D int: allowed start modes (seeds)
+    goal_rows, goal_cols,   # 1D int arrays
+    goal_modes,             # 1D int: allowed end modes
+    trans_rows,             # 1D int [n_trans]: transition cell rows
+    trans_cols,             # 1D int [n_trans]: transition cell cols
+    trans_from_mode,        # 1D int [n_trans]: source mode index
+    trans_to_mode,          # 1D int [n_trans]: target mode index
+    trans_cost_s,           # 1D float64 [n_trans]: transition penalty seconds
+    disable_heuristic=False,  # tests only: h≡0 turns the search into Dijkstra (admissibility oracle)
+):
+    """A* over (row, col, mode). The first (goal cell, allowed goal mode) state
+    popped wins; optimal under the per-mode admissible heuristic (§10). Returns
+    (best_goal_idx, path, total_cost) where path is int64 (N,3) of (row,col,mode)
+    from an origin-mode seed to the goal. (-1, empty, inf) if unreachable."""
+    rows = elevation.shape[0]
+    cols = elevation.shape[1]
+    n_modes = cost_mult_stack.shape[2]
+    rc = rows * cols  # cells per mode-plane; heap id = mode*rc + row*cols + col (§11)
+
+    goal_index = np.full((rows, cols), -1, dtype=np.int64)
+    for gi in range(goal_rows.shape[0]):
+        goal_index[goal_rows[gi], goal_cols[gi]] = gi
+    goal_mode_ok = np.zeros(n_modes, dtype=np.bool_)
+    for gi in range(goal_modes.shape[0]):
+        goal_mode_ok[goal_modes[gi]] = True
+
+    # §10: divide straight-line distance by the FASTEST base speed over goal modes
+    # -> smallest possible finishing time -> admissible lower bound. Independent of
+    # the state's current mode (a slow-mode state may switch to a fast mode later).
+    max_goal_speed = 0.0
+    for gi in range(goal_modes.shape[0]):
+        s = base_speed_kmh_arr[goal_modes[gi]]
+        if s > max_goal_speed:
+            max_goal_speed = s
+
+    g_score = np.full((rows, cols, n_modes), INF, dtype=np.float64)
+    parent = np.full((rows, cols, n_modes), -1, dtype=np.int64)  # parent's packed heap id
+    closed = np.zeros((rows, cols, n_modes), dtype=np.bool_)
+
+    # Per-cell transition index, built once before the loop. Sort transitions by
+    # packed cell key so each cell's edges are contiguous, then trans_head/trans_cnt
+    # give O(1) lookup -- a CSR layout, no numba-typed dict (compiles in nopython).
+    n_trans = trans_rows.shape[0]
+    trans_head = np.full((rows, cols), -1, dtype=np.int64)
+    trans_cnt = np.zeros((rows, cols), dtype=np.int64)
+    s_rows = trans_rows
+    s_cols = trans_cols
+    s_from = trans_from_mode
+    s_to = trans_to_mode
+    s_cost = trans_cost_s
+    if n_trans > 0:
+        key = np.empty(n_trans, dtype=np.int64)
+        for t in range(n_trans):
+            key[t] = trans_rows[t] * cols + trans_cols[t]
+        order = np.argsort(key)
+        s_rows = trans_rows[order]
+        s_cols = trans_cols[order]
+        s_from = trans_from_mode[order]
+        s_to = trans_to_mode[order]
+        s_cost = trans_cost_s[order]
+        for t in range(n_trans):
+            r = s_rows[t]
+            c = s_cols[t]
+            if trans_head[r, c] == -1:
+                trans_head[r, c] = t
+            trans_cnt[r, c] += 1
+
+    # Binary min-heap (lazy deletion). Capacity covers re-pushes from movement +
+    # transition relaxations across the n_modes-fold state space.
+    cap = rc * n_modes * 8
+    if cap < 1024:
+        cap = 1024
+    heap_id = np.empty(cap, dtype=np.int64)
+    heap_f = np.empty(cap, dtype=np.float64)
+    hsize = 0
+
+    def heuristic(r, c):
+        if disable_heuristic:
+            return 0.0
+        best = INF
+        for gi in range(goal_rows.shape[0]):
+            dr = (r - goal_rows[gi]) * cell_size_lat_m
+            dc = (c - goal_cols[gi]) * cell_size_lon_m
+            d = math.sqrt(dr * dr + dc * dc)
+            if d < best:
+                best = d
+        return best * 3.6 / max_goal_speed  # metres -> seconds at fastest goal speed
+
+    # Seed every allowed origin mode at the origin cell.
+    for oi in range(origin_modes.shape[0]):
+        m0 = origin_modes[oi]
+        g_score[origin_row, origin_col, m0] = 0.0
+        heap_id[hsize] = m0 * rc + origin_row * cols + origin_col
+        heap_f[hsize] = heuristic(origin_row, origin_col)
+        hsize += 1
+
+    while hsize > 0:
+        # Pop min.
+        cur_id = heap_id[0]
+        hsize -= 1
+        heap_id[0] = heap_id[hsize]
+        heap_f[0] = heap_f[hsize]
+        i = 0
+        while True:
+            l = 2 * i + 1
+            r2 = 2 * i + 2
+            sm = i
+            if l < hsize and heap_f[l] < heap_f[sm]:
+                sm = l
+            if r2 < hsize and heap_f[r2] < heap_f[sm]:
+                sm = r2
+            if sm != i:
+                tid = heap_id[i]; heap_id[i] = heap_id[sm]; heap_id[sm] = tid
+                tf = heap_f[i]; heap_f[i] = heap_f[sm]; heap_f[sm] = tf
+                i = sm
+            else:
+                break
+
+        m = cur_id // rc
+        rem = cur_id % rc
+        cr = rem // cols
+        cc = rem % cols
+        if closed[cr, cc, m]:
+            continue  # stale heap entry
+        closed[cr, cc, m] = True
+
+        if goal_index[cr, cc] >= 0 and goal_mode_ok[m]:
+            # First (goal cell, allowed goal mode) popped is optimal -- trace back.
+            length = 1
+            node = cur_id
+            pm = node // rc; prem = node % rc; pr = prem // cols; pc = prem % cols
+            while parent[pr, pc, pm] != -1:
+                length += 1
+                node = parent[pr, pc, pm]
+                pm = node // rc; prem = node % rc; pr = prem // cols; pc = prem % cols
+            path = np.empty((length, 3), dtype=np.int64)
+            node = cur_id
+            k = length - 1
+            while node != -1:
+                pm = node // rc; prem = node % rc; pr = prem // cols; pc = prem % cols
+                path[k, 0] = pr
+                path[k, 1] = pc
+                path[k, 2] = pm
+                k -= 1
+                node = parent[pr, pc, pm]
+            return goal_index[cr, cc], path, g_score[cr, cc, m]
+
+        g_cur = g_score[cr, cc, m]
+        if math.isnan(elevation[cr, cc]):
+            continue
+
+        cm = cost_mult_stack[:, :, m]
+        tfl = trail_friction_stack[m]
+        mg = max_grade_arr[m]
+        sfid = speed_function_ids[m]
+        bspd = base_speed_kmh_arr[m]
+
+        # Movement edges: 8-neighbour grid step in the SAME mode.
+        for dr in range(-1, 2):
+            for dc in range(-1, 2):
+                if dr == 0 and dc == 0:
+                    continue
+                nr = cr + dr
+                nc = cc + dc
+                if nr < 0 or nr >= rows or nc < 0 or nc >= cols:
+                    continue
+                if closed[nr, nc, m]:
+                    continue
+                edge = _movement_edge_time(
+                    cr, cc, nr, nc, dr, dc,
+                    elevation, cm, trail_grid, tfl,
+                    barrier_grid, boundary_mode_id,
+                    cell_size_lat_m, cell_size_lon_m, mg, sfid, bspd)
+                if not (edge < INF):
+                    continue
+                tentative = g_cur + edge
+                if tentative < g_score[nr, nc, m]:
+                    g_score[nr, nc, m] = tentative
+                    parent[nr, nc, m] = cur_id
+                    f = tentative + heuristic(nr, nc)
+                    if hsize < cap:
+                        heap_id[hsize] = m * rc + nr * cols + nc
+                        heap_f[hsize] = f
+                        j = hsize
+                        hsize += 1
+                        while j > 0:
+                            par = (j - 1) // 2
+                            if heap_f[j] < heap_f[par]:
+                                tid = heap_id[j]; heap_id[j] = heap_id[par]; heap_id[par] = tid
+                                tf2 = heap_f[j]; heap_f[j] = heap_f[par]; heap_f[par] = tf2
+                                j = par
+                            else:
+                                break
+
+        # Transition edges: same cell, mode change m -> to_m (flat penalty, no terrain).
+        if n_trans > 0 and trans_head[cr, cc] != -1:
+            base = trans_head[cr, cc]
+            cnt = trans_cnt[cr, cc]
+            for t in range(base, base + cnt):
+                if s_from[t] != m:
+                    continue
+                to_m = s_to[t]
+                if closed[cr, cc, to_m]:
+                    continue
+                tentative = g_cur + s_cost[t]
+                if tentative < g_score[cr, cc, to_m]:
+                    g_score[cr, cc, to_m] = tentative
+                    parent[cr, cc, to_m] = cur_id
+                    f = tentative + heuristic(cr, cc)
+                    if hsize < cap:
+                        heap_id[hsize] = to_m * rc + cr * cols + cc
+                        heap_f[hsize] = f
+                        j = hsize
+                        hsize += 1
+                        while j > 0:
+                            par = (j - 1) // 2
+                            if heap_f[j] < heap_f[par]:
+                                tid = heap_id[j]; heap_id[j] = heap_id[par]; heap_id[par] = tid
+                                tf2 = heap_f[j]; heap_f[j] = heap_f[par]; heap_f[par] = tf2
+                                j = par
+                            else:
+                                break
+
+    return -1, np.empty((0, 3), dtype=np.int64), INF

backend/services/navi_offroute/tests/test_offroute.py

@@ -989,3 +989,191 @@ def test_hybrid_consumes_parking_candidates(monkeypatch):
     trans = next(f for f in out["route"]["features"]
                  if f["properties"].get("kind") == "transition")
     assert trans["properties"]["name"] == "BLM Trailhead Lot"
+
+
+# ── Multi-mode A* kernel (unified-graph Phase 2; spec §2.3 / §10 / §11) ───────
+from services.navi_offroute.astar import astar_multigoal_multimode as _mm
+from services.navi_offroute.cost import MODE_PROFILES as _PROFILES
+
+_MODE_ORDER = ["foot", "2w", "4w", "vehicle"]   # spec §2.1 fixed index order
+_SFID = {"tobler": 0, "herzog": 1, "linear": 2}
+
+
+def _mode_param_arrays():
+    """Per-mode 1D param arrays (foot,2w,4w,vehicle order) + trail_friction_stack
+    [n_modes,256], built faithfully from MODE_PROFILES."""
+    n = len(_MODE_ORDER)
+    max_grade = _np.empty(n, dtype=_np.float64)
+    sfid = _np.empty(n, dtype=_np.int64)
+    base = _np.empty(n, dtype=_np.float64)
+    tfs = _np.full((n, 256), _np.inf, dtype=_np.float64)
+    for mi, name in enumerate(_MODE_ORDER):
+        p = _PROFILES[name]
+        max_grade[mi] = float(_np.tan(_np.radians(p.max_slope_deg)))
+        sfid[mi] = _SFID[p.speed_function]
+        base[mi] = p.base_speed_kmh
+        for tv, fr in p.trail_friction.items():
+            tfs[mi, tv] = _np.inf if fr is None else float(fr)
+    return max_grade, sfid, base, tfs
+
+
+def _empty_trans():
+    z = _np.empty(0, dtype=_np.int64)
+    return z, z.copy(), z.copy(), z.copy(), _np.empty(0, dtype=_np.float64)
+
+
+def test_multimode_foot_only_parity():
+    # foot-only, no transitions: the multimode kernel (1-mode stack) must reproduce
+    # astar_multigoal exactly -- it is a strict superset.
+    n = 8
+    elev, mult, trail, lookup, barr = _flat_inputs(n)
+    foot_mg = float(_np.tan(_np.radians(_PROFILES["foot"].max_slope_deg)))
+    gr = _np.array([n - 1], dtype=_np.int64)
+    gc = _np.array([n - 1], dtype=_np.int64)
+    idx1, path1, cost1 = astar_multigoal(
+        mult, elev, 30.0, 30.0, foot_mg, 0, 6.0, trail, lookup, barr, 2, 0, 0, gr, gc)
+
+    stack = mult.reshape(n, n, 1).copy()
+    tr, tc, tf, tt, tcost = _empty_trans()
+    idx2, path2, cost2 = _mm(
+        stack, elev, 30.0, 30.0,
+        _np.array([foot_mg]), _np.array([0], dtype=_np.int64), _np.array([6.0]),
+        trail, lookup.reshape(1, 256).copy(), barr, 2,
+        0, 0, _np.array([0], dtype=_np.int64), gr, gc, _np.array([0], dtype=_np.int64),
+        tr, tc, tf, tt, tcost)
+
+    assert idx2 == idx1 == 0
+    assert cost2 == pytest.approx(cost1, rel=1e-9, abs=1e-9)
+    assert _np.array_equal(path2[:, :2], path1)   # same (row,col) sequence
+    assert _np.all(path2[:, 2] == 0)              # all foot
+
+
+def test_multimode_parking_switch():
+    # Forest corridor (foot-only) -> parking cell -> open field where vehicle is
+    # fast and foot is slow. The optimizer must switch foot->vehicle at the parking
+    # cell and beat foot-only. The cost advantage is TERRAIN-driven (no trails / no
+    # friction<1), which keeps the §10 heuristic admissible -- a road's <1 friction
+    # would make effective speed exceed base speed and break the heuristic (a known
+    # property of the inherited single-mode kernel too).
+    rows, cols, road_start = 3, 50, 25
+    elev = _np.zeros((rows, cols), dtype=_np.float64)
+    n_modes = 4
+    stack = _np.full((rows, cols, n_modes), _np.inf, dtype=_np.float64)
+    stack[:, :, 0] = 1.0                       # foot: passable everywhere off-trail
+    stack[:, road_start:cols, 3] = 1.0         # vehicle: drivable only in the open field
+    trail = _np.zeros((rows, cols), dtype=_np.uint8)   # no trails anywhere
+    max_grade, sfid, base, tfs = _mode_param_arrays()
+    barr = _np.zeros((rows, cols), dtype=_np.uint8)
+
+    gr = _np.array([1], dtype=_np.int64)
+    gc = _np.array([cols - 1], dtype=_np.int64)
+    pr, pc = 1, road_start         # parking cell, foot<->vehicle, 60 s each way
+    tr = _np.array([pr, pr], dtype=_np.int64)
+    tc = _np.array([pc, pc], dtype=_np.int64)
+    tf = _np.array([0, 3], dtype=_np.int64)
+    tt = _np.array([3, 0], dtype=_np.int64)
+    tcost = _np.array([60.0, 60.0], dtype=_np.float64)
+    om = _np.array([0], dtype=_np.int64)        # start on foot
+    gm = _np.array([3, 0], dtype=_np.int64)     # finish vehicle or foot
+
+    idx, path, cost = _mm(
+        stack, elev, 30.0, 30.0, max_grade, sfid, base, trail, tfs, barr, 1,
+        1, 0, om, gr, gc, gm, tr, tc, tf, tt, tcost)
+
+    assert idx == 0
+    modes = path[:, 2]
+    assert modes[0] == 0 and modes[-1] == 3       # foot start, vehicle finish
+    switches = [k for k in range(1, len(path)) if modes[k] != modes[k - 1]]
+    assert len(switches) == 1                     # exactly one mode change
+    sk = switches[0]
+    assert modes[sk - 1] == 0 and modes[sk] == 3  # foot -> vehicle
+    assert tuple(path[sk, :2]) == (pr, pc)        # at the parking cell
+    assert tuple(path[sk - 1, :2]) == (pr, pc)    # same cell, mode-change edge
+
+    tr0, tc0, tf0, tt0, tcost0 = _empty_trans()
+    _, _, cost_foot_only = _mm(
+        stack, elev, 30.0, 30.0, max_grade, sfid, base, trail, tfs, barr, 1,
+        1, 0, _np.array([0], dtype=_np.int64), gr, gc, _np.array([0], dtype=_np.int64),
+        tr0, tc0, tf0, tt0, tcost0)
+    assert cost < cost_foot_only
+
+
+def test_multimode_no_transitions_independent():
+    # No transitions: the 4-mode search degrades to 4 independent single-mode
+    # searches -- the winning path never changes mode, and its cost equals the
+    # min over the four single-mode runs (vehicle wins on flat passable terrain).
+    # Off-trail only (no friction<1) keeps the heuristic admissible.
+    n = 12
+    elev = _np.zeros((n, n), dtype=_np.float64)
+    n_modes = 4
+    stack = _np.ones((n, n, n_modes), dtype=_np.float64)  # all modes passable off-trail
+    trail = _np.zeros((n, n), dtype=_np.uint8)            # no trails
+    max_grade, sfid, base, tfs = _mode_param_arrays()
+    barr = _np.zeros((n, n), dtype=_np.uint8)
+    gr = _np.array([n - 1], dtype=_np.int64)
+    gc = _np.array([n - 1], dtype=_np.int64)
+    all_modes = _np.array([0, 1, 2, 3], dtype=_np.int64)
+    tr, tc, tf, tt, tcost = _empty_trans()
+
+    idx, path, cost = _mm(
+        stack, elev, 30.0, 30.0, max_grade, sfid, base, trail, tfs, barr, 2,
+        0, 0, all_modes, gr, gc, all_modes, tr, tc, tf, tt, tcost)
+
+    assert _np.all(path[:, 2] == path[0, 2])       # single mode the whole way
+    winning_mode = int(path[0, 2])
+
+    single_costs = []
+    for mi in range(4):
+        _, _, c1 = astar_multigoal(
+            stack[:, :, mi].copy(), elev, 30.0, 30.0,
+            float(max_grade[mi]), int(sfid[mi]), float(base[mi]),
+            trail, tfs[mi].copy(), barr, 2, 0, 0, gr, gc)
+        single_costs.append(c1)
+    assert cost == pytest.approx(min(single_costs), rel=1e-9, abs=1e-9)
+    assert winning_mode == int(_np.argmin(single_costs))  # vehicle (fastest base)
+
+
+def test_multimode_heuristic_admissibility():
+    # §10 admissibility: h(r,c,m) must never exceed the true optimal remaining cost.
+    # vehicle (global-fastest base) is an allowed goal mode, so max_goal_speed is the
+    # global max -> h is a true lower bound. No trails (friction<1 would let a road
+    # beat base speed), so effective speed <= base speed everywhere.
+    rows, cols = 6, 10
+    rng = _np.random.RandomState(0)
+    elev = (rng.rand(rows, cols) * 20.0).astype(_np.float64)
+    n_modes = 4
+    stack = _np.ones((rows, cols, n_modes), dtype=_np.float64)
+    stack[2:4, 3:6, 1] = _np.inf   # a forest block impassable to wheeled modes
+    stack[2:4, 3:6, 2] = _np.inf
+    stack[2:4, 3:6, 3] = _np.inf
+    trail = _np.zeros((rows, cols), dtype=_np.uint8)
+    max_grade, sfid, base, tfs = _mode_param_arrays()
+    barr = _np.zeros((rows, cols), dtype=_np.uint8)
+    gr = _np.array([rows - 1], dtype=_np.int64)
+    gc = _np.array([cols - 1], dtype=_np.int64)
+    gm = _np.array([0, 3], dtype=_np.int64)        # foot or vehicle finish
+    tr = _np.array([0, 0], dtype=_np.int64)        # one foot<->vehicle transition
+    tc = _np.array([5, 5], dtype=_np.int64)
+    tf = _np.array([0, 3], dtype=_np.int64)
+    tt = _np.array([3, 0], dtype=_np.int64)
+    tcost = _np.array([60.0, 60.0], dtype=_np.float64)
+
+    max_goal_speed = max(float(base[g]) for g in gm)
+
+    sampled = 0
+    for r in range(0, rows, 2):
+        for c in range(0, cols, 3):
+            for m in (0, 3):
+                d = float(_np.hypot((r - (rows - 1)) * 30.0, (c - (cols - 1)) * 30.0))
+                h = d * 3.6 / max_goal_speed
+                # Oracle: same kernel with the heuristic disabled (Dijkstra), seeded
+                # only from this state -> exact true remaining cost.
+                _, _, true_cost = _mm(
+                    stack, elev, 30.0, 30.0, max_grade, sfid, base, trail, tfs, barr, 2,
+                    r, c, _np.array([m], dtype=_np.int64), gr, gc, gm,
+                    tr, tc, tf, tt, tcost, True)
+                if not _np.isfinite(true_cost):
+                    continue
+                assert h <= true_cost + 1e-6
+                sampled += 1
+    assert sampled > 0   # the sweep actually exercised reachable states