Files
Aislo/B05_Profile/B05_Profile_Engine_RidgeValley.py
eomsangdon acdd2cef4e refactor(B05): 능선-계곡 길찾기 그래프 뼈대 분리 (787→552줄)
- `B05_Profile_Engine_RidgeValley_Graph.py`(257줄) 신설 — 격자 조회(`_Grid`),
  노드 수집, 구획 마스크, 정속경사 세그먼트 판정, 엣지·연결 후보, 교각 계산.
- 원본에는 탐색(`_search_segment`)·fillet 선형·진입점 계산만 남김.
- 엣지 후보 상수(MAX_EDGE_LEN_M 등)도 함께 이동 — 쓰는 쪽이 전부 새 모듈.
- 외부 호출부 불변(`solve_ridge_valley_route` 경로 동일), pytest 359 passed.
2026-09-02 16:15:25 +09:00

553 lines
20 KiB
Python

"""B05 능선-계곡 정속경사 임도 길찾기 (대안 알고리즘).
격자 Dijkstra와 분리된 방식: 지형 스켈레톤에서 주/지 능선·계곡 polyline을
얻고, 능선↔계곡 노드 쌍을 잇는 정속경사 직선 세그먼트로 그래프를 만든 뒤
교각 페널티 Dijkstra로 노드 시퀀스를 탐색한다. 최종 선형은 직선+최소회전반경
원호(fillet)로 구성한다. 반환 스키마는 solve_optimal_route()와 동일하다.
"""
import heapq
import math
from pathlib import Path
from typing import Any
import numpy as np
from B05_Profile.B05_Profile_Engine_Geometry import (
circle_intrusions,
circumradius_2d,
point_to_polyline_dist_2d,
resample_polyline_2d,
)
from B05_Profile.B05_Profile_Engine_RidgeValley_Graph import (
_build_barrier_mask,
_build_edges,
_collect_nodes,
_endpoint_connectors,
_Grid,
_segment_feasible,
_turn_angle,
)
from B05_Profile.B05_Profile_Engine_Skeleton import load_or_build_skeleton
from B05_Profile.B05_Profile_Engine_Solver import (
_MODELS_SUBDIR,
_load_or_build_cost_surface,
)
from config.config_system import (
FOREST_ROAD_MIN_CURVE_R_M,
ROUTE_ALT_GRADE_TOLERANCE,
ROUTE_ALT_MAX_GRADE,
ROUTE_ALT_MIN_GRADE,
ROUTE_DEFAULT_GRADE_CLASS,
ROUTE_REQUIRED_POINT_TOLERANCE_M,
SKELETON_NODE_SPACING_M,
)
# 교각 페널티 (알고리즘 내부 상수) — 엣지 후보 상수는 _Graph 로 옮겼다(2026-09-02).
TURN_PENALTY_W = 60.0
MAX_TURN_DEG = 120.0
def _search_segment(
start_pt: dict[str, float],
end_pt: dict[str, float],
pos: np.ndarray,
adj: dict[int, list[tuple[int, float]]],
grid: _Grid,
barrier: np.ndarray,
blocked_circles: list[dict[str, float]],
max_uphill_grade: float,
max_downhill_grade: float,
tol: float,
endpoint_free_m: float,
min_radius: float,
) -> list[list[float]] | None:
"""start→end를 그래프 위에서 탐색해 노드 좌표 시퀀스를 반환 (실패 시 None)."""
start_xyz = [start_pt["x"], start_pt["y"], grid.z_at(start_pt["x"], start_pt["y"])]
end_xyz = [end_pt["x"], end_pt["y"], grid.z_at(end_pt["x"], end_pt["y"])]
direct_limit = max_uphill_grade if end_xyz[2] > start_xyz[2] else max_downhill_grade
if _segment_feasible(
np.asarray(start_xyz),
np.asarray(end_xyz),
grid,
barrier,
blocked_circles,
0.0,
direct_limit,
tol,
endpoint_free_m,
enforce_grade_window=False,
):
return [start_xyz, end_xyz]
start_conn = _endpoint_connectors(
start_pt,
pos,
grid,
barrier,
blocked_circles,
max_uphill_grade,
max_downhill_grade,
tol,
endpoint_free_m,
)
end_conn = _endpoint_connectors(
end_pt,
pos,
grid,
barrier,
blocked_circles,
max_uphill_grade,
max_downhill_grade,
tol,
endpoint_free_m,
)
if not start_conn or not end_conn:
return None
end_conn_map = {j: length for j, length in end_conn}
START, MAX_TURN = -1, math.radians(MAX_TURN_DEG)
def _xy(i):
return start_xyz if i == START else pos[i]
def _seg_len(i, j):
a, b = _xy(i), _xy(j)
return math.hypot(a[0] - b[0], a[1] - b[1])
def _fillet_ok(theta, len_in, len_out):
if theta < 1e-6:
return True
t_len = min_radius * math.tan(theta / 2.0)
return t_len <= len_in / 2.0 and t_len <= len_out / 2.0
dist: dict[tuple[int, int], float] = {}
parent: dict[tuple[int, int], tuple[int, int]] = {}
pq: list[tuple[float, int, int]] = []
for j, length in start_conn:
dist[(START, j)] = length
heapq.heappush(pq, (length, START, j))
best_state, best_cost = None, float("inf")
while pq:
d, u, v = heapq.heappop(pq)
if d > dist.get((u, v), float("inf")):
continue
if v in end_conn_map:
theta = _turn_angle(_xy(u), _xy(v), end_xyz)
if theta <= MAX_TURN and _fillet_ok(theta, _seg_len(u, v), end_conn_map[v]):
total = d + end_conn_map[v] + TURN_PENALTY_W * theta
if total < best_cost:
best_cost, best_state = total, (u, v)
for w, length in adj.get(v, []):
if w == u:
continue
theta = _turn_angle(_xy(u), _xy(v), pos[w])
if theta > MAX_TURN:
continue
if not _fillet_ok(theta, _seg_len(u, v), length):
continue
nd = d + length + TURN_PENALTY_W * theta
if nd < dist.get((v, w), float("inf")):
dist[(v, w)] = nd
parent[(v, w)] = (u, v)
heapq.heappush(pq, (nd, v, w))
if best_state is None:
return None
seq = [end_xyz]
state = best_state
while state is not None:
u, v = state
seq.append([float(pos[v][0]), float(pos[v][1]), float(pos[v][2])])
state = parent.get(state)
if state is None and u == START:
break
seq.append(start_xyz)
return seq[::-1]
def _fillet_alignment(
nodes: list[list[float]], radius: float, step_m: float
) -> tuple[list[list[float]], list[float]]:
"""노드 시퀀스를 직선 + 최소회전반경 원호(fillet) 선형으로 변환한다."""
n = len(nodes)
if n < 2:
return [list(p) for p in nodes], [float("inf")] * n
seg_len, seg_grade = [], []
for i in range(n - 1):
length = math.hypot(nodes[i + 1][0] - nodes[i][0], nodes[i + 1][1] - nodes[i][1])
seg_len.append(max(length, 1e-9))
seg_grade.append((nodes[i + 1][2] - nodes[i][2]) / max(length, 1e-9))
t_len = [0.0] * n
theta = [0.0] * n
for i in range(1, n - 1):
th = _turn_angle(nodes[i - 1], nodes[i], nodes[i + 1])
theta[i] = th
if th < 1e-6:
continue
t = radius * math.tan(th / 2.0)
t_len[i] = min(t, seg_len[i - 1] / 2.0, seg_len[i] / 2.0)
poly: list[list[float]] = []
radii: list[float] = []
def _append(pt, rad):
poly.append([float(pt[0]), float(pt[1]), float(pt[2])])
radii.append(rad)
_append(nodes[0], float("inf"))
for i in range(n - 1):
ax, ay, az = nodes[i]
bx, by, bz = nodes[i + 1]
length, g = seg_len[i], seg_grade[i]
ux, uy = (bx - ax) / length, (by - ay) / length
s0, s1 = t_len[i], length - t_len[i + 1]
n_pts = max(int((s1 - s0) / step_m), 1)
for k in range(n_pts + 1):
s = s0 + (s1 - s0) * (k / n_pts)
_append((ax + ux * s, ay + uy * s, az + g * s), float("inf"))
if i < n - 2 and t_len[i + 1] > 1e-9 and theta[i + 1] > 1e-6:
th = theta[i + 1]
t = t_len[i + 1]
eff_r = t / math.tan(th / 2.0)
cx0, cy0 = bx - ux * t, by - uy * t
nx_, ny_, _nz = nodes[i + 2]
length2 = seg_len[i + 1]
vx, vy = (nx_ - bx) / length2, (ny_ - by) / length2
cx1, cy1 = bx + vx * t, by + vy * t
z_in = az + g * (length - t)
z_out = bz + seg_grade[i + 1] * t
arc_len = eff_r * th
n_arc = max(int(arc_len / step_m), 2)
cross = ux * vy - uy * vx
sign = 1.0 if cross >= 0 else -1.0
ox, oy = cx0 + (-uy * sign) * eff_r, cy0 + (ux * sign) * eff_r
ang0 = math.atan2(cy0 - oy, cx0 - ox)
for k in range(1, n_arc):
a = ang0 + sign * th * (k / n_arc)
frac = k / n_arc
_append(
(
ox + eff_r * math.cos(a),
oy + eff_r * math.sin(a),
z_in + (z_out - z_in) * frac,
),
eff_r,
)
_ = (cx1, cy1)
_append(nodes[-1], float("inf"))
cleaned, cleaned_r = [poly[0]], [radii[0]]
for p, r in zip(poly[1:], radii[1:]):
if math.hypot(p[0] - cleaned[-1][0], p[1] - cleaned[-1][1]) > 0.05:
cleaned.append(p)
cleaned_r.append(r)
if len(cleaned) >= 2:
cleaned[-1] = poly[-1]
return cleaned, cleaned_r
def resolve_grade_bounds(options: dict[str, Any]) -> dict[str, float]:
"""options에서 방향별 경사 상/하한을 해석한다.
options의 경사 값 단위는 퍼센트(%)이므로 비율로 환산한다. default는 이미 비율이다.
"""
def _opt(key: str, default: float) -> float:
v = options.get(key)
return float(v) / 100.0 if v is not None else default
min_uphill_grade = _opt("min_uphill_grade", ROUTE_ALT_MIN_GRADE)
min_downhill_grade = _opt("min_downhill_grade", ROUTE_ALT_MIN_GRADE)
max_uphill_grade = _opt("max_uphill_grade", ROUTE_ALT_MAX_GRADE)
max_downhill_grade = _opt("max_downhill_grade", ROUTE_ALT_MAX_GRADE)
return {
"min_uphill_grade": min_uphill_grade,
"min_downhill_grade": min_downhill_grade,
"max_uphill_grade": max_uphill_grade,
"max_downhill_grade": max_downhill_grade,
"min_grade_for_edges": max(min_uphill_grade, min_downhill_grade),
"max_grade_for_edges": min(max_uphill_grade, max_downhill_grade),
}
def solve_ridge_valley_route(
project_root: Path,
filter_key: str,
smooth: bool,
points_data: dict[str, Any],
options: dict[str, Any],
method: str = "dtm",
) -> dict[str, Any]:
"""능선-계곡 정속경사 방식으로 BP→(CP…)→EP 경로를 계산한다."""
project_root = Path(project_root)
models_dir = project_root / _MODELS_SUBDIR
(x_coords, y_coords, z_grid, valid_mask, _dz_dx, _dz_dy, grid_res) = (
_load_or_build_cost_surface(project_root, models_dir, filter_key, method, smooth)
)
grid = _Grid(x_coords, y_coords, z_grid, valid_mask, grid_res)
bp = points_data.get("bp")
ep = points_data.get("ep")
# 번호가 비어 오는 경유점 처리는 `solve_optimal_route`와 같다 — None 정렬로 터지지 않게.
cp_list = sorted(points_data.get("cp", []), key=lambda item: item.get("order") or 0)
ap_list = points_data.get("ap", [])
fp_list = points_data.get("fp", [])
if not bp or not ep:
raise ValueError("BP/EP 가 배치되어 있지 않습니다.")
skeleton = load_or_build_skeleton(project_root, filter_key, method, smooth)
barrier = _build_barrier_mask(grid, skeleton)
gb = resolve_grade_bounds(options)
min_uphill_grade = gb["min_uphill_grade"]
min_downhill_grade = gb["min_downhill_grade"]
max_uphill_grade = gb["max_uphill_grade"]
max_downhill_grade = gb["max_downhill_grade"]
max_grade = gb["max_grade_for_edges"]
min_grade = gb["min_grade_for_edges"]
tol = float(options.get("alt_grade_tolerance") or ROUTE_ALT_GRADE_TOLERANCE)
min_curve_radius_m = options.get("min_curve_radius_m")
if not min_curve_radius_m or min_curve_radius_m <= 0:
grade_class = options.get("grade_class", ROUTE_DEFAULT_GRADE_CLASS)
min_curve_radius_m = FOREST_ROAD_MIN_CURVE_R_M.get(grade_class, 12.0)
min_curve_radius_m = float(min_curve_radius_m)
endpoint_free_m = float(SKELETON_NODE_SPACING_M) * 1.5
blocked = list(fp_list)
if not options.get("allow_avoid_pass_through", False):
blocked = blocked + list(ap_list)
pos, kind, _on_main = _collect_nodes(skeleton, float(SKELETON_NODE_SPACING_M))
adj = _build_edges(
pos, kind, grid, barrier, blocked, min_grade, max_grade, tol, endpoint_free_m
)
sequence = [bp] + cp_list + [ep]
def _label(idx):
if idx == 0:
return "BP"
if idx == len(sequence) - 1:
return "EP"
return f"CP{sequence[idx].get('order', idx)}"
all_nodes: list[list[float]] = []
node_seg_marks: list[int] = []
for i in range(len(sequence) - 1):
seg_nodes = _search_segment(
sequence[i],
sequence[i + 1],
pos,
adj,
grid,
barrier,
blocked,
max_uphill_grade,
max_downhill_grade,
tol,
endpoint_free_m,
min_curve_radius_m,
)
if not seg_nodes:
raise ValueError(
f"세그먼트 {i + 1} ({_label(i)} -> {_label(i + 1)}) 능선-계곡 정속경사 경로 "
f"탐색 실패: 오르막 {min_uphill_grade * 100:.0f}~{max_uphill_grade * 100:.0f}%·"
f"내리막 {min_downhill_grade * 100:.0f}~{max_downhill_grade * 100:.0f}%·"
f"허용오차 ±{tol * 100:.1f}%·최소곡선반지름 {min_curve_radius_m:.0f}m 제약으로 "
f"성립하는 지능선-지계곡 연결이 없습니다."
)
if i == 0:
all_nodes.extend(seg_nodes)
else:
all_nodes.extend(seg_nodes[1:])
node_seg_marks.append(len(all_nodes) - 1)
polyline, _vertex_radii = _fillet_alignment(
all_nodes, min_curve_radius_m, step_m=max(grid.res, 2.0)
)
n = len(polyline)
chainage_m = [0.0] * n
length_m = 0.0
max_grade_pct = 0.0
max_uphill_pct = 0.0
max_downhill_pct = 0.0
grade_sums = 0.0
slope_violations = 0
for i in range(n - 1):
x1, y1, z1 = polyline[i]
x2, y2, z2 = polyline[i + 1]
hd = math.hypot(x2 - x1, y2 - y1)
chainage_m[i + 1] = chainage_m[i] + hd
if hd > 0.01:
dz = z2 - z1
s = abs(dz) / hd
length_m += hd
grade_sums += s * hd
max_grade_pct = max(max_grade_pct, s)
if dz > 0:
max_uphill_pct = max(max_uphill_pct, s)
applicable = max_uphill_grade
else:
max_downhill_pct = max(max_downhill_pct, s)
applicable = max_downhill_grade
if s > applicable + tol:
slope_violations += 1
avg_grade_pct = (grade_sums / length_m) if length_m > 0 else 0.0
curve_check_step = max(2.0 * grid.res, 4.0)
resampled = resample_polyline_2d(polyline, chainage_m, curve_check_step)
total_chainage = chainage_m[-1] if chainage_m else 0.0
curve_violations = 0
min_curve_radius_actual = float("inf")
radii = [float("inf")] * len(resampled)
for i in range(1, len(resampled) - 1):
radius = circumradius_2d(resampled[i - 1], resampled[i], resampled[i + 1])
radii[i] = radius
min_curve_radius_actual = min(min_curve_radius_actual, radius)
if radius < min_curve_radius_m * 0.99:
curve_violations += 1
curve_warning_segments = []
run_start = None
for i in range(1, len(resampled)):
violating = i < len(resampled) - 1 and radii[i] < min_curve_radius_m * 0.99
if violating and run_start is None:
run_start = i
if (not violating) and run_start is not None:
run_end = i - 1
ch_s = min(run_start * curve_check_step, total_chainage)
ch_e = min(run_end * curve_check_step, total_chainage)
curve_warning_segments.append(
{
"chainage_start_m": round(ch_s, 2),
"chainage_end_m": round(ch_e, 2),
"min_radius_m": round(min(radii[run_start : run_end + 1]), 2),
"required_radius_m": round(min_curve_radius_m, 2),
"polyline_start_index": int(np.searchsorted(chainage_m, ch_s)),
"polyline_end_index": int(np.searchsorted(chainage_m, ch_e)),
}
)
run_start = None
def _nearest_vertex(pt) -> int:
best, best_d = 0, float("inf")
for i, p in enumerate(polyline):
d = (p[0] - pt[0]) ** 2 + (p[1] - pt[1]) ** 2
if d < best_d:
best, best_d = i, d
return best
segments = []
prev_idx = 0
for si, mark in enumerate(node_seg_marks):
end_idx = _nearest_vertex(all_nodes[mark])
s, e = prev_idx, max(end_idx, prev_idx)
seg_max_grade = 0.0
for i in range(s, e):
x1, y1, z1 = polyline[i]
x2, y2, z2 = polyline[i + 1]
hd = math.hypot(x2 - x1, y2 - y1)
if hd > 0.01:
seg_max_grade = max(seg_max_grade, abs(z2 - z1) / hd)
segments.append(
{
"index": si,
"from": _label(si),
"to": _label(si + 1),
"point_start": s,
"point_end": e,
"chainage_start_m": round(chainage_m[s], 2),
"chainage_end_m": round(chainage_m[e], 2),
"length_m": round(chainage_m[e] - chainage_m[s], 2),
"max_grade_pct": round(seg_max_grade * 100, 2),
}
)
prev_idx = e
tol_req = ROUTE_REQUIRED_POINT_TOLERANCE_M
required_point_checks = []
for idx, pt in enumerate(sequence):
d = point_to_polyline_dist_2d(pt["x"], pt["y"], polyline)
required_point_checks.append(
{
"point": _label(idx),
"x": pt["x"],
"y": pt["y"],
"distance_m": round(d, 3),
"snap_distance_m": 0.0,
"point_on_valid_terrain": grid.valid_at(pt["x"], pt["y"]),
"tolerance_m": tol_req,
"within_tolerance": bool(d <= tol_req),
}
)
required_points_ok = all(c["within_tolerance"] for c in required_point_checks)
avoid_intrusions = circle_intrusions(polyline, ap_list, lambda i: f"AP{i + 1}")
forbidden_intrusions = circle_intrusions(polyline, fp_list, lambda i: f"FP{i + 1}")
constant_grade_segments = []
for i in range(len(all_nodes) - 1):
length = math.hypot(
all_nodes[i + 1][0] - all_nodes[i][0], all_nodes[i + 1][1] - all_nodes[i][1]
)
if length > 0.01:
constant_grade_segments.append(
{
"index": i,
"length_m": round(length, 2),
"grade_pct": round((all_nodes[i + 1][2] - all_nodes[i][2]) / length * 100, 2),
}
)
conditions_snapshot = {
"filter": filter_key,
"method": method,
"smooth": smooth,
"algorithm": "ridge_valley",
"grade_class": options.get("grade_class", ROUTE_DEFAULT_GRADE_CLASS),
"paved": bool(options.get("paved", False)),
"max_uphill_grade_pct": round(max_uphill_grade * 100, 2),
"max_downhill_grade_pct": round(max_downhill_grade * 100, 2),
"min_uphill_grade_pct": round(min_uphill_grade * 100, 2),
"min_downhill_grade_pct": round(min_downhill_grade * 100, 2),
"grade_tolerance_pct": round(tol * 100, 2),
"min_curve_radius_m": round(min_curve_radius_m, 2),
"weights": options.get("weights") or {},
"avoid_count": len(ap_list),
"forbidden_count": len(fp_list),
}
return {
"polyline": polyline,
"chainage_m": [round(v, 3) for v in chainage_m],
"segments": segments,
"required_point_checks": required_point_checks,
"required_points_ok": required_points_ok,
"avoid_intrusions": avoid_intrusions,
"forbidden_intrusions": forbidden_intrusions,
"curve_warning_segments": curve_warning_segments,
"avoid_retry_performed": False,
"conditions_snapshot": conditions_snapshot,
"constant_grade_segments": constant_grade_segments,
"metrics": {
"length_m": round(length_m, 2),
"avg_grade_pct": round(avg_grade_pct * 100, 2),
"max_grade_pct": round(max_grade_pct * 100, 2),
"max_uphill_pct": round(max_uphill_pct * 100, 2),
"max_downhill_pct": round(max_downhill_pct * 100, 2),
"slope_violations": slope_violations,
"curve_violations": curve_violations,
"min_curve_radius_m": round(min_curve_radius_actual, 2)
if math.isfinite(min_curve_radius_actual)
else None,
"min_curve_radius_limit_m": round(min_curve_radius_m, 2),
},
}