657 lines
24 KiB
Python
657 lines
24 KiB
Python
"""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),
|
|
},
|
|
}
|