From 9d684bef43f9038e2766a3776d2e3af3acd3ec82 Mon Sep 17 00:00:00 2001 From: Matt Date: Mon, 25 May 2026 08:13:30 +0000 Subject: [PATCH] feat(offroute): numba A* with anisotropic Tobler, exponentially-inflated cost grid (combined #17+#18) Replace MCP_Geometric in _pathfind_wilderness with a numba-jit anisotropic A* (new astar.py): signed-slope speed (climbing != descending; tobler peaks at -0.05), hard cliff, per-edge avg context multiplier, trail-takes-both via 256-entry lookup, per-edge barriers (strict/pragmatic/emergency), multi-goal A* (first popped wins) with admissible distance/base-speed heuristic. New compute_cost_multiplier_grid (slope-free context multiplier) + exponential inflation (sigma=1.8; inf->HARD=50*p95 for blur, inf re-imposed). numba>=0.59 added (numba 0.65.1). fix: wilderness leg is always foot effort; mode parameter reserved for future flexibility. _pathfind_wilderness keeps the mode param (threaded from _route_A/B/C) but hardcodes cost_mode=foot for the cost grid, trail friction, speed function, base speed, and max grade. Off-trail math for MTB/ATV/vehicle is not well-grounded and real-world wilderness traversal is foot regardless (push the bike, walk past where the vehicle stops). User mode still drives entry-point eligibility (query_radius highway filter) and Valhalla network costing. Matches the original pre-#17 design: wilderness ALWAYS uses foot. Co-Authored-By: Claude Opus 4.7 (1M context) --- backend/pyproject.toml | 1 + backend/services/navi_offroute/astar.py | 249 ++++++++++++++++++ backend/services/navi_offroute/cost.py | 85 ++++++ backend/services/navi_offroute/router.py | 96 ++++--- .../navi_offroute/tests/test_offroute.py | 138 ++++++++++ 5 files changed, 536 insertions(+), 33 deletions(-) create mode 100644 backend/services/navi_offroute/astar.py diff --git a/backend/pyproject.toml b/backend/pyproject.toml index a26b04d..a4dfdac 100644 --- a/backend/pyproject.toml +++ b/backend/pyproject.toml @@ -26,6 +26,7 @@ dependencies = [ "pmtiles>=3", # planet-DEM PMTiles reader # navi-offroute (extraction #8): off-network router + readers. "scikit-image>=0.22", # MCP_Geometric least-cost pathfinding (router.py) + "numba>=0.59", # navi-offroute anisotropic A* JIT pathfinder (astar.py) "rasterio>=1.3", # barriers/wilderness/trails/friction raster readers "psutil>=5.9", # MEMORY_LIMIT_GB enforcement in router.py # scripts/overture_import.py: Overture Places ETL (S3 Parquet -> overture PG). diff --git a/backend/services/navi_offroute/astar.py b/backend/services/navi_offroute/astar.py new file mode 100644 index 0000000..2d77c55 --- /dev/null +++ b/backend/services/navi_offroute/astar.py @@ -0,0 +1,249 @@ +"""Numba-jit anisotropic A* for wilderness pathfinding (replaces MCP_Geometric). + +Single-mode, multi-goal least-time pathfinder over a raster grid. Unlike the old +isotropic MCP, the per-edge time is anisotropic: it depends on the *signed* slope +between the two cells (climbing != descending) via the mode's speed function +(signed Tobler / Herzog / linear), the average per-cell context multiplier of the +two endpoints (or the trail friction when either endpoint is on a trail), and a +barrier/boundary rule. The first goal cell popped from the open set wins, which is +optimal under the admissible heuristic (straight-line distance / base speed). + +Cliffs (|grade| > max_grade) are a hard wall here; smoothing is deferred to #19. +""" +import math + +import numpy as np +from numba import njit +from scipy.ndimage import gaussian_filter + +INF = np.inf + +# Exponential-inflation parameters (see inflate_cost_multiplier). +INFLATE_SIGMA = 1.8 # ~25% contribution at a 3-cell radius +INFLATE_HARD_FACTOR = 50.0 # HARD sentinel = 50 x p95 of finite multipliers +INFLATE_HARD_CAP = 1e12 # cap to avoid float overflow in the blur + + +def inflate_cost_multiplier(mult, sigma=INFLATE_SIGMA): + """Exponentially inflate a context-cost multiplier grid so hard cells bleed a + decaying penalty into their neighbours, giving A* a smooth gradient field to + descend instead of hugging walls. + + Impassable (inf) cells are replaced by a large finite HARD sentinel for the blur + only, then restored to inf at their original positions so true impassability is + preserved exactly. HARD = INFLATE_HARD_FACTOR * p95(finite), capped. + """ + inf_mask = ~np.isfinite(mult) + finite = mult[~inf_mask] + if finite.size == 0: + return np.full(mult.shape, np.inf, dtype=np.float64) + p95 = float(np.percentile(finite, 95)) + hard = min(INFLATE_HARD_FACTOR * p95, INFLATE_HARD_CAP) + work = np.where(inf_mask, hard, mult).astype(np.float64) + blurred = gaussian_filter(work, sigma=sigma, mode="nearest") + blurred[inf_mask] = np.inf + return blurred + + +@njit(cache=True) +def _speed_kmh(signed_grade, speed_function_id, base_speed_kmh, max_grade): + """Mode speed (km/h) for a signed grade. Inlined per speed_function_id: + 0=tobler (signed; peaks at grade=-0.05), 1=herzog wheeled, 2=linear degrade.""" + if speed_function_id == 0: + return 0.6 * base_speed_kmh * math.exp(-3.5 * abs(signed_grade + 0.05)) + elif speed_function_id == 1: + s = signed_grade + sa = abs(s) + denom = (1337.8 * s**6 + 278.19 * s**5 - 517.39 * s**4 + - 78.199 * s**3 + 93.419 * s**2 + 19.825 * sa + 1.64) + if denom < 0.1: + denom = 0.1 + rel = 1.0 / denom + if rel < 0.05: + rel = 0.05 + elif rel > 1.5: + rel = 1.5 + return base_speed_kmh * rel + else: + v = base_speed_kmh * (1.0 - abs(signed_grade) / max_grade) + if v < 0.0: + v = 0.0 + return v + + +@njit(cache=True) +def astar_multigoal( + cost_mult, # 2D float64: per-cell context multiplier, post-inflation (inf=impassable) + elevation, # 2D float64: metres (NaN = impassable) + cell_size_lat_m, # float + cell_size_lon_m, # float + max_grade, # float: tan(max_slope) + speed_function_id, # int: 0=tobler 1=herzog 2=linear + base_speed_kmh, # float + trail_grid, # 2D uint8: 0=none else trail value (5/15/25) + trail_friction_lookup, # 1D float64 len 256: friction by trail value (inf=impassable) + barrier_grid, # 2D uint8: 255=barrier + boundary_mode_id, # int: 0=strict 1=pragmatic 2=emergency + origin_row, origin_col, + goal_rows, goal_cols, # 1D int arrays +): + """A* from (origin_row,origin_col) to the nearest (by time) of the goals. + Returns (best_goal_idx, path) where path is an int64 (N,2) array of (row,col) + from origin to goal. (-1, empty) if no goal is reachable.""" + rows, cols = elevation.shape + + 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 + + g_score = np.full((rows, cols), INF, dtype=np.float64) + parent = np.full((rows, cols), -1, dtype=np.int64) + closed = np.zeros((rows, cols), dtype=np.bool_) + + # Binary min-heap (lazy deletion): parallel id/priority arrays. + cap = rows * cols * 4 + 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): + 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 / base_speed_kmh # metres -> seconds at base speed + + # Seed origin. + g_score[origin_row, origin_col] = 0.0 + heap_id[0] = origin_row * cols + origin_col + heap_f[0] = 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 + r = 2 * i + 2 + sm = i + if l < hsize and heap_f[l] < heap_f[sm]: + sm = l + if r < hsize and heap_f[r] < heap_f[sm]: + sm = r + 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 + + cr = cur_id // cols + cc = cur_id % cols + if closed[cr, cc]: + continue # stale heap entry + closed[cr, cc] = True + + if goal_index[cr, cc] >= 0: + # First goal popped is optimal — trace back. + length = 1 + node = cur_id + while parent[node // cols, node % cols] != -1: + length += 1 + node = parent[node // cols, node % cols] + path = np.empty((length, 2), dtype=np.int64) + node = cur_id + k = length - 1 + while node != -1: + pr = node // cols + pc = node % cols + path[k, 0] = pr + path[k, 1] = pc + k -= 1 + node = parent[pr, pc] + return goal_index[cr, cc], path, g_score[cr, cc] + + g_cur = g_score[cr, cc] + elev_cur = elevation[cr, cc] + if math.isnan(elev_cur): + continue + + 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]: + continue + elev_n = elevation[nr, nc] + if math.isnan(elev_n): + continue + + 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 + if abs(signed_grade) > max_grade: + continue # hard cliff + spd = _speed_kmh(signed_grade, speed_function_id, base_speed_kmh, max_grade) + if spd <= 1e-9: + continue + base_time = dist * 3.6 / spd + + 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): + continue # 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)): + continue # 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: + continue + 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 + + tentative = g_cur + edge + if tentative < g_score[nr, nc]: + g_score[nr, nc] = tentative + parent[nr, nc] = cur_id + f = tentative + heuristic(nr, nc) + if hsize < cap: + # push + sift-up + heap_id[hsize] = 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 + + return -1, np.empty((0, 2), dtype=np.int64), INF diff --git a/backend/services/navi_offroute/cost.py b/backend/services/navi_offroute/cost.py index 16b8514..c8dd343 100755 --- a/backend/services/navi_offroute/cost.py +++ b/backend/services/navi_offroute/cost.py @@ -203,6 +203,91 @@ PRAGMATIC_BARRIER_MULTIPLIER = 5.0 # COST GRID COMPUTATION # ═══════════════════════════════════════════════════════════════════════════════ +def _per_cell_grade(elevation, cell_size_lat_m, cell_size_lon_m): + """Per-cell slope magnitude (rise/run), matching compute_cost_grid's gradient. + Used only to classify the vehicle off-trail flat-field special case.""" + grade = np.zeros(elevation.shape, dtype=np.float32) + dy = np.zeros(elevation.shape, dtype=np.float32) + dy[1:-1, :] = ((elevation[:-2, :] - elevation[2:, :]) / (2 * cell_size_lat_m)) ** 2 + dy[0, :] = ((elevation[0, :] - elevation[1, :]) / cell_size_lat_m) ** 2 + dy[-1, :] = ((elevation[-2, :] - elevation[-1, :]) / cell_size_lat_m) ** 2 + dy[:, 1:-1] += ((elevation[:, 2:] - elevation[:, :-2]) / (2 * cell_size_lon_m)) ** 2 + dy[:, 0] += ((elevation[:, 1] - elevation[:, 0]) / cell_size_lon_m) ** 2 + dy[:, -1] += ((elevation[:, -1] - elevation[:, -2]) / cell_size_lon_m) ** 2 + np.sqrt(dy, out=grade) + return grade + + +def compute_cost_multiplier_grid( + elevation: np.ndarray, + cell_size_lat_m: float, + cell_size_lon_m: float, + friction: Optional[np.ndarray] = None, + friction_raw: Optional[np.ndarray] = None, + wilderness: Optional[np.ndarray] = None, + mode: Literal["foot", "mtb", "atv", "vehicle"] = "foot", +) -> np.ndarray: + """Per-cell SLOPE-FREE context cost multiplier for the anisotropic A* pathfinder. + + Returns a float64 grid: 1.0 baseline, higher = harder, np.inf = impassable. It + combines the base WorldCover friction, the mode's terrain-friction overrides, the + vehicle off-trail flat-field special case, and wilderness impassability. Slope, + trails, and barriers are handled PER-EDGE in the A* kernel and are NOT applied here. + + This is a strict refactor of compute_cost_grid's friction-and-mode pieces. + (elevation + cell sizes are needed only for the vehicle flat-field slope check — + that classification is per-cell, distinct from the per-edge traversal slope.) + """ + if mode not in MODE_PROFILES: + raise ValueError(f"mode must be one of {list(MODE_PROFILES.keys())}") + profile = MODE_PROFILES[mode] + + mult = np.ones(elevation.shape, dtype=np.float64) + + # Base WorldCover friction. + if friction is not None: + if friction.shape != elevation.shape: + raise ValueError("Friction shape mismatch") + np.multiply(mult, friction, out=mult) + + # NaN elevation -> impassable. + mult[np.isnan(elevation)] = np.inf + + # Mode-specific terrain friction overrides. + if friction_raw is not None and profile.terrain_friction_override: + if friction_raw.shape != elevation.shape: + raise ValueError("Friction_raw shape mismatch") + for wc_class, override in profile.terrain_friction_override.items(): + if override is None: + continue + if override == np.inf: + np.putmask(mult, friction_raw == wc_class, np.inf) + else: + m = friction_raw == wc_class + mult[m] *= override + del m + + # Vehicle off-trail flat-field special case (overrides the inf set above on flat + # grassland/cropland). Uses a per-cell slope classification. + if mode == "vehicle" and profile.off_trail_flat_threshold_deg > 0 and friction_raw is not None: + grade = _per_cell_grade(elevation, cell_size_lat_m, cell_size_lon_m) + slope_deg = np.degrees(np.arctan(grade)) + flat_field = ( + (slope_deg <= profile.off_trail_flat_threshold_deg) + & ((friction_raw == 30) | (friction_raw == 40)) + ) + mult[flat_field] = profile.off_trail_flat_friction + del grade, slope_deg, flat_field + + # Wilderness areas (mode-specific). + if wilderness is not None and profile.wilderness_impassable: + if wilderness.shape != elevation.shape: + raise ValueError("Wilderness shape mismatch") + np.putmask(mult, wilderness == 255, np.inf) + + return mult + + def compute_cost_grid( elevation: np.ndarray, cell_size_m: float, diff --git a/backend/services/navi_offroute/router.py b/backend/services/navi_offroute/router.py index 1d8fcc5..045eaee 100755 --- a/backend/services/navi_offroute/router.py +++ b/backend/services/navi_offroute/router.py @@ -31,10 +31,10 @@ import requests import psycopg2 import psycopg2.extras from shapely.geometry import LineString -from skimage.graph import MCP_Geometric +from .astar import astar_multigoal, inflate_cost_multiplier from shared.dem import DEMReader, dem_path -from .cost import compute_cost_grid +from .cost import compute_cost_grid, compute_cost_multiplier_grid, MODE_PROFILES from .friction import FrictionReader, friction_to_multiplier from .barriers import BarrierReader, WildernessReader, wilderness_tif_path from .trails import TrailReader @@ -956,7 +956,7 @@ class OffrouteRouter: # Run wilderness pathfinding wilderness_result = self._pathfind_wilderness( start_lat, start_lon, end_lat, end_lon, - entry_points, boundary_mode, "start" + entry_points, boundary_mode, "start", mode=mode ) if wilderness_result.get("status") == "error": @@ -1036,7 +1036,7 @@ class OffrouteRouter: # Run wilderness pathfinding FROM END toward entry points wilderness_result = self._pathfind_wilderness( end_lat, end_lon, start_lat, start_lon, - entry_points, boundary_mode, "end" + entry_points, boundary_mode, "end", mode=mode ) if wilderness_result.get("status") == "error": @@ -1120,7 +1120,7 @@ class OffrouteRouter: # Phase 1: Wilderness pathfinding from START wilderness_start_result = self._pathfind_wilderness( start_lat, start_lon, end_lat, end_lon, - entry_points_start, boundary_mode, "start" + entry_points_start, boundary_mode, "start", mode=mode ) if wilderness_start_result.get("status") == "error": @@ -1134,7 +1134,7 @@ class OffrouteRouter: # Phase 2: Wilderness pathfinding from END (run after freeing phase 1 memory) wilderness_end_result = self._pathfind_wilderness( end_lat, end_lon, start_lat, start_lon, - entry_points_end, boundary_mode, "end" + entry_points_end, boundary_mode, "end", mode=mode ) if wilderness_end_result.get("status") == "error": @@ -1177,7 +1177,8 @@ class OffrouteRouter: dest_lat: float, dest_lon: float, entry_points: List[Dict], boundary_mode: str, - label: str + label: str, + mode: str = "foot", ) -> Dict: """ Run MCP wilderness pathfinding from origin toward entry points. @@ -1258,24 +1259,52 @@ class OffrouteRouter: target_shape=elevation.shape ) - # Compute cost grid (ALWAYS foot mode for wilderness) - cost = compute_cost_grid( + # ── Anisotropic A* pathfinding (replaces isotropic MCP) ── + # Wilderness pathfinding ALWAYS uses foot effort, regardless of the user's mode. + # The off-trail cost math for MTB/ATV/vehicle is not well-grounded (no peer-reviewed + # off-road model), and real-world wilderness traversal is foot anyway: you push the + # bike and walk past where the vehicle stops. The user's mode still affects entry-point + # eligibility (the highway filter at the query_radius call sites) and the Valhalla + # network-leg costing -- just not this wilderness leg. + _ = mode # reserved for future flexibility; unused for wilderness cost + cost_mode = "foot" + profile = MODE_PROFILES[cost_mode] + cell_size_m = meta["cell_size_m"] + + # Wilderness grid only matters for modes that treat it as impassable. + wilderness = None + if profile.wilderness_impassable and self.wilderness_reader is not None: + wilderness = self.wilderness_reader.get_wilderness_grid( + south=bbox["south"], north=bbox["north"], + west=bbox["west"], east=bbox["east"], + target_shape=elevation.shape, + ) + + # Per-cell context multiplier (slope-free; trails/barriers are per-edge in A*), + # then exponentially inflated so hard cells bleed a decaying penalty outward. + cost_mult = compute_cost_multiplier_grid( elevation, - cell_size_m=meta["cell_size_m"], + cell_size_lat_m=cell_size_m, + cell_size_lon_m=cell_size_m, friction=friction_mult, friction_raw=friction_raw, - trails=trails, - barriers=barriers, - wilderness=None, - mvum=None, - boundary_mode=boundary_mode, - mode="foot", + wilderness=wilderness, + mode=cost_mode, ) + cost_mult = inflate_cost_multiplier(cost_mult) # Free intermediate arrays del friction_mult, friction_raw gc.collect() + # Trail friction lookup (length-256, indexed by trail value; inf = impassable). + trail_friction_lookup = np.full(256, np.inf, dtype=np.float64) + for tv, fric in profile.trail_friction.items(): + trail_friction_lookup[tv] = np.inf if fric is None else float(fric) + + speed_function_id = {"tobler": 0, "herzog": 1, "linear": 2}.get(profile.speed_function, 0) + max_grade = float(np.tan(np.radians(profile.max_slope_deg))) + # Convert origin to pixel coordinates origin_row, origin_col = self.dem_reader.latlon_to_pixel(origin_lat, origin_lon, meta) @@ -1283,7 +1312,7 @@ class OffrouteRouter: if not (0 <= origin_row < rows and 0 <= origin_col < cols): return {"status": "error", "message": f"{label.capitalize()} point outside grid bounds"} - # Map entry points to pixels + # Map entry points to pixels (these are the A* goals). entry_pixels = [] for ep in entry_points: row, col = self.dem_reader.latlon_to_pixel(ep["lat"], ep["lon"], meta) @@ -1293,28 +1322,29 @@ class OffrouteRouter: if not entry_pixels: return {"status": "error", "message": f"No entry points map to grid bounds for {label}"} - # Run MCP - mcp = MCP_Geometric(cost, fully_connected=True) - cumulative_costs, traceback = mcp.find_costs([(origin_row, origin_col)]) + goal_rows = np.array([ep["row"] for ep in entry_pixels], dtype=np.int64) + goal_cols = np.array([ep["col"] for ep in entry_pixels], dtype=np.int64) + boundary_mode_id = {"strict": 0, "pragmatic": 1, "emergency": 2}.get(boundary_mode, 1) - # Find nearest reachable entry point - best_entry = None - best_cost = np.inf + # Run multi-goal A* (first goal popped wins). + elevation = np.ascontiguousarray(elevation, dtype=np.float64) + best_goal_idx, path_indices, best_cost = astar_multigoal( + cost_mult, elevation, + float(cell_size_m), float(cell_size_m), + max_grade, speed_function_id, float(profile.base_speed_kmh), + np.ascontiguousarray(trails, dtype=np.uint8), trail_friction_lookup, + np.ascontiguousarray(barriers, dtype=np.uint8), boundary_mode_id, + int(origin_row), int(origin_col), + goal_rows, goal_cols, + ) - for ep in entry_pixels: - ep_cost = cumulative_costs[ep["row"], ep["col"]] - if ep_cost < best_cost: - best_cost = ep_cost - best_entry = ep - - if best_entry is None or np.isinf(best_cost): + if best_goal_idx < 0 or len(path_indices) == 0 or np.isinf(best_cost): return { "status": "error", "message": f"No path found from {label} to any entry point (blocked by impassable terrain)" } - # Traceback path - path_indices = mcp.traceback((best_entry["row"], best_entry["col"])) + best_entry = entry_pixels[best_goal_idx] # Convert to coordinates and collect stats coords = [] @@ -1350,7 +1380,7 @@ class OffrouteRouter: on_trail_pct = float(100 * on_trail_cells / total_cells) if total_cells > 0 else 0 # Free memory - del mcp, cumulative_costs, traceback, cost, trails, barriers, elevation + del cost_mult, trails, barriers, elevation gc.collect() return { diff --git a/backend/services/navi_offroute/tests/test_offroute.py b/backend/services/navi_offroute/tests/test_offroute.py index 76e1730..18e242e 100644 --- a/backend/services/navi_offroute/tests/test_offroute.py +++ b/backend/services/navi_offroute/tests/test_offroute.py @@ -559,3 +559,141 @@ def test_query_radius_soft_cap_filters_beyond_radius(monkeypatch): idx = object.__new__(EntryPointIndex) out = idx.query_radius(44.1, -115.0, 50, limit=10) # 50 km = 50000 m cap assert [r["id"] for r in out] == [1, 2] + + +# ── Anisotropic A* pathfinder (#17+#18) ─────────────────────────────────── +import numpy as _np +import services.navi_offroute.router as _router_mod +from services.navi_offroute.astar import ( + _speed_kmh, astar_multigoal, inflate_cost_multiplier, +) +from services.navi_offroute.cost import compute_cost_multiplier_grid + +_MG = float(_np.tan(_np.radians(40.0))) + + +def test_signed_tobler_asymmetry(): + # Same magnitude, opposite sign -> downhill faster than uphill; peak near -0.05. + up = _speed_kmh(0.2, 0, 6.0, _MG) + down = _speed_kmh(-0.2, 0, 6.0, _MG) + assert down > up + peak = _speed_kmh(-0.05, 0, 6.0, _MG) + assert peak >= _speed_kmh(0.0, 0, 6.0, _MG) + assert peak >= _speed_kmh(-0.15, 0, 6.0, _MG) + + +def test_inflation_bumps_neighbors_and_preserves_inf(): + grid = _np.ones((30, 30), dtype=_np.float64) + grid[15, 15] = 100.0 # high finite cost + out = inflate_cost_multiplier(grid) + assert out[15, 16] > 1.0 # neighbor inflated + assert out[0, 0] < 1.05 # far corner ~baseline + + grid2 = _np.ones((30, 30), dtype=_np.float64) + grid2[15, 15] = _np.inf # impassable + out2 = inflate_cost_multiplier(grid2) + assert _np.isinf(out2[15, 15]) # inf preserved exactly + assert _np.isfinite(out2[15, 16]) and out2[15, 16] > 1.0 # neighbor bumped, not inf + + +def _flat_inputs(n): + elev = _np.zeros((n, n), dtype=_np.float64) + mult = _np.ones((n, n), dtype=_np.float64) + trail = _np.zeros((n, n), dtype=_np.uint8) + lookup = _np.full(256, _np.inf, dtype=_np.float64) + barr = _np.zeros((n, n), dtype=_np.uint8) + return elev, mult, trail, lookup, barr + + +def test_astar_small_synthetic_shortest_path(): + elev, mult, trail, lookup, barr = _flat_inputs(30) + gr = _np.array([29], dtype=_np.int64) + gc = _np.array([29], dtype=_np.int64) + idx, path, cost = astar_multigoal( + mult, elev, 30.0, 30.0, _MG, 0, 6.0, trail, lookup, barr, 2, 0, 0, gr, gc) + assert idx == 0 + assert tuple(path[0]) == (0, 0) + assert tuple(path[-1]) == (29, 29) + assert len(path) == 30 # pure diagonal on a flat grid + assert cost > 0 and _np.isfinite(cost) + + +def test_astar_multigoal_picks_cheaper(): + elev, mult, trail, lookup, barr = _flat_inputs(30) + gr = _np.array([0, 20], dtype=_np.int64) # goal0 at (0,5) near; goal1 at (20,20) far + gc = _np.array([5, 20], dtype=_np.int64) + idx, path, cost = astar_multigoal( + mult, elev, 30.0, 30.0, _MG, 0, 6.0, trail, lookup, barr, 2, 0, 0, gr, gc) + assert idx == 0 + assert tuple(path[-1]) == (0, 5) + + +def test_compute_cost_multiplier_grid_math(): + elev = _np.zeros((4, 4), dtype=_np.float64) + friction = _np.full((4, 4), 2.0, dtype=_np.float64) + # mtb override: grass(30)=2.0, water(80)=inf + fr = _np.full((4, 4), 30, dtype=_np.uint8) + fr[0, 0] = 80 + mult = compute_cost_multiplier_grid( + elev, 30.0, 30.0, friction=friction, friction_raw=fr, wilderness=None, mode="mtb") + assert mult[1, 1] == 4.0 # 2.0 friction * 2.0 grass override + assert _np.isinf(mult[0, 0]) # water impassable + + +# ── _pathfind_wilderness mode wiring (mtb profile -> herzog + mtb trail set) ── + +class _FakeDEM: + def get_elevation_grid(self, south, north, west, east): + return _np.zeros((10, 10), dtype=_np.float64), {"cell_size_m": 30.0} + def latlon_to_pixel(self, lat, lon, meta): + return (0, 0) if lat == 44.0 else (9, 9) + def pixel_to_latlon(self, row, col, meta): + return (44.0 + row * 0.001, -115.0 + col * 0.001) + + +class _FakeGrid: + def __init__(self, val, dtype): + self.val, self.dtype = val, dtype + def _grid(self, **k): + return _np.full((10, 10), self.val, dtype=self.dtype) + + +def test_pathfind_wilderness_always_uses_foot_effort(monkeypatch): + # Even when called with mode="mtb", the wilderness cost is computed as foot: + # compute_cost_multiplier_grid receives mode="foot", and A* gets the foot speed + # function (tobler=0), foot base speed (6.0), and foot trail friction. + captured = {} + + def fake_mult(elevation, cell_size_lat_m, cell_size_lon_m, + friction=None, friction_raw=None, wilderness=None, mode="foot"): + captured["mult_mode"] = mode + return _np.ones((10, 10), dtype=_np.float64) + + def fake_astar(cost_mult, elevation, clat, clon, max_grade, sfid, base, trails, + lookup, barriers, bmid, orow, ocol, grows, gcols): + captured["speed_function_id"] = sfid + captured["base_speed"] = base + captured["lookup"] = lookup + return 0, _np.array([[0, 0], [9, 9]], dtype=_np.int64), 6.0 + + monkeypatch.setattr(_router_mod, "compute_cost_multiplier_grid", fake_mult) + monkeypatch.setattr(_router_mod, "astar_multigoal", fake_astar) + monkeypatch.setattr(OffrouteRouter, "_init_readers", lambda self: None) + + r = object.__new__(OffrouteRouter) + r.dem_reader = _FakeDEM() + r.friction_reader = type("F", (), {"get_friction_grid": lambda self, **k: _np.full((10, 10), 30, dtype=_np.uint8)})() + r.barrier_reader = type("B", (), {"get_barrier_grid": lambda self, **k: _np.zeros((10, 10), dtype=_np.uint8)})() + r.trail_reader = type("T", (), {"get_trails_grid": lambda self, **k: _np.zeros((10, 10), dtype=_np.uint8)})() + r.wilderness_reader = None # foot is not wilderness_impassable -> not loaded anyway + + ep = [{"lat": 44.001, "lon": -115.001, "highway_class": "track", "name": "t", "land_status": "open"}] + out = r._pathfind_wilderness(44.0, -115.0, 44.001, -115.001, ep, "pragmatic", "start", mode="mtb") + + assert out["status"] == "ok" + assert captured["mult_mode"] == "foot" # cost grid built as foot despite mode=mtb + assert captured["speed_function_id"] == 0 # tobler (foot) + assert captured["base_speed"] == 6.0 # foot base speed + assert captured["lookup"][5] == 0.1 # foot road + assert captured["lookup"][15] == 0.3 # foot track + assert captured["lookup"][25] == 0.5 # foot foot-trail