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