eomsangdon
acdd2cef4e
- `B05_Profile_Engine_RidgeValley_Graph.py`(257줄) 신설 — 격자 조회(`_Grid`), 노드 수집, 구획 마스크, 정속경사 세그먼트 판정, 엣지·연결 후보, 교각 계산. - 원본에는 탐색(`_search_segment`)·fillet 선형·진입점 계산만 남김. - 엣지 후보 상수(MAX_EDGE_LEN_M 등)도 함께 이동 — 쓰는 쪽이 전부 새 모듈. - 외부 호출부 불변(`solve_ridge_valley_route` 경로 동일), pytest 359 passed.
258 lines
8.3 KiB
Python
258 lines
8.3 KiB
Python
"""B05 능선-계곡 길찾기 — 그래프 뼈대(격자 조회·노드·엣지).
|
|
|
|
`B05_Profile_Engine_RidgeValley` 에서 떼어낸 앞단이다(700줄 제한, 2026-09-02).
|
|
탐색·선형(fillet)·진입점 계산은 원래 파일에 남고, 여기에는 **비용면 격자 조회와
|
|
정속경사 세그먼트 판정·엣지 생성**만 둔다. 호출부는 원래 파일뿐이다.
|
|
"""
|
|
|
|
import math
|
|
from typing import Any
|
|
|
|
import numpy as np
|
|
|
|
# 엣지 후보 탐색 파라미터 (알고리즘 내부 상수)
|
|
MAX_EDGE_LEN_M = 400.0
|
|
MIN_EDGE_LEN_M = 20.0
|
|
MAX_NEIGHBORS_PER_NODE = 16
|
|
|
|
|
|
class _Grid:
|
|
"""비용면 격자에 대한 표고/유효성 조회 헬퍼."""
|
|
|
|
def __init__(self, x, y, z, valid, grid_res):
|
|
self.x = np.asarray(x, dtype=np.float64)
|
|
self.y = np.asarray(y, dtype=np.float64)
|
|
self.z = np.asarray(z, dtype=np.float64)
|
|
self.valid = np.asarray(valid, dtype=bool)
|
|
self.res = float(grid_res)
|
|
|
|
def _idx(self, coords: np.ndarray, v: float) -> int:
|
|
i = int(np.clip(np.searchsorted(coords, v), 0, len(coords) - 1))
|
|
j = max(i - 1, 0)
|
|
return j if abs(v - coords[j]) <= abs(coords[i] - v) else i
|
|
|
|
def rc(self, px: float, py: float) -> tuple[int, int]:
|
|
return self._idx(self.y, py), self._idx(self.x, px)
|
|
|
|
def z_at(self, px: float, py: float) -> float:
|
|
r, c = self.rc(px, py)
|
|
return float(self.z[r, c])
|
|
|
|
def valid_at(self, px: float, py: float) -> bool:
|
|
in_bounds = (self.x[0] <= px <= self.x[-1]) and (self.y[0] <= py <= self.y[-1])
|
|
if not in_bounds:
|
|
return False
|
|
r, c = self.rc(px, py)
|
|
return bool(self.valid[r, c])
|
|
|
|
|
|
def _collect_nodes(
|
|
skeleton: dict[str, Any], spacing_m: float
|
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
|
"""능선/계곡 polyline 정점을 spacing 간격으로 다운샘플해 노드 배열을 만든다."""
|
|
pos, kind, on_main = [], [], []
|
|
|
|
def _add(polys, k, is_main):
|
|
for item in polys:
|
|
pl = item["polyline"]
|
|
acc = spacing_m
|
|
prev = None
|
|
for p in pl:
|
|
step = spacing_m if prev is None else math.hypot(p[0] - prev[0], p[1] - prev[1])
|
|
acc += step
|
|
prev = p
|
|
if acc >= spacing_m:
|
|
acc = 0.0
|
|
pos.append([p[0], p[1], p[2]])
|
|
kind.append(k)
|
|
on_main.append(is_main)
|
|
|
|
_add(skeleton.get("minor_ridge", []), 0, False)
|
|
_add(skeleton.get("minor_valley", []), 1, False)
|
|
_add(skeleton.get("main_ridge", []), 0, True)
|
|
_add(skeleton.get("main_valley", []), 1, True)
|
|
|
|
if not pos:
|
|
return (np.zeros((0, 3)), np.zeros(0, dtype=np.int8), np.zeros(0, dtype=bool))
|
|
return (
|
|
np.asarray(pos, dtype=np.float64),
|
|
np.asarray(kind, dtype=np.int8),
|
|
np.asarray(on_main, dtype=bool),
|
|
)
|
|
|
|
|
|
def _build_barrier_mask(grid: _Grid, skeleton: dict[str, Any]) -> np.ndarray:
|
|
"""주능선/주계곡 셀을 True로 표시한 구획 경계 마스크."""
|
|
barrier = np.zeros(grid.z.shape, dtype=bool)
|
|
for key in ("main_ridge", "main_valley"):
|
|
for item in skeleton.get(key, []):
|
|
for p in item["polyline"]:
|
|
r, c = grid.rc(p[0], p[1])
|
|
barrier[r, c] = True
|
|
return barrier
|
|
|
|
|
|
def _segment_feasible(
|
|
a: np.ndarray,
|
|
b: np.ndarray,
|
|
grid: _Grid,
|
|
barrier: np.ndarray,
|
|
blocked_circles: list[dict[str, float]],
|
|
min_grade: float,
|
|
max_grade: float,
|
|
tol: float,
|
|
endpoint_free_m: float,
|
|
enforce_grade_window: bool = True,
|
|
) -> bool:
|
|
"""a→b 직선이 정속경사 세그먼트로 성립하는지 검사한다."""
|
|
dx, dy = b[0] - a[0], b[1] - a[1]
|
|
length = math.hypot(dx, dy)
|
|
if length < 1e-6:
|
|
return False
|
|
design_grade = (b[2] - a[2]) / length
|
|
g = abs(design_grade)
|
|
if enforce_grade_window:
|
|
if not (min_grade <= g <= max_grade):
|
|
return False
|
|
elif g > max_grade:
|
|
return False
|
|
|
|
step = max(grid.res, 1.0)
|
|
n_steps = max(int(length / step), 1)
|
|
for i in range(n_steps + 1):
|
|
t = i / n_steps
|
|
px, py = a[0] + t * dx, a[1] + t * dy
|
|
if not grid.valid_at(px, py):
|
|
return False
|
|
s = t * length
|
|
z_design = a[2] + design_grade * s
|
|
z_terrain = grid.z_at(px, py)
|
|
allowed = tol * max(s, length - s) + grid.res
|
|
if abs(z_terrain - z_design) > allowed:
|
|
return False
|
|
if min(s, length - s) > endpoint_free_m:
|
|
r, c = grid.rc(px, py)
|
|
if barrier[r, c]:
|
|
return False
|
|
for circ in blocked_circles:
|
|
if math.hypot(px - circ["x"], py - circ["y"]) < circ["radius_m"]:
|
|
return False
|
|
return True
|
|
|
|
|
|
def _build_edges(
|
|
pos: np.ndarray,
|
|
kind: np.ndarray,
|
|
grid: _Grid,
|
|
barrier: np.ndarray,
|
|
blocked_circles: list[dict[str, float]],
|
|
min_grade: float,
|
|
max_grade: float,
|
|
tol: float,
|
|
endpoint_free_m: float,
|
|
) -> dict[int, list[tuple[int, float]]]:
|
|
"""능선↔계곡 노드 쌍의 정속경사 직선 엣지를 만든다 (무방향, 길이 저장)."""
|
|
from scipy.spatial import cKDTree
|
|
|
|
adj: dict[int, list[tuple[int, float]]] = {i: [] for i in range(len(pos))}
|
|
if len(pos) == 0:
|
|
return adj
|
|
|
|
ridge_idx = np.nonzero(kind == 0)[0]
|
|
valley_idx = np.nonzero(kind == 1)[0]
|
|
if len(ridge_idx) == 0 or len(valley_idx) == 0:
|
|
return adj
|
|
|
|
valley_tree = cKDTree(pos[valley_idx, :2])
|
|
for ri in ridge_idx:
|
|
cand = valley_tree.query_ball_point(pos[ri, :2], MAX_EDGE_LEN_M)
|
|
cand = sorted(
|
|
cand,
|
|
key=lambda j: (
|
|
(pos[ri, 0] - pos[valley_idx[j], 0]) ** 2
|
|
+ (pos[ri, 1] - pos[valley_idx[j], 1]) ** 2
|
|
),
|
|
)
|
|
added = 0
|
|
for j in cand:
|
|
vi = int(valley_idx[j])
|
|
length = math.hypot(pos[ri, 0] - pos[vi, 0], pos[ri, 1] - pos[vi, 1])
|
|
if length < MIN_EDGE_LEN_M:
|
|
continue
|
|
if pos[ri, 2] <= pos[vi, 2]:
|
|
continue
|
|
if not _segment_feasible(
|
|
pos[ri],
|
|
pos[vi],
|
|
grid,
|
|
barrier,
|
|
blocked_circles,
|
|
min_grade,
|
|
max_grade,
|
|
tol,
|
|
endpoint_free_m,
|
|
):
|
|
continue
|
|
adj[int(ri)].append((vi, length))
|
|
adj[vi].append((int(ri), length))
|
|
added += 1
|
|
if added >= MAX_NEIGHBORS_PER_NODE:
|
|
break
|
|
return adj
|
|
|
|
|
|
def _endpoint_connectors(
|
|
pt: dict[str, float],
|
|
pos: np.ndarray,
|
|
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,
|
|
) -> list[tuple[int, float]]:
|
|
"""BP/CP/EP를 그래프 노드에 잇는 연결 세그먼트 후보."""
|
|
from scipy.spatial import cKDTree
|
|
|
|
if len(pos) == 0:
|
|
return []
|
|
p = np.array([pt["x"], pt["y"], grid.z_at(pt["x"], pt["y"])])
|
|
tree = cKDTree(pos[:, :2])
|
|
cand = tree.query_ball_point(p[:2], MAX_EDGE_LEN_M)
|
|
cand = sorted(cand, key=lambda j: (p[0] - pos[j, 0]) ** 2 + (p[1] - pos[j, 1]) ** 2)
|
|
out = []
|
|
for j in cand:
|
|
length = math.hypot(p[0] - pos[j, 0], p[1] - pos[j, 1])
|
|
if length < 1e-6:
|
|
out.append((int(j), max(length, 0.01)))
|
|
continue
|
|
applicable = max_uphill_grade if pos[j, 2] > p[2] else max_downhill_grade
|
|
if _segment_feasible(
|
|
p,
|
|
pos[j],
|
|
grid,
|
|
barrier,
|
|
blocked_circles,
|
|
0.0,
|
|
applicable,
|
|
tol,
|
|
endpoint_free_m,
|
|
enforce_grade_window=False,
|
|
):
|
|
out.append((int(j), length))
|
|
if len(out) >= MAX_NEIGHBORS_PER_NODE:
|
|
break
|
|
return out
|
|
|
|
|
|
def _turn_angle(p_prev, p_curr, p_next) -> float:
|
|
"""진행방향 변화(교각) [rad]. 0 = 직진."""
|
|
v1 = (p_curr[0] - p_prev[0], p_curr[1] - p_prev[1])
|
|
v2 = (p_next[0] - p_curr[0], p_next[1] - p_curr[1])
|
|
n1, n2 = math.hypot(*v1), math.hypot(*v2)
|
|
if n1 < 1e-9 or n2 < 1e-9:
|
|
return 0.0
|
|
cosang = max(-1.0, min(1.0, (v1[0] * v2[0] + v1[1] * v2[1]) / (n1 * n2)))
|
|
return math.acos(cosang)
|