eomsangdonandClaude Opus 5.5
394fb7ba60
Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01Se7ytqbohmBQnfYpwYFnYf
111 lines
5.1 KiB
Python
111 lines
5.1 KiB
Python
"""추천 노선 큰 R — 2단(PLAN 328-2 · 요구 224-4 「R 은 클수록 좋음」).
|
|
|
|
`B05_Profile_Engine_RouteInitial_Pool._trace` 가 부름. 예상노선 추적(전처리 초기값)은 안 거침.
|
|
|
|
1단(`_trace`)이 예정노선 규칙(단순화 · 같은 방향 묶음 · R 맞춤)으로 짠 선을 받아, 곡선마다 R 을
|
|
[맞춘 R, 접선 자리 ÷ tan(IA/2)] 안에서 탐색 경로 점과 가장 먼 거리 ≤ `ROUTE_RECOMMEND_MAX_GAP_M`
|
|
인 가장 큰 값(1 m 내림)으로 고른다. 맞는 값이 없으면 맞춘 R 그대로.
|
|
|
|
고른 R 은 편집 갈래(`simplify=False` · `radii`)로 못박아 다시 짠다 — 화면이 곡선을 교각점 하나로
|
|
펴서(`flattenServerPlan`) [확인]이 다시 짜는 것과 같은 입력이라 저장 뒤 R 이 응답 R 과 같다.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
|
|
import numpy as np
|
|
from shapely.geometry import LineString, Point
|
|
|
|
from common_util.common_util_route_polyline import (
|
|
PlannedPolyline,
|
|
_inner_angle_deg,
|
|
build_planned_polyline,
|
|
)
|
|
from config.config_system_design import ROUTE_RECOMMEND_MAX_GAP_M
|
|
|
|
Xy = tuple[float, float]
|
|
|
|
|
|
def _flat(outline: PlannedPolyline) -> tuple[list[Xy], list[bool], list[float | None]]:
|
|
"""곡선 묶음을 교각점 하나로 편 노드 · 곡선 켬 · R — 화면 `flattenServerPlan` 과 같은 꼴."""
|
|
starts = {curve.node_first: curve for curve in outline.curves}
|
|
inside = {i for c in outline.curves for i in range(c.node_first + 1, c.node_last + 1)}
|
|
points: list[Xy] = []
|
|
flags: list[bool] = []
|
|
radii: list[float | None] = []
|
|
for index, node in enumerate(outline.nodes):
|
|
if index in inside:
|
|
continue
|
|
curve = starts.get(index)
|
|
points.append(curve.apex if curve else (node.x, node.y))
|
|
flags.append(curve is not None)
|
|
radii.append(round(curve.radius_m, 2) if curve else None)
|
|
return points, flags, radii
|
|
|
|
|
|
def _cross(u: np.ndarray, v: np.ndarray) -> np.ndarray:
|
|
return u[..., 0] * v[..., 1] - u[..., 1] * v[..., 0]
|
|
|
|
|
|
def _gap(samples: np.ndarray, before: Xy, apex: Xy, after: Xy, radii: np.ndarray) -> np.ndarray:
|
|
"""R 마다 점들이 「직선 · 원호 · 직선」(앞뒤 직선은 절반까지)에서 가장 먼 거리(m)."""
|
|
a = np.asarray(apex, dtype=float)
|
|
p = samples - a # (S, 2) 교각점 기준
|
|
units = [(np.asarray(end, dtype=float) - a) for end in (before, after)]
|
|
halves = [float(np.hypot(*u)) / 2 for u in units]
|
|
units = [u / (2 * h) for u, h in zip(units, halves)]
|
|
inner = math.radians(_inner_angle_deg(before, apex, after))
|
|
tangent = radii[:, None] * math.tan((math.pi - inner) / 2) # (M, 1)
|
|
legs = []
|
|
for unit, half in zip(units, halves):
|
|
along = p @ unit
|
|
side = np.abs(_cross(unit, p))
|
|
legs.append(np.hypot(side, np.maximum(0.0, np.maximum(tangent - along, along - half))))
|
|
bisector = (units[0] + units[1]) / np.hypot(*(units[0] + units[1]))
|
|
center = (radii / math.sin(inner / 2))[:, None, None] * bisector # (M, 1, 2)
|
|
starts = [tangent[..., None] * unit - center for unit in units] # 중심 → 접선점
|
|
q = p[None] - center # (M, S, 2) 중심 → 점
|
|
turn = _cross(starts[0], starts[1])
|
|
within = (_cross(starts[0], q) * turn >= 0) & (_cross(q, starts[1]) * turn >= 0)
|
|
arc = np.where(within, np.abs(np.hypot(q[..., 0], q[..., 1]) - radii[:, None]), np.inf)
|
|
return np.minimum(np.minimum(legs[0], legs[1]), arc).max(axis=1)
|
|
|
|
|
|
def widen(
|
|
outline: PlannedPolyline,
|
|
path: list[Xy],
|
|
min_radius_m: float,
|
|
max_gap_m: float = ROUTE_RECOMMEND_MAX_GAP_M,
|
|
) -> PlannedPolyline:
|
|
"""곡선마다 허용 이탈 안 가장 큰 R(1 m 내림)로 못박아 다시 짠 선. 곡선이 없으면 그대로."""
|
|
if not outline.curves:
|
|
return outline
|
|
points, flags, radii = _flat(outline)
|
|
line = LineString(path)
|
|
coords = np.asarray(path, dtype=float)
|
|
along = np.concatenate([[0.0], np.cumsum(np.hypot(*np.diff(coords, axis=0).T))])
|
|
for index in range(1, len(points) - 1):
|
|
if not flags[index]:
|
|
continue
|
|
before, apex, after = points[index - 1], points[index], points[index + 1]
|
|
half_tan = math.tan(math.radians(180.0 - _inner_angle_deg(before, apex, after)) / 2)
|
|
if half_tan <= 1e-9:
|
|
continue
|
|
room = min(math.dist(before, apex), math.dist(apex, after)) / 2 / half_tan
|
|
candidates = np.arange(math.floor(room), float(radii[index]), -1.0) # 큰 것부터
|
|
# 이 곡선이 쓰는 자리 = 앞뒤 직선 가운데점 사이 — 그 사이 탐색 경로 점만 잼
|
|
ends = [
|
|
line.project(Point((apex[0] + end[0]) / 2, (apex[1] + end[1]) / 2))
|
|
for end in (before, after)
|
|
]
|
|
samples = coords[(along >= min(ends)) & (along <= max(ends))]
|
|
if not candidates.size or not len(samples):
|
|
continue
|
|
ok = candidates[_gap(samples, before, apex, after, candidates) <= max_gap_m]
|
|
if ok.size:
|
|
radii[index] = float(ok[0])
|
|
return build_planned_polyline(
|
|
points, min_radius_m=min_radius_m, simplify=False, curve_flags=flags, radii=radii
|
|
)
|