Aislo/common_util/common_util_route_polyline_geom.py

"""예상노선 → 폴리라인 — 점 다듬기 · 꺾임점 뽑기 · 원호 맞춤 바탕 (`common_util_route_polyline` 에서 나눔 · 밖에서는 `common_util_route_polyline` 로 부름)."""

from __future__ import annotations

import math


# 같은 자리로 볼 점 사이 거리(m) — 이보다 가까우면 뒤엣것을 버린다. 원본 점군에 중복·
# 미세 진동이 섞여 있으면 내각이 튀어 없는 곡선이 생긴다.
DUPLICATE_TOLERANCE_M = 0.5

# 꺾임점(IP)을 뽑는 단순화 허용오차(m). 예상노선은 격자 탐색이 낸 **조밀한 점군**이라
# (용화 실측: 1,097m 에 331점 = 약 3.3m 간격) 점마다 곡선을 끼우면 접선 자리가 1.5m 밖에
# 안 나와 반지름이 2~6m 로 뭉개진다. 격자 해상도(`ROUTE_GRID_RES_M` 2.0m)의 두 배로 잡아
# 계단 모양만 걷어내고 실제 굴곡은 남긴다.
SIMPLIFY_TOLERANCE_M = 4.0

# 남은 노드 사이가 이보다 멀면 그 구간만 더 촘촘히 다시 뽑는다(m).
#
# 왜 필요한가(2026-09-06 실측) — Douglas-Peucker 허용오차는 **절대 거리**라 굴곡이 완만하고
# 길수록 통째로 삼켜진다. 4.5km 짜리 S자 노선에서 노드가 20개(간격 238m)·곡선 2곳만 남아
# 「S자」가 사라졌다. 같은 4m 로 1.1km 노선은 노드 25개(간격 46m)·곡선 13곳으로 알맞았다.
# 노선 길이로 허용오차를 바꾸면 짧고 급한 굴곡이 다시 뭉개지므로, **간격이 벌어진 구간만**
# 골라 허용오차를 절반으로 낮춰 다시 뽑는다.
MAX_NODE_SPACING_M = 100.0

# 위 되뽑기를 몇 겹까지 할지 — 겹마다 허용오차가 절반이 된다(4 → 2 → 1 → 0.5m).
MAX_REFINE_DEPTH = 3

# 원호를 몇 도마다 한 점씩 찍을지 — 촘촘할수록 매끄럽지만 정점이 늘어난다.
ARC_STEP_DEG = 5.0


def _distance(a: tuple[float, float], b: tuple[float, float]) -> float:
    return math.hypot(b[0] - a[0], b[1] - a[1])


def dedupe_points(
    points: list[tuple[float, float]], tolerance_m: float = DUPLICATE_TOLERANCE_M
) -> list[tuple[float, float]]:
    """붙어 있는 점을 하나로 줄인다. 순서는 그대로 둔다."""
    cleaned: list[tuple[float, float]] = []
    for point in points:
        if not cleaned or _distance(cleaned[-1], point) > tolerance_m:
            cleaned.append(point)
    return cleaned


def _perpendicular_distance(
    point: tuple[float, float], start: tuple[float, float], end: tuple[float, float]
) -> float:
    """점에서 선분까지의 수직 거리(m). 선분이 한 점이면 그 점까지의 거리."""
    dx, dy = end[0] - start[0], end[1] - start[1]
    if dx == 0 and dy == 0:
        return _distance(point, start)
    return abs(dy * point[0] - dx * point[1] + end[0] * start[1] - end[1] * start[0]) / math.hypot(
        dx, dy
    )


def _douglas_peucker(points: list[tuple[float, float]], tolerance_m: float) -> list[int]:
    """남길 점의 **원본 색인**을 돌려준다 — 되뽑기가 원본 구간을 다시 꺼내야 해서 색인이다."""
    if len(points) < 3:
        return list(range(len(points)))
    keep = {0, len(points) - 1}
    stack = [(0, len(points) - 1)]
    while stack:
        first, last = stack.pop()
        if last <= first + 1:
            continue
        worst, worst_index = -1.0, first
        for index in range(first + 1, last):
            gap = _perpendicular_distance(points[index], points[first], points[last])
            if gap > worst:
                worst, worst_index = gap, index
        if worst > tolerance_m:
            keep.add(worst_index)
            stack.append((first, worst_index))
            stack.append((worst_index, last))
    return sorted(keep)


def simplify_to_nodes(
    points: list[tuple[float, float]],
    tolerance_m: float = SIMPLIFY_TOLERANCE_M,
    max_spacing_m: float = MAX_NODE_SPACING_M,
    depth: int = MAX_REFINE_DEPTH,
) -> list[tuple[float, float]]:
    """조밀한 점군에서 **꺾임점(IP)** 만 남긴다 — Douglas-Peucker + 벌어진 구간 되뽑기.

    예상노선은 격자 탐색이 낸 점군이라 3m 간격으로 촘촘하다. 그대로 두면 곡선을 끼울
    접선 자리가 없어 반지름이 뭉개진다. 원래 선에서 `tolerance_m` 보다 멀어지지 않는
    선에서 점을 걷어내므로 **모양은 그대로**다.

    남은 노드 사이가 `max_spacing_m` 을 넘으면 **그 구간만** 허용오차를 절반으로 낮춰 다시
    뽑는다 — 완만하고 긴 굴곡이 통째로 삼켜지는 것을 막는다(2026-09-06 S자 노선 실측).
    """
    if len(points) < 3:
        return list(points)
    kept = _douglas_peucker(points, tolerance_m)
    if depth <= 0 or max_spacing_m <= 0:
        return [points[index] for index in kept]

    result: list[tuple[float, float]] = [points[kept[0]]]
    for previous, current in zip(kept, kept[1:]):
        if _distance(points[previous], points[current]) > max_spacing_m and current > previous + 1:
            refined = simplify_to_nodes(
                points[previous : current + 1],
                tolerance_m / 2,
                max_spacing_m,
                depth - 1,
            )
            result.extend(refined[1:])
        else:
            result.append(points[current])
    return result


def _inner_angle_deg(
    before: tuple[float, float], at: tuple[float, float], after: tuple[float, float]
) -> float:
    """세 점이 이루는 내각(도). 일직선이면 180."""
    ax, ay = before[0] - at[0], before[1] - at[1]
    bx, by = after[0] - at[0], after[1] - at[1]
    la, lb = math.hypot(ax, ay), math.hypot(bx, by)
    if la <= 0 or lb <= 0:
        return 180.0
    cosine = max(-1.0, min(1.0, (ax * bx + ay * by) / (la * lb)))
    return math.degrees(math.acos(cosine))


def _unit(from_point: tuple[float, float], to_point: tuple[float, float]) -> tuple[float, float]:
    length = _distance(from_point, to_point)
    if length <= 0:
        return (0.0, 0.0)
    return ((to_point[0] - from_point[0]) / length, (to_point[1] - from_point[1]) / length)


def _node_indices(
    points: list[tuple[float, float]], nodes: list[tuple[float, float]]
) -> list[int] | None:
    """노드가 원본 점 목록의 몇 번째인지 — 노드는 원본 자리 그대로라 순서대로 찾힌다.

    사용자가 옮긴 노드처럼 원본에 없는 점이 섞이면 `None` 을 돌려준다(그때는 피팅을 건너뛴다).
    """
    indices: list[int] = []
    cursor = 0
    for node in nodes:
        while cursor < len(points) and points[cursor] != node:
            cursor += 1
        if cursor >= len(points):
            return None
        indices.append(cursor)
        cursor += 1
    return indices


def _point_to_segment_m(
    point: tuple[float, float], start: tuple[float, float], end: tuple[float, float]
) -> float:
    """점에서 선분까지 거리(m)."""
    dx, dy = end[0] - start[0], end[1] - start[1]
    if dx == 0 and dy == 0:
        return _distance(point, start)
    ratio = ((point[0] - start[0]) * dx + (point[1] - start[1]) * dy) / (dx * dx + dy * dy)
    ratio = max(0.0, min(1.0, ratio))
    return _distance(point, (start[0] + dx * ratio, start[1] + dy * ratio))


def _corner_deviation_m(
    samples: list[tuple[float, float]],
    before: tuple[float, float],
    after: tuple[float, float],
    start: tuple[float, float],
    end: tuple[float, float],
    center: tuple[float, float],
    radius: float,
) -> float:
    """예상노선 점들이 「직선-곡선-직선」에서 얼마나 벗어나는지 — **평균** 거리(m).

    사용자 확정(2026-09-07): 반지름은 법정 하한을 지키되 **예정노선에 가장 가까운 값**을 고른다.
    그 「가까움」을 재는 자다. 원호 구간 밖의 점은 접선 두 개까지의 거리로 잰다.

    ⚠ 최대값이 아니라 **평균**을 쓴다(2026-09-07 실측) — 최대값은 반지름에 거의 반응하지
    않는다. 작은 반지름이면 접선 두 개가 구간의 대부분을 덮어, 가장 먼 점은 어차피 꺾임점을
    가로지르는 직선이 정하기 때문이다. 그래서 실제 원호 R 40m 짜리 곡선에서도 하한(12m)이
    골라졌다. 평균으로 재면 「굴곡을 얼마나 잘 따라가나」가 값에 들어온다.
    """
    if not samples:
        return 0.0
    total = 0.0
    for sample in samples:
        gap = min(
            _point_to_segment_m(sample, before, start),
            _point_to_segment_m(sample, end, after),
            # 원호까지 거리 — 중심에서 잰 반지름 차. 원호 밖 각도면 끝점 거리가 더 가까워
            # 위 두 값이 이미 그것을 담는다.
            abs(_distance(sample, center) - radius),
        )
        total += gap
    return total / len(samples)


def _arc_geometry(
    before: tuple[float, float],
    at: tuple[float, float],
    after: tuple[float, float],
    inner_deg: float,
    radius: float,
    half_tan: float,
) -> tuple[tuple[float, float], tuple[float, float], tuple[float, float]] | None:
    """반지름 하나에 대한 (접선시작, 접선끝, 중심). 원호를 못 끼우면 None."""
    tangent = radius * half_tan
    to_before = _unit(at, before)
    to_after = _unit(at, after)
    start = (at[0] + to_before[0] * tangent, at[1] + to_before[1] * tangent)
    end = (at[0] + to_after[0] * tangent, at[1] + to_after[1] * tangent)
    bisector = (to_before[0] + to_after[0], to_before[1] + to_after[1])
    bisector_length = math.hypot(*bisector)
    if bisector_length <= 1e-9:
        return None
    center_distance = radius / math.sin(math.radians(inner_deg) / 2)
    center = (
        at[0] + bisector[0] / bisector_length * center_distance,
        at[1] + bisector[1] / bisector_length * center_distance,
    )
    return start, end, center


def _fit_radius_m(
    samples: list[tuple[float, float]],
    before: tuple[float, float],
    at: tuple[float, float],
    after: tuple[float, float],
    inner_deg: float,
    half_tan: float,
    min_radius_m: float,
    max_radius_m: float,
    steps: int = 48,
) -> float:
    """법정 하한 이상에서 **예정노선과 가장 덜 벌어지는** 반지름을 고른다.

    왜 하한 고정이 아닌가(2026-09-07 사용자 확정) — 「반지름은 법정 최소 값을 지키되 기존
    예정노선에 가까운 폴리라인을 찾는 게 키」. 꺾임이 뚜렷한 자리는 하한이 가장 가깝지만
    (중앙종거 M = R(1/cos(Δ/2) − 1) 이라 R 이 클수록 꺾임점에서 멀어짐), **완만하고 긴 굴곡**은
    반대로 큰 반지름이 원본을 잘 따라간다. 그래서 자리마다 재서 고른다.
    """
    if not samples or max_radius_m <= min_radius_m:
        return min_radius_m
    best_radius = min_radius_m
    best_gap = float("inf")
    for step in range(steps + 1):
        # 작은 반지름 쪽을 촘촘히 본다 — 하한 부근에서 값이 빠르게 변한다.
        ratio = step / steps
        radius = min_radius_m * (max_radius_m / min_radius_m) ** ratio
        geometry = _arc_geometry(before, at, after, inner_deg, radius, half_tan)
        if geometry is None:
            continue
        start, end, center = geometry
        gap = _corner_deviation_m(samples, before, after, start, end, center, radius)
        if gap < best_gap - 1e-9:
            best_gap, best_radius = gap, radius
    return best_radius


def _arc_points(
    center: tuple[float, float],
    start: tuple[float, float],
    end: tuple[float, float],
    clockwise: bool,
) -> list[tuple[float, float]]:
    """중심과 두 끝점으로 원호 위 점을 찍는다(양 끝 포함하지 않음 — 부르는 쪽이 붙인다)."""
    radius = _distance(center, start)
    if radius <= 0:
        return []
    start_angle = math.atan2(start[1] - center[1], start[0] - center[0])
    end_angle = math.atan2(end[1] - center[1], end[0] - center[0])
    sweep = end_angle - start_angle
    if clockwise:
        while sweep > 0:
            sweep -= 2 * math.pi
    else:
        while sweep < 0:
            sweep += 2 * math.pi
    steps = max(1, int(abs(math.degrees(sweep)) / ARC_STEP_DEG))
    return [
        (
            center[0] + radius * math.cos(start_angle + sweep * step / steps),
            center[1] + radius * math.sin(start_angle + sweep * step / steps),
        )
        for step in range(1, steps)
    ]


def _turn_sign(
    before: tuple[float, float], at: tuple[float, float], after: tuple[float, float]
) -> int:
    """도는 방향 — 왼쪽 +1, 오른쪽 −1, 곧게 0."""
    cross = (at[0] - before[0]) * (after[1] - at[1]) - (at[1] - before[1]) * (after[0] - at[0])
    if abs(cross) <= 1e-9:
        return 0
    return 1 if cross > 0 else -1


def _line_intersection(
    a1: tuple[float, float],
    a2: tuple[float, float],
    b1: tuple[float, float],
    b2: tuple[float, float],
) -> tuple[float, float] | None:
    """두 **직선**(선분 아님)의 교차점. 나란하면 None."""
    dx1, dy1 = a2[0] - a1[0], a2[1] - a1[1]
    dx2, dy2 = b2[0] - b1[0], b2[1] - b1[1]
    denominator = dx1 * dy2 - dy1 * dx2
    if abs(denominator) <= 1e-12:
        return None
    t = ((b1[0] - a1[0]) * dy2 - (b1[1] - a1[1]) * dx2) / denominator
    return (a1[0] + dx1 * t, a1[1] + dy1 * t)