769 lines
32 KiB
Python
769 lines
32 KiB
Python
"""능선-계곡 정속경사 임도 길찾기 (신규 알고리즘, algorithm="ridge_valley").
|
||
|
||
기존 utils/route_solver.py 의 격자 Dijkstra 와 완전히 분리된 방식:
|
||
|
||
1. terrain_skeleton 으로 주/지 능선·계곡 polyline 을 얻는다.
|
||
2. 지능선·지계곡 위의 정점을 노드로 하는 그래프를 만들고, 서로 다른 종류
|
||
(능선↔계곡)의 노드 쌍을 잇는 "직선 정속경사 세그먼트"가 성립할 때만
|
||
엣지를 만든다:
|
||
- 설계 종단경사 하한은 ROUTE_ALT_MIN_GRADE(8%), 상한은 기존 UI 의 최대 오르막/
|
||
최대 내리막(options["max_uphill_grade"]/["max_downhill_grade"], 미지정 시
|
||
ROUTE_ALT_MAX_GRADE=14%) 을 방향별로 그대로 적용한다.
|
||
- 지능선→지계곡은 내리막, 지계곡→지능선은 오르막 (능선이 더 높아야 함)
|
||
- 세그먼트 내 종단경사는 상수(설계상 변화 0)이고, 지형이 그 정속경사
|
||
직선에서 벗어나는 표고 편차가 ROUTE_ALT_GRADE_TOLERANCE(±0.5%) × 진행거리
|
||
이내여야 한다 (절·성토가 과대해지지 않는 정속경사 성립 조건)
|
||
- valid_mask 내부, FP(금지) 통과 불가, AP(회피)도 기본 통과 불가
|
||
- 주능선/주계곡은 구획 경계로 취급: 엣지가 중간에서 가로지를 수 없고
|
||
주선 위에 놓인 노드를 '통과 지점'으로 삼아서만 넘는다.
|
||
3. BP→(CP…)→EP 를 방향전환(교각) 페널티가 있는 Dijkstra 로 탐색해
|
||
가능한 한 직선이 유지되는 노드 시퀀스를 얻는다. 교각에 최소회전반경
|
||
원호(fillet)를 삽입할 수 없는 조합(접선장 > 세그먼트 절반)은 탐색에서 배제.
|
||
4. 최종 선형은 직선(tangent) + 최소회전반경 원호로 구성하고, 종단은
|
||
세그먼트별 정속경사로 부여한다.
|
||
|
||
반환 스키마는 기존 solve_optimal_route() 와 동일하게 맞춰 프론트엔드가
|
||
그대로 렌더링할 수 있다.
|
||
"""
|
||
|
||
import heapq
|
||
import math
|
||
from pathlib import Path
|
||
from typing import Any, Dict, List, Optional, Tuple
|
||
|
||
import numpy as np
|
||
|
||
import config
|
||
from utils.route_solver import (
|
||
_load_or_build_cost_surface,
|
||
_circle_intrusions,
|
||
_circumradius_2d,
|
||
_point_to_polyline_dist_2d,
|
||
_resample_polyline_2d,
|
||
)
|
||
from utils.terrain_skeleton import load_or_build_skeleton
|
||
|
||
# 엣지 후보 탐색 파라미터 (알고리즘 내부 상수)
|
||
MAX_EDGE_LEN_M = 400.0 # 지능선-지계곡 직선 세그먼트 최대 길이
|
||
MIN_EDGE_LEN_M = 20.0 # 최소 길이 (짧은 지그재그 억제 + fillet 여유)
|
||
MAX_NEIGHBORS_PER_NODE = 16 # 노드당 엣지 후보 상한 (그래프 크기 제어)
|
||
TURN_PENALTY_W = 60.0 # 교각(rad)당 비용 — 직선 유지 유도
|
||
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[N,3], kind[N] {0=ridge,1=valley}, on_main[N] bool)
|
||
주능선/주계곡 위 노드는 구획 경계 '통과 지점' 후보로 그래프에 포함된다.
|
||
"""
|
||
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:
|
||
if prev is None:
|
||
step = spacing_m
|
||
else:
|
||
step = 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 직선이 정속경사 세그먼트로 성립하는지 검사한다.
|
||
|
||
- 설계경사 |dz|/L 이 [min_grade, max_grade] (연결 세그먼트는 상한만)
|
||
- 지형 표고가 정속경사 직선에서 벗어나는 편차 ≤ tol × 진행거리
|
||
- 경로 전체가 valid_mask 내부, 회피/금지 원 밖
|
||
- 주능선/주계곡(barrier)은 양끝 endpoint_free_m 이내를 제외하고 통과 불가
|
||
"""
|
||
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
|
||
# 정속경사 설계선 대비 지형 표고 편차: 진행거리에 비례한 허용치(±tol)
|
||
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 를 그래프 노드에 잇는 연결 세그먼트 후보.
|
||
시·종점 배치는 사용자 주도이므로 경사 하한(8%)은 적용하지 않고 상한만 지킨다.
|
||
상한은 실제 오르막/내리막 방향에 맞는 한계(최대 오르막/최대 내리막 설정)를 적용한다."""
|
||
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],
|
||
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,
|
||
) -> Optional[List[List[float]]]:
|
||
"""start→end 를 그래프 위에서 탐색해 노드 좌표 시퀀스를 반환 (실패 시 None).
|
||
|
||
상태 = (직전 노드, 현재 노드) 로 두어 교각 페널티와 fillet 성립성(접선장
|
||
T = R·tan(Δ/2) ≤ 인접 세그먼트 절반)을 하드 제약으로 반영한다.
|
||
경사 상한은 진행 방향(오르막/내리막)에 맞는 한계를 적용한다.
|
||
"""
|
||
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
|
||
# v 에서 EP 로 빠질 수 있으면 종료 후보
|
||
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
|
||
# 역추적: end ← … ← start
|
||
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) 선형으로 변환한다.
|
||
|
||
종단은 세그먼트별 정속경사를 유지한다: 각 꼭짓점에서 접선장 T 만큼
|
||
직선이 짧아져도 해당 세그먼트의 설계경사(grade)를 그대로 적용해
|
||
접선점 표고를 계산하고, 원호 구간은 두 접선점 표고를 선형 보간한다.
|
||
반환: (polyline[[x,y,z]...], 각 정점의 곡선반경 리스트[inf=직선])
|
||
"""
|
||
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):
|
||
L = math.hypot(nodes[i + 1][0] - nodes[i][0], nodes[i + 1][1] - nodes[i][1])
|
||
seg_len.append(max(L, 1e-9))
|
||
seg_grade.append((nodes[i + 1][2] - nodes[i][2]) / max(L, 1e-9))
|
||
|
||
# 각 내부 꼭짓점의 접선장 T (세그먼트 절반을 넘지 않게 클램프)
|
||
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]
|
||
L, g = seg_len[i], seg_grade[i]
|
||
ux, uy = (bx - ax) / L, (by - ay) / L
|
||
s0, s1 = t_len[i], L - t_len[i + 1]
|
||
# 직선부 (접선점 s0 → s1) 를 step_m 간격으로 샘플
|
||
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) # 클램프된 T 로 실현되는 반경
|
||
cx0, cy0 = bx - ux * t, by - uy * t # 진입 접선점
|
||
nx_, ny_, nz_ = nodes[i + 2]
|
||
L2 = seg_len[i + 1]
|
||
vx, vy = (nx_ - bx) / L2, (ny_ - by) / L2
|
||
cx1, cy1 = bx + vx * t, by + vy * t # 진출 접선점
|
||
z_in = az + g * (L - t)
|
||
z_out = bz + seg_grade[i + 1] * t
|
||
arc_len = eff_r * th
|
||
n_arc = max(int(arc_len / step_m), 2)
|
||
# 원호를 두 접선벡터의 회전 보간(교점 기준 베지어 근사가 아닌 정확 원호):
|
||
# 회전 중심은 교각 이등분선 위에 있으며, 접선점에서 수직 방향으로 eff_r.
|
||
cross = ux * vy - uy * vx # 좌회전(+) / 우회전(-)
|
||
sign = 1.0 if cross >= 0 else -1.0
|
||
# 회전 중심 = 진입 접선점 + 진행방향 법선(좌/우) × eff_r
|
||
ox, oy = cx0 + (-uy * sign) * eff_r, cy0 + (ux * sign) * eff_r
|
||
ang0 = math.atan2(cy0 - oy, cx0 - ox)
|
||
# sign 방향으로 교각 th 만큼 회전하며 원호를 표본화
|
||
# (진출 접선점 (cx1, cy1) 은 ang0 + sign*th 위치와 일치한다)
|
||
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 에서 방향별 경사 상/하한을 해석한다 (테스트 가능한 단위로 분리).
|
||
|
||
max_uphill_grade/max_downhill_grade 는 기존 UI 필드를 그대로 재사용하고,
|
||
min_uphill_grade/min_downhill_grade 는 동일한 방식(None=기본값, 0=제한없음)으로
|
||
새로 추가된 필드다. 그래프 엣지 생성 시에는 두 방향 중 더 엄격한 값(상한=min,
|
||
하한=max)을 적용해 어느 방향으로 지나도 안전하도록 한다.
|
||
"""
|
||
def _opt(key: str, default: float) -> float:
|
||
v = options.get(key)
|
||
return float(v) if v is not None else default
|
||
|
||
min_uphill_grade = _opt("min_uphill_grade", config.ROUTE_ALT_MIN_GRADE)
|
||
min_downhill_grade = _opt("min_downhill_grade", config.ROUTE_ALT_MIN_GRADE)
|
||
max_uphill_grade = _opt("max_uphill_grade", config.ROUTE_ALT_MAX_GRADE)
|
||
max_downhill_grade = _opt("max_downhill_grade", config.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_id: str,
|
||
filter_key: str,
|
||
smooth: bool,
|
||
points_data: Dict[str, Any],
|
||
options: Dict[str, Any],
|
||
instance_dir: Path,
|
||
method: str = "dtm",
|
||
) -> Dict[str, Any]:
|
||
"""능선-계곡 정속경사 방식으로 BP→(CP…)→EP 경로를 계산한다.
|
||
반환 스키마는 solve_optimal_route() 와 동일."""
|
||
terrain_dir = Path(instance_dir) / project_id / "terrain_models"
|
||
|
||
(x_coords, y_coords, z_grid, valid_mask, _dz_dx, _dz_dy, grid_res) = (
|
||
_load_or_build_cost_surface(terrain_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")
|
||
cp_list = sorted(points_data.get("cp", []), key=lambda x: x.get("order", 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(terrain_dir, filter_key, method, smooth)
|
||
barrier = _build_barrier_mask(grid, skeleton)
|
||
|
||
# 정속경사 파라미터. 최대/최소 오르막·내리막은 동일한 방식(resolve_grade_bounds)으로
|
||
# 방향별 해석한다 — 상한은 기존 UI 필드 재사용, 하한은 신규 필드(요청: "하한도 상한과
|
||
# 동일하게 적용").
|
||
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 config.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", config.ROUTE_DEFAULT_GRADE_CLASS)
|
||
min_curve_radius_m = config.FOREST_ROAD_MIN_CURVE_R_M.get(grade_class, 12.0)
|
||
min_curve_radius_m = float(min_curve_radius_m)
|
||
endpoint_free_m = float(config.SKELETON_NODE_SPACING_M) * 1.5
|
||
|
||
# FP 는 항상 통과 불가, AP 도 기본 통과 불가 (옵션 허용 시 제외)
|
||
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(config.SKELETON_NODE_SPACING_M))
|
||
adj = _build_edges(
|
||
pos, kind, grid, barrier, blocked,
|
||
min_grade, max_grade, tol, endpoint_free_m,
|
||
)
|
||
|
||
# BP→CP…→EP 순차 탐색
|
||
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)
|
||
)
|
||
|
||
# ---- 이하 지표 산출 (기존 solver 와 동일 스키마) ----
|
||
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: # fillet 표본화 오차 여유
|
||
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
|
||
|
||
# 세그먼트(BP→CP→EP) 경계: 노드 마크 좌표에 가장 가까운 최종 정점으로 매핑
|
||
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 = config.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):
|
||
L = math.hypot(all_nodes[i + 1][0] - all_nodes[i][0],
|
||
all_nodes[i + 1][1] - all_nodes[i][1])
|
||
if L > 0.01:
|
||
constant_grade_segments.append({
|
||
"index": i,
|
||
"length_m": round(L, 2),
|
||
"grade_pct": round((all_nodes[i + 1][2] - all_nodes[i][2]) / L * 100, 2),
|
||
})
|
||
|
||
conditions_snapshot = {
|
||
"filter": filter_key,
|
||
"method": method,
|
||
"smooth": smooth,
|
||
"algorithm": "ridge_valley",
|
||
"grade_class": options.get("grade_class", config.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),
|
||
},
|
||
}
|