"""B05 최적 경로 탐색 오케스트레이터. 확정된 지표면 모델(B04_wf1_Surface/models)의 표고를 DTM 격자에 샘플링해 비용면을 만들고(캐시 재사용), BP→CP…→EP 다구간 Dijkstra로 최적 경로를 계산한 뒤 평활·측점·곡률·제약검증 메트릭을 반환한다. """ import math from pathlib import Path from typing import Any import numpy as np from B05_wf2_Route.B05_wf2_Route_Engine_Geometry import ( circle_intrusions, circumradius_2d, compute_gradients, point_to_polyline_dist_2d, resample_polyline_2d, single_segment_dijkstra, ) from config.config_system import ( FOREST_ROAD_MAX_GRADE, FOREST_ROAD_MIN_CURVE_R_M, ROUTE_DEFAULT_GRADE_CLASS, ROUTE_GRID_RES_M, ROUTE_MAX_COST_CELLS, ROUTE_MAX_GRADE, ROUTE_MAX_GRADE_PAVED, ROUTE_REQUIRED_POINT_TOLERANCE_M, ROUTE_W_AVOID, ROUTE_W_CURVE, ROUTE_W_DIST, ROUTE_W_GRADE, ROUTE_W_SIDE, ROUTE_WEIGHT_MAX, ) # B04 지표면 모델 폴더명 (비용면의 표고 원본) _MODELS_SUBDIR = Path("B04_wf1_Surface") / "models" # B05 비용면 캐시 폴더명 _ROUTE_CACHE_SUBDIR = Path("B05_wf2_Route") / "route" def _load_dtm_grid(models_dir: Path, filter_key: str, smooth: bool): """필터의 정규 DTM 격자(x, y, z, valid_mask)를 로드한다.""" suffix = "_smooth" if smooth else "" dtm_path = models_dir / f"dtm_{filter_key}{suffix}.npz" if not dtm_path.exists(): dtm_path = models_dir / f"dtm_{filter_key}.npz" if not dtm_path.exists(): raise FileNotFoundError(f"DTM 파일을 찾을 수 없습니다: dtm_{filter_key}{suffix}.npz") d = np.load(dtm_path) return ( np.asarray(d["x"]), np.asarray(d["y"]), np.asarray(d["z"], dtype=np.float64), np.asarray(d["valid_mask"]), ) def _sample_surface_on_grid( models_dir: Path, filter_key: str, method: str, smooth: bool, x_coords: np.ndarray, y_coords: np.ndarray, dtm_z: np.ndarray, ) -> np.ndarray: """확정 지표면 모델의 표고를 DTM 격자에 샘플링한다(실패 시 DTM으로 폴백).""" if method == "dtm": return dtm_z models_dir = Path(models_dir) xx, yy = np.meshgrid(x_coords, y_coords) query = np.column_stack([xx.ravel(), yy.ravel()]) def _finalize(z_flat: np.ndarray) -> np.ndarray: z = np.asarray(z_flat, dtype=np.float64).reshape(len(y_coords), len(x_coords)) bad = ~np.isfinite(z) if bad.any(): z[bad] = dtm_z[bad] return z try: suffix = "_smooth" if smooth else "" if method == "tin": from scipy.interpolate import LinearNDInterpolator path = models_dir / f"tin_{filter_key}{suffix}.npz" if not path.exists(): path = models_dir / f"tin_{filter_key}.npz" d = np.load(path) verts = np.asarray(d["vertices"], dtype=np.float64) interp = LinearNDInterpolator(verts[:, :2], verts[:, 2]) return _finalize(interp(query)) if method == "nurbs": from scipy.interpolate import RectBivariateSpline d = np.load(models_dir / f"nurbs_{filter_key}.npz") cx = np.asarray(d["control_x"], dtype=np.float64) cy = np.asarray(d["control_y"], dtype=np.float64) cz = np.asarray(d["control_z"], dtype=np.float64) degree = int(d["degree"][0]) if "degree" in d else 3 spline = RectBivariateSpline( cy, cx, cz, kx=min(degree, len(cy) - 1), ky=min(degree, len(cx) - 1) ) return _finalize(spline(y_coords, x_coords).ravel()) if method == "implicit": from scipy.interpolate import RBFInterpolator d = np.load(models_dir / f"implicit_{filter_key}.npz") centers = np.asarray(d["centers_xy"], dtype=np.float64) cz = np.asarray(d["center_z"], dtype=np.float64) smoothing = float(d["smoothing"][0]) if "smoothing" in d else 0.0 interp = RBFInterpolator( centers, cz, neighbors=min(64, len(centers)), smoothing=smoothing, kernel="thin_plate_spline", ) out = np.empty(len(query), dtype=np.float64) for s in range(0, len(query), 50_000): e = min(s + 50_000, len(query)) out[s:e] = interp(query[s:e]) return _finalize(out) if method == "meshfree": from scipy.interpolate import griddata d = np.load(models_dir / f"meshfree_{filter_key}.npz") pts = np.asarray(d["points"], dtype=np.float64) z = griddata(pts[:, :2], pts[:, 2], query, method="linear") return _finalize(z) except Exception: return dtm_z return dtm_z def _source_npz_paths(models_dir: Path, filter_key: str, method: str, smooth: bool) -> list[Path]: """비용면이 의존하는 소스 모델 파일(존재하는 것만, 안정 순서).""" suffix = "_smooth" if smooth else "" candidates = [ models_dir / f"dtm_{filter_key}{suffix}.npz", models_dir / f"dtm_{filter_key}.npz", ] if method != "dtm": candidates.append(models_dir / f"{method}_{filter_key}{suffix}.npz") candidates.append(models_dir / f"{method}_{filter_key}.npz") seen: set[Path] = set() out: list[Path] = [] for p in candidates: if p.exists() and p not in seen: seen.add(p) out.append(p) return out def _cost_surface_signature(models_dir: Path, filter_key: str, method: str, smooth: bool) -> str: """비용면 재빌드 필요 시 바뀌는 서명(격자해상도 + 소스파일 mtime/size).""" parts = [f"res={ROUTE_GRID_RES_M}", f"method={method}", f"smooth={smooth}"] for p in _source_npz_paths(models_dir, filter_key, method, smooth): st = p.stat() parts.append(f"{p.name}:{int(st.st_mtime)}:{st.st_size}") return "|".join(parts) def _build_cost_surface(models_dir: Path, filter_key: str, method: str, smooth: bool): """다운샘플된 비용면(좌표·표고·footprint·기울기)을 만든다.""" x_full, y_full, dtm_z_full, valid_full = _load_dtm_grid(models_dir, filter_key, smooth) target_res = ROUTE_GRID_RES_M src_res = (x_full[-1] - x_full[0]) / (len(x_full) - 1) step = max(1, int(round(target_res / src_res))) x_sub = np.ascontiguousarray(x_full[::step]) y_sub = np.ascontiguousarray(y_full[::step]) n_cells = len(x_sub) * len(y_sub) if n_cells > ROUTE_MAX_COST_CELLS: raise ValueError( f"경로 비용면 격자 셀 수({n_cells:,})가 한도({ROUTE_MAX_COST_CELLS:,})를 " f"초과합니다. ROUTE_GRID_RES_M({target_res} m)를 키우거나 영역을 줄이세요." ) dtm_z_sub = np.array(dtm_z_full[::step, ::step], dtype=np.float64) valid_sub = np.array(valid_full[::step, ::step]) del dtm_z_full, valid_full z_sub = _sample_surface_on_grid(models_dir, filter_key, method, smooth, x_sub, y_sub, dtm_z_sub) z_sub = np.asarray(z_sub, dtype=np.float64) if not np.all(np.isfinite(z_sub)): finite = z_sub[np.isfinite(z_sub)] fill = float(finite.mean()) if finite.size else 0.0 z_sub = np.where(np.isfinite(z_sub), z_sub, fill) res_y = (y_sub[-1] - y_sub[0]) / (len(y_sub) - 1) if len(y_sub) > 1 else target_res res_x = (x_sub[-1] - x_sub[0]) / (len(x_sub) - 1) if len(x_sub) > 1 else target_res dz_dx, dz_dy = compute_gradients(z_sub, res_y, res_x) grid_res = float(0.5 * (res_x + res_y)) return x_sub, y_sub, z_sub, valid_sub, dz_dx, dz_dy, grid_res def _load_or_build_cost_surface( project_root: Path, models_dir: Path, filter_key: str, method: str, smooth: bool ): """비용면을 반환한다(서명 일치 시 캐시 재사용, 아니면 재빌드·재캐시).""" cache_dir = project_root / _ROUTE_CACHE_SUBDIR suffix = "_smooth" if smooth else "" cache_path = cache_dir / f"cost_surface_{filter_key}_{method}{suffix}.npz" signature = _cost_surface_signature(models_dir, filter_key, method, smooth) if cache_path.exists(): try: cached = np.load(cache_path, allow_pickle=False) if str(cached["signature"]) == signature: return ( cached["x"], cached["y"], cached["z"], cached["valid_mask"], cached["dz_dx"], cached["dz_dy"], float(cached["target_res"][0]), ) except Exception: pass surface = _build_cost_surface(models_dir, filter_key, method, smooth) x_sub, y_sub, z_sub, valid_sub, dz_dx, dz_dy, target_res = surface try: cache_dir.mkdir(parents=True, exist_ok=True) np.savez_compressed( cache_path, x=x_sub, y=y_sub, z=z_sub, valid_mask=valid_sub, dz_dx=dz_dx, dz_dy=dz_dy, target_res=np.array([target_res], np.float64), signature=np.array(signature), ) except Exception: pass return surface def _empty_route_result() -> dict[str, Any]: return { "polyline": [], "chainage_m": [], "segments": [], "required_point_checks": [], "required_points_ok": False, "avoid_intrusions": [], "forbidden_intrusions": [], "curve_warning_segments": [], "avoid_retry_performed": False, "conditions_snapshot": {}, "metrics": { "length_m": 0.0, "avg_grade_pct": 0.0, "max_grade_pct": 0.0, "slope_violations": 0, "curve_violations": 0, "min_curve_radius_m": None, "min_curve_radius_limit_m": 0.0, }, } def solve_optimal_route( project_root: Path, filter_key: str, smooth: bool, points_data: dict[str, Any], options: dict[str, Any], method: str = "dtm", _avoid_retry: bool = False, ) -> dict[str, Any]: """다구간 비용면 경로 탐색을 오케스트레이션해 최종 좌표·메트릭을 반환한다.""" project_root = Path(project_root) models_dir = project_root / _MODELS_SUBDIR ( x_coords_sub, y_coords_sub, z_grid_sub, valid_mask_sub, dz_dx, dz_dy, target_res, ) = _load_or_build_cost_surface(project_root, models_dir, filter_key, method, smooth) 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 fp_list: valid_mask_sub = np.array(valid_mask_sub, copy=True) xx, yy = np.meshgrid(x_coords_sub, y_coords_sub) for fp in fp_list: inside = (xx - fp["x"]) ** 2 + (yy - fp["y"]) ** 2 < float(fp["radius_m"]) ** 2 valid_mask_sub[inside] = False if not bp or not ep: return _empty_route_result() sequence = [bp] + cp_list + [ep] def _nearest_coord_index(coords: np.ndarray, value: float) -> int: upper = int(np.clip(np.searchsorted(coords, value), 0, len(coords) - 1)) lower = max(upper - 1, 0) return lower if abs(value - coords[lower]) <= abs(coords[upper] - value) else upper def get_grid_indices(pt: dict[str, float]) -> tuple[int, int, bool]: c = _nearest_coord_index(x_coords_sub, pt["x"]) r = _nearest_coord_index(y_coords_sub, pt["y"]) in_bounds = float(x_coords_sub[0]) <= pt["x"] <= float(x_coords_sub[-1]) and float( y_coords_sub[0] ) <= pt["y"] <= float(y_coords_sub[-1]) point_on_valid_terrain = in_bounds and bool(valid_mask_sub[r, c]) if not point_on_valid_terrain: valid_ys, valid_xs = np.where(valid_mask_sub) if len(valid_ys) > 0: dists = (valid_ys - r) ** 2 + (valid_xs - c) ** 2 best_idx = np.argmin(dists) r, c = int(valid_ys[best_idx]), int(valid_xs[best_idx]) return r, c, point_on_valid_terrain tol_req = ROUTE_REQUIRED_POINT_TOLERANCE_M required_snap = [] for pt in sequence: r, c, point_on_valid_terrain = get_grid_indices(pt) sx, sy = float(x_coords_sub[c]), float(y_coords_sub[r]) snap_dist = math.hypot(pt["x"] - sx, pt["y"] - sy) required_snap.append( { "r": r, "c": c, "snap_x": sx, "snap_y": sy, "snap_dist": snap_dist, "point_on_valid_terrain": point_on_valid_terrain, } ) weights = options.get("weights") or { "dist": ROUTE_W_DIST, "grade": ROUTE_W_GRADE, "side": ROUTE_W_SIDE, "curve": ROUTE_W_CURVE, "avoid": ROUTE_W_AVOID, } paved = options.get("paved", False) grade_class = options.get("grade_class", ROUTE_DEFAULT_GRADE_CLASS) base_max_grade = FOREST_ROAD_MAX_GRADE.get(grade_class, ROUTE_MAX_GRADE) max_grade = max(base_max_grade, ROUTE_MAX_GRADE_PAVED) if paved else base_max_grade min_curve_radius_m = options.get("min_curve_radius_m") if not min_curve_radius_m or min_curve_radius_m <= 0: min_curve_radius_m = FOREST_ROAD_MIN_CURVE_R_M.get(grade_class, 12.0) def _grade_opt(key: str) -> float: v = options.get(key) return float(v) if (v is not None and float(v) > 0) else max_grade max_uphill_grade = _grade_opt("max_uphill_grade") max_downhill_grade = _grade_opt("max_downhill_grade") def _point_label(idx: int, pt: dict[str, Any]) -> str: if idx == 0: return "BP" if idx == len(sequence) - 1: return "EP" return f"CP{pt.get('order', idx)}" full_path_grid: list[tuple[int, int]] = [] segment_bounds: list[dict[str, Any]] = [] for i in range(len(sequence) - 1): pt_start = sequence[i] pt_end = sequence[i + 1] r_s, c_s, _ = get_grid_indices(pt_start) r_e, c_e, _ = get_grid_indices(pt_end) segment = single_segment_dijkstra( r_s, c_s, r_e, c_e, x_coords_sub, y_coords_sub, z_grid_sub, valid_mask_sub, dz_dx, dz_dy, ap_list, weights, max_grade, target_res, min_curve_radius_m, max_uphill_grade, max_downhill_grade, ) if not segment: fp_note = "·금지구역(FP)" if fp_list else "" raise ValueError( f"세그먼트 {i + 1} ({_point_label(i, pt_start)} -> {_point_label(i + 1, pt_end)}) " f"경로 탐색 실패: 종단경사 한계({max_grade * 100:.0f}%)·최소곡선반지름" f"({min_curve_radius_m:.0f}m)·회피지역{fp_note} 제약으로 통과 경로가 없습니다." ) start_idx = max(len(full_path_grid) - 1, 0) if i > 0 and len(segment) > 0: full_path_grid.extend(segment[1:]) else: full_path_grid.extend(segment) segment_bounds.append( { "index": i, "from": _point_label(i, pt_start), "to": _point_label(i + 1, pt_end), "point_start": start_idx, "point_end": len(full_path_grid) - 1, } ) polyline = [] for r, c in full_path_grid: polyline.append([float(x_coords_sub[c]), float(y_coords_sub[r]), float(z_grid_sub[r, c])]) def _grid_z(px: float, py: float) -> float: c_idx = _nearest_coord_index(x_coords_sub, px) r_idx = _nearest_coord_index(y_coords_sub, py) return float(z_grid_sub[r_idx, c_idx]) def _pin_coord_for(seq_idx: int) -> list[float]: snap = required_snap[seq_idx] pt = sequence[seq_idx] if snap["point_on_valid_terrain"]: return [pt["x"], pt["y"], _grid_z(pt["x"], pt["y"])] return [snap["snap_x"], snap["snap_y"], _grid_z(snap["snap_x"], snap["snap_y"])] pin_coords: dict[int, list[float]] = {} for sb in segment_bounds: pin_coords[sb["point_start"]] = _pin_coord_for(sb["index"]) pin_coords[sb["point_end"]] = _pin_coord_for(sb["index"] + 1) if len(polyline) > 4: original = polyline smoothed_polyline = [] window_size = 3 padded = [original[0]] * (window_size // 2) + original + [original[-1]] * (window_size // 2) for i in range(len(original)): if i in pin_coords: smoothed_polyline.append(list(pin_coords[i])) continue window = padded[i : i + window_size] sx = sum(p[0] for p in window) / window_size sy = sum(p[1] for p in window) / window_size sz = sum(p[2] for p in window) / window_size smoothed_polyline.append([sx, sy, sz]) polyline = smoothed_polyline else: for i, coord in pin_coords.items(): if 0 <= i < len(polyline): polyline[i] = list(coord) n = len(polyline) chainage_m = [0.0] * n length_m = 0.0 slope_violations = 0 max_grade_pct = 0.0 grade_sums = 0.0 max_uphill_pct = 0.0 max_downhill_pct = 0.0 for i in range(n - 1): x1, y1, z1 = polyline[i] x2, y2, z2 = polyline[i + 1] h_dist = math.hypot(x2 - x1, y2 - y1) chainage_m[i + 1] = chainage_m[i] + h_dist if h_dist > 0.01: dz = z2 - z1 segment_slope = abs(dz) / h_dist length_m += h_dist grade_sums += segment_slope * h_dist max_grade_pct = max(max_grade_pct, segment_slope) applicable = max_uphill_grade if dz > 0 else max_downhill_grade if dz > 0: max_uphill_pct = max(max_uphill_pct, segment_slope) else: max_downhill_pct = max(max_downhill_pct, segment_slope) if segment_slope > applicable: slope_violations += 1 avg_grade_pct = (grade_sums / length_m) if length_m > 0 else 0.0 curve_check_step = max(2.0 * target_res, 4.0) resampled = resample_polyline_2d(polyline, chainage_m, curve_check_step) total_chainage = chainage_m[-1] if chainage_m else 0.0 def _poly_index_at_chainage(ch: float) -> int: idx = int(np.searchsorted(chainage_m, ch)) if chainage_m else 0 return int(min(max(idx, 0), max(len(polyline) - 1, 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: 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 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": _poly_index_at_chainage(ch_s), "polyline_end_index": _poly_index_at_chainage(ch_e), } ) run_start = None segments = [] for sb in segment_bounds: s, e = sb["point_start"], min(sb["point_end"], n - 1) 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": sb["index"], "from": sb["from"], "to": sb["to"], "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), } ) required_point_checks = [] for idx, pt in enumerate(sequence): label = _point_label(idx, pt) poly_dist = point_to_polyline_dist_2d(pt["x"], pt["y"], polyline) snap = required_snap[idx] snap_dist = snap["snap_dist"] dist_val = poly_dist if snap["point_on_valid_terrain"] else max(poly_dist, snap_dist) required_point_checks.append( { "point": label, "x": pt["x"], "y": pt["y"], "distance_m": round(dist_val, 3), "snap_distance_m": round(snap_dist, 3), "point_on_valid_terrain": snap["point_on_valid_terrain"], "tolerance_m": tol_req, "within_tolerance": bool(dist_val <= 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}") allow_pass = bool(options.get("allow_avoid_pass_through", False)) any_intrusion = any(a["intrudes"] for a in avoid_intrusions) if any_intrusion and not allow_pass and not _avoid_retry: boosted = dict(options) bw = dict(weights) bw["avoid"] = min(bw.get("avoid", ROUTE_W_AVOID) * 10.0, ROUTE_WEIGHT_MAX) boosted["weights"] = bw try: retried = solve_optimal_route( project_root, filter_key, smooth, points_data, boosted, method=method, _avoid_retry=True, ) retried["avoid_retry_performed"] = True return retried except Exception: pass conditions_snapshot = { "filter": filter_key, "method": method, "smooth": smooth, "grade_class": grade_class, "paved": paved, "max_grade_pct": round(max_grade * 100, 2), "max_uphill_grade_pct": round(max_uphill_grade * 100, 2), "max_downhill_grade_pct": round(max_downhill_grade * 100, 2), "min_curve_radius_m": round(min_curve_radius_m, 2), "weights": weights, "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": _avoid_retry, "conditions_snapshot": conditions_snapshot, "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), }, }