260705_2
This commit is contained in:
@@ -0,0 +1,656 @@
|
||||
"""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),
|
||||
},
|
||||
}
|
||||
Reference in New Issue
Block a user