diff --git a/B05_Profile/B05_Profile_Engine_RidgeValley.py b/B05_Profile/B05_Profile_Engine_RidgeValley.py index c44bed63..eaa53da9 100644 --- a/B05_Profile/B05_Profile_Engine_RidgeValley.py +++ b/B05_Profile/B05_Profile_Engine_RidgeValley.py @@ -19,6 +19,15 @@ from B05_Profile.B05_Profile_Engine_Geometry import ( 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, @@ -34,255 +43,11 @@ from config.config_system import ( SKELETON_NODE_SPACING_M, ) -# 엣지 후보 탐색 파라미터 (알고리즘 내부 상수) -MAX_EDGE_LEN_M = 400.0 -MIN_EDGE_LEN_M = 20.0 -MAX_NEIGHBORS_PER_NODE = 16 +# 교각 페널티 (알고리즘 내부 상수) — 엣지 후보 상수는 _Graph 로 옮겼다(2026-09-02). TURN_PENALTY_W = 60.0 MAX_TURN_DEG = 120.0 -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) - - def _search_segment( start_pt: dict[str, float], end_pt: dict[str, float], diff --git a/B05_Profile/B05_Profile_Engine_RidgeValley_Graph.py b/B05_Profile/B05_Profile_Engine_RidgeValley_Graph.py new file mode 100644 index 00000000..e3ba5390 --- /dev/null +++ b/B05_Profile/B05_Profile_Engine_RidgeValley_Graph.py @@ -0,0 +1,257 @@ +"""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)