Merge pull request #18 from zvx-echo6/feat/smooth-max-grade

fix(offroute): smooth max_grade penalty instead of hard cliff (DEM noise robustness)

backend/services/navi_offroute/astar.py

@@ -8,7 +8,9 @@ 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.
+Cliffs (|grade| > max_grade) incur a smooth exponential penalty rather than a hard
+wall, so a single noisy DEM cell can't fabricate an impassable edge; only truly
+absurd grades (penalty > SLOPE_PENALTY_CAP) are dropped.
 """
 import math
 
@@ -23,6 +25,13 @@ 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
 
+# Smooth slope penalty (replaces the old hard max_grade cliff). No penalty up to
+# max_grade; past it the edge cost is multiplied by exp(overshoot * SCALE), so a single
+# noisy DEM cell can't make an edge unconditionally impassable. Only truly absurd grades
+# (penalty above the cap) are dropped.
+SLOPE_PENALTY_SCALE = 10.0    # cost multiplier doubles for ~7% overshoot past max_grade
+SLOPE_PENALTY_CAP = 1e6       # beyond this, treat as impassable (true bad data)
+
 
 def inflate_cost_multiplier(mult, sigma=INFLATE_SIGMA):
     """Exponentially inflate a context-cost multiplier grid so hard cells bleed a
@@ -194,12 +203,19 @@ def astar_multigoal(
                 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:
+                # Smooth slope penalty: free up to max_grade, then an exponential ramp on
-                    continue  # hard cliff
+                # the overshoot; only an absurd grade (penalty > cap) is impassable.
+                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:
+                        continue  # absurd grade (DEM noise or true vertical) -- give up
                 spd = _speed_kmh(signed_grade, speed_function_id, base_speed_kmh, max_grade)
                 if spd <= 1e-9:
                     continue
                 base_time = dist * 3.6 / spd
+                base_time *= slope_penalty
 
                 tv_cur = trail_grid[cr, cc]
                 tv_n = trail_grid[nr, nc]

backend/services/navi_offroute/tests/test_offroute.py

@@ -697,3 +697,71 @@ def test_pathfind_wilderness_always_uses_foot_effort(monkeypatch):
     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
+
+
+# ── Smooth max_grade penalty (#19) ────────────────────────────────────────
+from services.navi_offroute.astar import SLOPE_PENALTY_CAP as _CAP  # noqa: F401
+
+
+def test_smooth_max_grade_penalty():
+    # 5x5 wall across row 2 with one crossing gap at (2,0) (the cliff cell); diagonal
+    # dodges of (2,0) are blocked so the gap can only be crossed via the penalised
+    # vertical edges. A second gap at (2,4) is opened only for the "routes around" case.
+    mg = float(_np.tan(_np.radians(40.0)))
+
+    def run(bump, second_gap):
+        elev = _np.zeros((5, 5), dtype=_np.float64)
+        elev[2, 0] = bump
+        mult = _np.ones((5, 5), dtype=_np.float64)
+        for c in (1, 2, 3):
+            mult[2, c] = _np.inf
+        if not second_gap:
+            mult[2, 4] = _np.inf          # close the detour: (2,0) is the only crossing
+        mult[1, 1] = _np.inf              # block diagonal dodge into (2,0)
+        mult[3, 1] = _np.inf              # block diagonal dodge out of (2,0)
+        trail = _np.zeros((5, 5), dtype=_np.uint8)
+        lookup = _np.full(256, _np.inf, dtype=_np.float64)
+        barr = _np.zeros((5, 5), dtype=_np.uint8)
+        gr = _np.array([4], dtype=_np.int64)
+        gc = _np.array([0], dtype=_np.int64)
+        return astar_multigoal(mult, elev, 30.0, 30.0, mg, 0, 6.0,
+                               trail, lookup, barr, 2, 0, 0, gr, gc)
+
+    def cells(path):
+        return {tuple(int(x) for x in p) for p in path}
+
+    # Flat control (only gap): crosses (2,0).
+    idx0, p0, c0 = run(0.0, second_gap=False)
+    assert idx0 == 0 and (2, 0) in cells(p0)
+
+    # Moderate cliff (grade ~0.87 > max 0.84), no alternative: the smooth penalty lets A*
+    # TRAVERSE it (a hard cliff would have returned no path) at a finite, raised cost.
+    idxm, pm, cm = run(26.0, second_gap=False)
+    assert idxm == 0 and (2, 0) in cells(pm)
+    assert _np.isfinite(cm) and cm > c0
+
+    # Absurd grade (~10) past the cap is still truly impassable: no alternative -> no path.
+    idxa, pa, ca = run(300.0, second_gap=False)
+    assert idxa == -1
+
+    # ...but when an alternative exists, A* routes AROUND the impassable cliff cell.
+    idxr, pr, cr = run(300.0, second_gap=True)
+    assert idxr == 0 and (2, 0) not in cells(pr) and (2, 4) in cells(pr)
+
+
+def test_signed_grade_at_max_threshold():
+    # A cell at EXACTLY max_grade incurs no penalty (the ramp is on the overshoot).
+    mg = 0.5
+    elev = _np.zeros((2, 1), dtype=_np.float64)
+    elev[1, 0] = mg * 30.0  # rise/run = 15/30 = 0.5 == mg exactly
+    mult = _np.ones((2, 1), dtype=_np.float64)
+    trail = _np.zeros((2, 1), dtype=_np.uint8)
+    lookup = _np.full(256, _np.inf, dtype=_np.float64)
+    barr = _np.zeros((2, 1), dtype=_np.uint8)
+    gr = _np.array([1], dtype=_np.int64)
+    gc = _np.array([0], 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)
+    expected = 30.0 * 3.6 / _speed_kmh(mg, 0, 6.0, mg)  # penalty = 1.0 at threshold
+    assert idx == 0
+    assert abs(cost - expected) < 1e-6