265 lines
9.0 KiB
Python
265 lines
9.0 KiB
Python
"""B05 경로 설계 기하 유틸리티 및 단일 구간 Dijkstra.
|
|
|
|
8-연결 격자에서 종단경사·측사면·곡률을 비용으로 반영하는 방향 인식
|
|
상태공간 Dijkstra와, 곡률/거리/교차 계산 등 순수 기하 헬퍼를 제공한다.
|
|
config에 독립적이며 모든 파라미터는 호출측에서 주입한다.
|
|
"""
|
|
|
|
import heapq
|
|
import math
|
|
from typing import Any
|
|
|
|
import numpy as np
|
|
|
|
|
|
def get_neighbors(r: int, c: int, rows: int, cols: int):
|
|
"""8-연결 이웃을 (nr, nc, dir_idx)로 순회한다. (방향 인덱스 0..7)"""
|
|
neighbors = [
|
|
(-1, 0, 4), # S
|
|
(-1, 1, 3), # SE
|
|
(0, 1, 2), # E
|
|
(1, 1, 1), # NE
|
|
(1, 0, 0), # N
|
|
(1, -1, 7), # NW
|
|
(0, -1, 6), # W
|
|
(-1, -1, 5), # SW
|
|
]
|
|
for dr, dc, d_idx in neighbors:
|
|
nr, nc = r + dr, c + dc
|
|
if 0 <= nr < rows and 0 <= nc < cols:
|
|
yield nr, nc, d_idx
|
|
|
|
|
|
def compute_gradients(
|
|
z_grid: np.ndarray, res_y: float, res_x: float | None = None
|
|
) -> tuple[np.ndarray, np.ndarray]:
|
|
"""실제 격자 간격으로 지형 기울기를 계산해 (dz_dx, dz_dy)를 반환한다."""
|
|
if res_x is None:
|
|
res_x = res_y
|
|
dz_dy, dz_dx = np.gradient(z_grid, res_y, res_x)
|
|
return dz_dx, dz_dy
|
|
|
|
|
|
def side_slope_magnitude(dr: int, dc: int, gx: float, gy: float) -> float:
|
|
"""진행 방향에 수직인 지형 기울기(측사면=절토/성토 프록시)의 크기."""
|
|
norm = math.hypot(dc, dr)
|
|
if norm == 0:
|
|
return 0.0
|
|
px, py = -dr / norm, dc / norm
|
|
return abs(gx * px + gy * py)
|
|
|
|
|
|
def circumradius_2d(p0, p1, p2) -> float:
|
|
"""연속 세 점을 지나는 수평면 외접원 반지름(중간 정점의 이산 곡률 반경)."""
|
|
ax, ay = p0[0], p0[1]
|
|
bx, by = p1[0], p1[1]
|
|
cx, cy = p2[0], p2[1]
|
|
a = math.hypot(bx - cx, by - cy)
|
|
b = math.hypot(ax - cx, ay - cy)
|
|
c = math.hypot(ax - bx, ay - by)
|
|
if a < 0.01 or b < 0.01 or c < 0.01:
|
|
return float("inf")
|
|
area2 = abs((bx - ax) * (cy - ay) - (cx - ax) * (by - ay))
|
|
if area2 < 1e-9:
|
|
return float("inf")
|
|
return (a * b * c) / (2.0 * area2)
|
|
|
|
|
|
def point_to_polyline_dist_2d(px: float, py: float, polyline) -> float:
|
|
"""점에서 폴리라인까지 최소 수평거리(점-선분 거리)."""
|
|
if not polyline:
|
|
return float("inf")
|
|
best = float("inf")
|
|
for i in range(len(polyline) - 1):
|
|
ax, ay = polyline[i][0], polyline[i][1]
|
|
bx, by = polyline[i + 1][0], polyline[i + 1][1]
|
|
dx, dy = bx - ax, by - ay
|
|
seg2 = dx * dx + dy * dy
|
|
if seg2 <= 1e-12:
|
|
d = math.hypot(px - ax, py - ay)
|
|
else:
|
|
t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / seg2))
|
|
d = math.hypot(px - (ax + t * dx), py - (ay + t * dy))
|
|
if d < best:
|
|
best = d
|
|
if len(polyline) == 1:
|
|
best = math.hypot(px - polyline[0][0], py - polyline[0][1])
|
|
return best
|
|
|
|
|
|
def circle_intrusions(polyline, circles, labeler) -> list[dict[str, Any]]:
|
|
"""각 원(AP/FP)에 대해 경로의 최소 이격거리와 원 내부 폴리라인 길이를 보고한다."""
|
|
out = []
|
|
for i, circ in enumerate(circles):
|
|
cx, cy, radius = circ["x"], circ["y"], circ["radius_m"]
|
|
clearance = point_to_polyline_dist_2d(cx, cy, polyline)
|
|
intruded_len = 0.0
|
|
for j in range(len(polyline) - 1):
|
|
ax, ay = polyline[j][0], polyline[j][1]
|
|
bx, by = polyline[j + 1][0], polyline[j + 1][1]
|
|
mx, my = 0.5 * (ax + bx), 0.5 * (ay + by)
|
|
if math.hypot(mx - cx, my - cy) < radius:
|
|
intruded_len += math.hypot(bx - ax, by - ay)
|
|
out.append(
|
|
{
|
|
"index": i,
|
|
"label": labeler(i),
|
|
"x": cx,
|
|
"y": cy,
|
|
"radius_m": radius,
|
|
"min_clearance_m": round(clearance, 3),
|
|
"intrusion_length_m": round(intruded_len, 3),
|
|
"intrudes": bool(clearance < radius or intruded_len > 0.0),
|
|
}
|
|
)
|
|
return out
|
|
|
|
|
|
def resample_polyline_2d(polyline, chainage_m, step_m: float):
|
|
"""폴리라인을 고정 호장 간격으로 재샘플한다(도로 스케일 곡률 측정용)."""
|
|
if len(polyline) < 2 or step_m <= 0:
|
|
return [[p[0], p[1]] for p in polyline]
|
|
total = chainage_m[-1]
|
|
if total <= 0:
|
|
return [[polyline[0][0], polyline[0][1]]]
|
|
targets = np.arange(0.0, total + step_m * 0.5, step_m)
|
|
out = []
|
|
j = 0
|
|
for t in targets:
|
|
while j < len(chainage_m) - 2 and chainage_m[j + 1] < t:
|
|
j += 1
|
|
seg_len = chainage_m[j + 1] - chainage_m[j]
|
|
frac = 0.0 if seg_len <= 1e-9 else (t - chainage_m[j]) / seg_len
|
|
frac = min(max(frac, 0.0), 1.0)
|
|
x = polyline[j][0] + frac * (polyline[j + 1][0] - polyline[j][0])
|
|
y = polyline[j][1] + frac * (polyline[j + 1][1] - polyline[j][1])
|
|
out.append([x, y])
|
|
return out
|
|
|
|
|
|
def turn_radius_from_grid(diff: int, grid_res: float) -> float:
|
|
"""8-연결 격자의 단일 방향전환이 함의하는 곡선 반경(m) 근사."""
|
|
if diff <= 0:
|
|
return float("inf")
|
|
angle_rad = math.radians(45.0 * diff)
|
|
step_len = grid_res * (math.sqrt(2) if diff == 1 else 1.0)
|
|
return step_len / angle_rad
|
|
|
|
|
|
def single_segment_dijkstra(
|
|
r_start: int,
|
|
c_start: int,
|
|
r_end: int,
|
|
c_end: int,
|
|
x_coords: np.ndarray,
|
|
y_coords: np.ndarray,
|
|
z_grid: np.ndarray,
|
|
valid_mask: np.ndarray,
|
|
dz_dx: np.ndarray,
|
|
dz_dy: np.ndarray,
|
|
ap_list: list[dict[str, Any]],
|
|
weights: dict[str, float],
|
|
max_grade: float,
|
|
grid_res: float,
|
|
min_curve_radius_m: float,
|
|
max_uphill_grade: float | None = None,
|
|
max_downhill_grade: float | None = None,
|
|
) -> list[tuple[int, int]]:
|
|
"""시작→끝 셀 방향 인식 상태공간 Dijkstra 탐색.
|
|
|
|
하드 제약: 종단경사 초과 링크·135도 이상 급전환은 통행불가.
|
|
min_curve_radius_m은 방향전환 소프트 패널티로 반영된다.
|
|
"""
|
|
rows, cols = z_grid.shape
|
|
w_dist = weights.get("dist", 1.0)
|
|
w_grade = weights.get("grade", 2.0)
|
|
w_side = weights.get("side", 1.5)
|
|
w_curve = weights.get("curve", 0.5)
|
|
w_avoid = weights.get("avoid", 10.0)
|
|
|
|
up_limit = max_uphill_grade if max_uphill_grade else max_grade
|
|
down_limit = max_downhill_grade if max_downhill_grade else max_grade
|
|
|
|
dist: dict[tuple[int, int, int], float] = {}
|
|
parent: dict[tuple[int, int, int], tuple[int, int, int]] = {}
|
|
pq: list[tuple[float, int, int, int]] = []
|
|
|
|
for d in range(8):
|
|
dist[(r_start, c_start, d)] = 0.0
|
|
heapq.heappush(pq, (0.0, r_start, c_start, d))
|
|
|
|
found_dest = False
|
|
best_dest_state = None
|
|
|
|
while pq:
|
|
d_cost, r, c, d = heapq.heappop(pq)
|
|
if d_cost > dist.get((r, c, d), float("inf")):
|
|
continue
|
|
if r == r_end and c == c_end:
|
|
found_dest = True
|
|
best_dest_state = (r, c, d)
|
|
break
|
|
|
|
for nr, nc, nd in get_neighbors(r, c, rows, cols):
|
|
if not valid_mask[nr, nc]:
|
|
continue
|
|
|
|
diff = abs(d - nd)
|
|
diff = min(diff, 8 - diff)
|
|
if diff >= 3:
|
|
continue # U턴/급전환은 통행불가
|
|
|
|
curve_penalty = 0.0
|
|
if diff > 0:
|
|
turn_radius = turn_radius_from_grid(diff, grid_res)
|
|
tightness = min(min_curve_radius_m / turn_radius, 20.0)
|
|
curve_penalty = w_curve * tightness * grid_res
|
|
|
|
is_diagonal = nd % 2 != 0
|
|
h_dist = grid_res * math.sqrt(2) if is_diagonal else grid_res
|
|
|
|
z_curr = z_grid[r, c]
|
|
z_next = z_grid[nr, nc]
|
|
dz = z_next - z_curr
|
|
grade = abs(dz) / h_dist
|
|
|
|
applicable_limit = up_limit if dz > 0 else down_limit
|
|
if grade > applicable_limit:
|
|
continue
|
|
f_grade = (grade / applicable_limit) ** 2 * 10.0
|
|
|
|
side_slope = side_slope_magnitude(nr - r, nc - c, dz_dx[nr, nc], dz_dy[nr, nc])
|
|
f_side = side_slope * 5.0
|
|
|
|
nx_model = x_coords[nc]
|
|
ny_model = y_coords[nr]
|
|
avoid_penalty = 0.0
|
|
for ap in ap_list:
|
|
dist_to_ap = math.hypot(nx_model - ap["x"], ny_model - ap["y"])
|
|
if dist_to_ap < ap["radius_m"]:
|
|
avoid_penalty += w_avoid * 1000.0 * h_dist
|
|
|
|
link_cost = (
|
|
w_dist * h_dist
|
|
+ w_grade * f_grade * h_dist
|
|
+ w_side * f_side * h_dist
|
|
+ curve_penalty
|
|
+ avoid_penalty
|
|
)
|
|
next_cost = d_cost + link_cost
|
|
state_next = (nr, nc, nd)
|
|
if next_cost < dist.get(state_next, float("inf")):
|
|
dist[state_next] = next_cost
|
|
parent[state_next] = (r, c, d)
|
|
heapq.heappush(pq, (next_cost, nr, nc, nd))
|
|
|
|
if not found_dest:
|
|
return []
|
|
|
|
path = []
|
|
curr = best_dest_state
|
|
while curr:
|
|
path.append((curr[0], curr[1]))
|
|
curr = parent.get(curr)
|
|
return path[::-1]
|