"""초기 계획노선 갈래의 격자 탐색(PLAN 20 · 23장) — `B05_Profile_Engine_RouteInitial` 이 부름. 탐색 = 예상노선 둘레 복도(`CORRIDOR_M`) 안 `SEARCH_CELL_M` 격자 · 16 방향 Dijkstra(scipy csgraph). 상태 = (칸, 진행 방향, 꺾은 뒤 곧게 온 걸음 수). 탐색 경로의 꺾임점이 곧 노드다. 그래프(`RouteGraph`)는 요청마다 **한 번만** 짠다 — 복도 안 모든 링크를 상태 간선으로 펼쳐 두고, 탐색 상한 · 목표 비중 · 벌점 크기가 바뀌는 탐색은 간선 비용만 다시 계산한다(지형상 불가능 상한 이분 탐색 · 후보 여러 벌을 한 요청 안에서). """ from __future__ import annotations import math from dataclasses import dataclass import numpy as np from scipy import ndimage from scipy.sparse import csr_matrix from scipy.sparse.csgraph import breadth_first_order, connected_components, dijkstra from B05_Profile.B05_Profile_Engine_RouteInitial_Metrics import GRADE_SMOOTH_M, smoothed_ground # 토공 가늠 폭(m) = 유효너비 3 + 길어깨 0.5 × 2(별표2 간선). ROAD_WIDTH_M = 4.0 # 탐색 복도 반폭(m) — 예상노선에서 이보다 멀리는 가지 않음(법령 값 아님 · 알고리즘 값). CORRIDOR_M = 100.0 SEARCH_CELL_M = 4.0 # 탐색 격자 칸(m) — 비용면(2 m)을 솎아 씀 LENGTH_COST = 0.05 # 1 m 당 거리 비용 — 목표가 평평한 곳에서 헤매지 않게 TURN_COST = 1.0 # 꺾을 때 벌점 = 최소 곡선반지름 원호 길이(R·θ) × 이 값 — 곧게 달리기 선호 SOFT_STREAM_COST = 20.0 # 계류 버퍼 안 나란히 달리기(지형상 불가능할 때만 · 1 m 당) BOUND_BISECT = 7 # 지형상 불가능할 때 막는 기울기를 찾는 이분 탐색 횟수(구간 1/128 까지) STREAM_CROSS_COS = math.cos(math.radians(45.0)) # 버퍼 안 이동 방향 한도(계류 쪽 · 반대쪽 ±45°) OBJECTIVES = ("expected", "earthwork", "grade") # 16 방향(행, 열) — +x 에서 반시계. 나이트 걸음(1, 2) 등을 넣어 22.5° 안팎 방향을 곧게 달림. DIRECTIONS = ( (0, 1), (1, 2), (1, 1), (2, 1), (1, 0), (2, -1), (1, -1), (1, -2), (0, -1), (-1, -2), (-1, -1), (-2, -1), (-1, 0), (-2, 1), (-1, 1), (-1, 2), ) @dataclass class Terrain: """지반 격자 — 탐색 비용면(`B05_Profile_Engine_Solver._load_or_build_cost_surface`) 그대로.""" x: np.ndarray # 열 좌표(오름차순) y: np.ndarray # 행 좌표(오름차순) z: np.ndarray # [행, 열] 표고 valid: np.ndarray # [행, 열] 지형 있음 class NoRoute(ValueError): """제약을 지키며 시점과 종점을 잇는 길이 없음.""" def _cell_size(coords: np.ndarray) -> float: return float((coords[-1] - coords[0]) / (len(coords) - 1)) if len(coords) > 1 else 1.0 def _distance_grid( lines: list[list[tuple[float, float]]], xs: np.ndarray, ys: np.ndarray, cell: float ) -> np.ndarray: """격자 칸마다 선까지 거리(m). 선이 격자에 안 닿으면 모두 무한.""" marked = np.zeros((len(ys), len(xs)), dtype=bool) for line in lines: for (x0, y0), (x1, y1) in zip(line, line[1:]): count = max(2, int(math.hypot(x1 - x0, y1 - y0) / (cell * 0.5)) + 1) px = np.linspace(x0, x1, count) py = np.linspace(y0, y1, count) cols = np.rint((px - xs[0]) / cell).astype(int) rows = np.rint((py - ys[0]) / cell).astype(int) inside = (cols >= 0) & (cols < len(xs)) & (rows >= 0) & (rows < len(ys)) marked[rows[inside], cols[inside]] = True if not marked.any(): return np.full(marked.shape, np.inf) return ndimage.distance_transform_edt(~marked) * cell class SearchGrid: """탐색 격자 — 복도 · 거리 · 기울기를 한 번만 만든다.""" def __init__( self, expected: list[tuple[float, float]], terrain: Terrain, streams: list[list[tuple[float, float]]], *, stream_offset_m: float, grade_smooth_m: float = GRADE_SMOOTH_M, ) -> None: step = max(1, int(round(SEARCH_CELL_M / _cell_size(terrain.x)))) self.xs, self.ys = terrain.x[::step], terrain.y[::step] # 기울기 · 옆경사는 비교값과 같은 고른 지반에서 잰다(`smoothed_ground`) self.z = smoothed_ground(terrain, grade_smooth_m)[::step, ::step] self.stream_offset_m = stream_offset_m self.cell = _cell_size(self.xs) self.d_expected = _distance_grid([expected], self.xs, self.ys, self.cell) self.d_stream = _distance_grid(streams, self.xs, self.ys, self.cell) valid = np.asarray(terrain.valid[::step, ::step], dtype=bool) self.corridor = (self.d_expected <= CORRIDOR_M) & valid self.grad_y, self.grad_x = np.gradient(self.z, self.cell) finite = np.where(np.isfinite(self.d_stream), self.d_stream, 0.0) self.away_y, self.away_x = np.gradient(finite, self.cell) # 계류에서 멀어지는 쪽 self.index = np.full(self.z.shape, -1, dtype=np.int64) self.cells = int(self.corridor.sum()) self.index[self.corridor] = np.arange(self.cells) rows, cols = np.nonzero(self.corridor) self.centres = np.column_stack([self.xs[cols], self.ys[rows]]) def nearest(self, point: tuple[float, float]) -> int: dx = self.centres[:, 0] - point[0] return int(np.argmin(np.hypot(dx, self.centres[:, 1] - point[1]))) def links(self, dr: int, dc: int, *, limit: float) -> tuple: """복도 안 한 방향 링크 모두 — (앞 번호, 뒤 번호, 길이, 세 목표[3, n] · 1 m 당, 넘은 기울기[n], 계류 버퍼 안 나란히[n], 지반 기울기[n]).""" rows, cols = self.z.shape first = (slice(max(0, -dr), rows - max(0, dr)), slice(max(0, -dc), cols - max(0, dc))) second = (slice(max(0, dr), rows - max(0, -dr)), slice(max(0, dc), cols - max(0, -dc))) a, b = self.index[first], self.index[second] ok = (a >= 0) & (b >= 0) norm = math.hypot(dr, dc) grade = (np.abs(self.z[second] - self.z[first]) / (self.cell * norm))[ok] # 계류 버퍼 안에서는 계류 쪽 · 반대쪽 ±45° 로만(건너기 · 빠져나가기) in_buffer = (self.d_stream[first] < self.stream_offset_m) | ( self.d_stream[second] < self.stream_offset_m ) ax = self.away_x[first] + self.away_x[second] ay = self.away_y[first] + self.away_y[second] size = np.hypot(ax, ay) across = np.abs(ax * dc + ay * dr) >= STREAM_CROSS_COS * norm * size along = (in_buffer & ~across & (size > 1e-6))[ok] over = np.maximum(0.0, grade - limit) # 옆경사 = 진행 방향에 수직인 지반 기울기(두 칸 평균) cross = np.abs( (self.grad_x[first][ok] + self.grad_x[second][ok]) * -dr + (self.grad_y[first][ok] + self.grad_y[second][ok]) * dc ) / (2.0 * norm) # 토공 가늠(1 m 당 m³) — 옆경사 위 반절 · 반성 단면 W²·s/4 + 상한 넘는 기울기가 측점 # 반 간격(10 m) 동안 쌓는 높이 차 단면(지형상 불가능할 때만 0 이 아님) earth = ROAD_WIDTH_M**2 * cross / 4.0 + ROAD_WIDTH_M * over * 10.0 offset = 0.5 * (self.d_expected[first][ok] + self.d_expected[second][ok]) terms = np.stack([offset, earth, (grade / limit) ** 2]) return a[ok], b[ok], self.cell * norm, terms, over, along, grade def _turn_steps(min_radius_m: float, cell: float) -> int: """꺾은 뒤 곧게 가야 할 칸 수 — 앞뒤 원호 접선이 한 직선에 들어가게. 원호를 끼우는 쪽(`build_planned_polyline`)은 직선의 절반까지만 접선으로 쓰므로 직선은 2 · R · tan(θ/2) 이상이어야 한다. θ = 이웃 방향 사이 가장 큰 각(나이트 걸음 26.57°). 걸음은 칸 길이 단위로 센다 — 곧은 · 대각 걸음 1, 나이트 걸음 2. """ widest = math.atan2(1.0, 2.0) return max(1, math.ceil(2.0 * min_radius_m * math.tan(widest / 2.0) / cell)) class RouteGraph: """상태 그래프 — 복도 안 모든 링크를 한 번 펼쳐 두고 탐색마다 비용 · 가릴 간선만 바꾼다. 상태 = (칸, 방향, 층). 층 = 꺾은 뒤 곧게 온 칸 수 + 1(K + 1 에서 멈춤) · 층 0 = 시점. 이웃 방향으로 꺾기는 K 칸 곧게 온 뒤에만(층 K + 1) → 노드마다 최소 반지름 원호가 들어감. 시점은 한 칸 더(K + 1 칸) — 기점을 격자 칸 가운데에서 예상노선 끝점으로 옮기며 첫 직선이 짧아지는 몫. 종점은 K 칸 곧게 들어온 상태를 먼저, 없으면 아무 상태(마지막 곡선 자리가 모자라면 목록에 뜸). """ def __init__( self, grid: SearchGrid, expected: list[tuple[float, float]], *, limit: float, min_radius_m: float, ) -> None: self.grid, self.expected, self.limit = grid, expected, limit count = len(DIRECTIONS) self.layers = _turn_steps(min_radius_m, grid.cell) + 2 # 꺾을 수 있는 층 = layers - 1 full = self.layers - 1 angles = [math.atan2(dr, dc) for dr, dc in DIRECTIONS] heads, tails, link_of, turn = [], [], [], [] per_link = [] offset = 0 for out, (dr, dc) in enumerate(DIRECTIONS): a, b, length, terms, over, along, grade = grid.links(dr, dc, limit=limit) ids = np.arange(offset, offset + a.size) offset += a.size per_link.append((np.full(a.size, length), terms, over, along, grade)) units = max(1, int(length / grid.cell + 1e-9)) # 곧은 · 대각 1, 나이트 2 for walked in range(self.layers): # 곧게 heads.append(self._state(a, out, walked)) tails.append(self._state(b, out, min(walked + units, full))) link_of.append(ids) turn.append(np.zeros(a.size)) for came in ((out - 1) % count, (out + 1) % count): # 이웃 방향에서 꺾어 들어옴 bend = abs((angles[out] - angles[came] + math.pi) % (2.0 * math.pi) - math.pi) heads.append(self._state(a, came, full)) tails.append(self._state(b, out, min(units + 1, full))) link_of.append(ids) turn.append(np.full(a.size, TURN_COST * min_radius_m * bend)) if offset == 0: raise NoRoute("예상노선 둘레에 지날 수 있는 링크가 없습니다.") self.length = np.concatenate([p[0] for p in per_link]) self.terms = np.hstack([p[1] for p in per_link]) self.over = np.concatenate([p[2] for p in per_link]) self.along = np.concatenate([p[3] for p in per_link]) self.grade = np.concatenate([p[4] for p in per_link]) self.size = grid.cells * count * self.layers # 간선 틀(CSR)은 한 번만 — 탐색마다 못 쓰는 간선을 걸러 내고 비용만 갈아 끼움 link_of = np.concatenate(link_of) order = csr_matrix( (np.arange(1.0, link_of.size + 1.0), (np.concatenate(heads), np.concatenate(tails))), shape=(self.size, self.size), ) self.indices, self.indptr = order.indices, order.indptr edge = (order.data - 1.0).astype(np.int64) self.edge_link = link_of[edge] self.edge_turn = np.concatenate(turn)[edge] start, end = grid.nearest(expected[0]), grid.nearest(expected[-1]) self.starts = [self._state(start, d, 0) for d in range(count)] self.straight = [self._state(end, d, full) for d in range(count)] self.ends = [self._state(end, d, w) for d in range(count) for w in range(self.layers)] def _state(self, cell, direction: int, walked: int): return (cell * len(DIRECTIONS) + direction) * self.layers + walked def _usable(self, cap: float, strict_stream: bool) -> np.ndarray: """링크별 쓸 수 있음 — `cap` 넘는 기울기 · (엄격하면) 계류 버퍼 안 나란히 빼고.""" usable = self.grade <= cap return usable & ~self.along if strict_stream else usable def _graph(self, usable: np.ndarray, link_cost: np.ndarray | None) -> csr_matrix: """쓸 수 있는 간선만 남긴 그래프 + 시점 상태 모두로 가는 원점(마지막 상태 `size`).""" keep = usable[self.edge_link] indptr = np.concatenate([[0], np.cumsum(keep)])[self.indptr] indptr = np.append(indptr, indptr[-1] + len(self.starts)) indices = np.concatenate([self.indices[keep], self.starts]) if link_cost is None: data = np.ones(indices.size) else: cost = link_cost[self.edge_link[keep]] + self.edge_turn[keep] data = np.concatenate([cost, np.full(len(self.starts), 1e-9)]) return csr_matrix((data, indices, indptr), shape=(self.size + 1, self.size + 1)) def joined(self, cap: float) -> bool: """`cap` 까지 기울기를 풀면 시점 · 종점이 이어지나(계류는 벌점 — 기울기만 따짐).""" graph = self._graph(self._usable(cap, False), None) reached = breadth_first_order(graph, self.size, return_predecessors=False) return bool(np.isin(self.ends, reached).any()) def solve( self, weights: dict[str, float], *, cap: float, strict_stream: bool, grade_cost: float = 0.0, ) -> list[tuple[float, float]]: """노드(꺾임점) 목록 — 첫 · 끝은 예상노선 양 끝 그대로. 세 목표는 쓸 수 있는 링크 평균이 1 이 되게 정규화(길이 가중)한 뒤 비중을 곱한다. `grade_cost` = 상한을 넘은 기울기 벌점(1 m 당 × 넘은 비율 / 상한) — `cap` 이 상한보다 클 때(지형상 불가능)만 뜻이 있다. """ usable = self._usable(cap, strict_stream) if not usable.any(): raise NoRoute("예상노선 둘레에 제약을 지키며 지날 수 있는 링크가 없습니다.") length = self.length[usable] scale = (self.terms[:, usable] * length).sum(axis=1) / max(length.sum(), 1e-9) share = np.array([weights[key] for key in OBJECTIVES]) / max(sum(weights.values()), 1e-9) factor = np.divide(share, scale, out=np.zeros_like(share), where=scale > 1e-12) penalty = grade_cost * self.over / self.limit if not strict_stream: penalty = penalty + SOFT_STREAM_COST * self.along link_cost = self.length * (LENGTH_COST + factor @ self.terms + penalty) distance, predecessors = dijkstra( self._graph(usable, link_cost), indices=self.size, return_predecessors=True ) final = min(self.straight, key=lambda s: distance[s]) if not np.isfinite(distance[final]): final = min(self.ends, key=lambda s: distance[s]) if not np.isfinite(distance[final]): raise NoRoute("복도 안에서 제약을 지키며 시점과 종점을 잇는 길이 없습니다.") chain = [int(final)] while predecessors[chain[-1]] >= 0: chain.append(int(predecessors[chain[-1]])) chain = chain[::-1][1:] # 원점 뺌 # 꺾임점 = 방향이 바뀌는 칸. 첫 · 끝은 예상노선 끝점 그대로(기점 · 종점은 옮기지 않음). count, layers = len(DIRECTIONS), self.layers nodes = [tuple(self.expected[0])] for previous, current in zip(chain[1:], chain[2:]): if (previous // layers) % count != (current // layers) % count: cell = previous // layers // count nodes.append((float(self.grid.centres[cell, 0]), float(self.grid.centres[cell, 1]))) nodes.append(tuple(self.expected[-1])) return nodes def minimax_grade(self) -> float: """복도 안에서 시점과 종점을 잇는 데 **어떤 길도 넘어야 하는** 가장 작은 기울기 상한(비율). 꺾기 규칙 없이 칸끼리만 이은 그래프에서 이분 탐색 — 그러니 실제 필요한 값의 아래 한계다. """ grid = self.grid pieces = [] for dr, dc in DIRECTIONS: a, b, *_, grade = grid.links(dr, dc, limit=self.limit) pieces.append((a, b, grade)) heads = np.concatenate([p[0] for p in pieces]) tails = np.concatenate([p[1] for p in pieces]) grades = np.concatenate([p[2] for p in pieces]) start, end = grid.nearest(self.expected[0]), grid.nearest(self.expected[-1]) def joined(cap: float) -> bool: keep = grades <= cap graph = csr_matrix( (np.ones(int(keep.sum())), (heads[keep], tails[keep])), shape=(grid.cells, grid.cells), ) _, labels = connected_components(graph, directed=False) return bool(labels[start] == labels[end]) low, high = 0.0, float(grades.max()) if grades.size else 0.0 if not joined(high): raise NoRoute("복도 안에서 시점과 종점이 이어지지 않습니다.") for _ in range(12): # 아래 한계일 뿐 — 구간 1/4096 이면 넉넉함 middle = 0.5 * (low + high) low, high = (low, middle) if joined(middle) else (middle, high) return high def terrain_bound(self) -> float: """반지름 규칙까지 지키며 시점 · 종점을 잇는 데 **어떤 길도 넘어야 하는** 기울기(비율). 아래 한계 = 꺾기 규칙 없는 최소최대(`minimax_grade`). 이어지는 값을 1.5 배씩 찾은 뒤 그 사이를 상태 그래프 그대로 이분 탐색한다(계류는 벌점 — 기울기만 따짐). """ ceiling = float(self.grade.max(initial=0.0)) low = max(self.minimax_grade(), self.limit) high = low while not self.joined(high): if high >= ceiling: raise NoRoute("복도 안에서 반지름 규칙을 지키며 시점과 종점을 잇는 길이 없습니다.") low, high = high, min(high * 1.5, ceiling) for _ in range(BOUND_BISECT): if high - low <= 1e-4: break middle = 0.5 * (low + high) low, high = (low, middle) if self.joined(middle) else (middle, high) return high