mirror of
https://github.com/zvx-echo6/navi.git
synced 2026-08-26 17:31:37 +00:00
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>
This commit is contained in:
GitHub
parent
b738227453
commit
47a4047cd7
2 changed files with 498 additions and 0 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue