ai_fwt_pro/main/xingxing-server/plugins_func/nav_copilot/geo.py

from __future__ import annotations

import math
from typing import Iterable, Optional, Tuple


EARTH_RADIUS_M = 6371000.0
_PI = math.pi
_X_PI = _PI * 3000.0 / 180.0
_A = 6378245.0
_EE = 0.00669342162296594323
_VALID_COORD_TYPES = {"WGS84", "GCJ02", "BD09"}


def normalize_coord_type(coord_type: object, *, default: str = "WGS84") -> str:
    raw = str(coord_type or "").strip().upper()
    if not raw:
        return default

    aliases = {
        "GPS": "WGS84",
        "GNSS": "WGS84",
        "WGS-84": "WGS84",
        "WGS_84": "WGS84",
        "GCJ": "GCJ02",
        "GCJ-02": "GCJ02",
        "GCJ_02": "GCJ02",
        "BD-09": "BD09",
        "BD_09": "BD09",
        "BD09LL": "BD09",
        "BAIDU": "BD09",
    }
    normalized = aliases.get(raw, raw)
    if normalized not in _VALID_COORD_TYPES:
        raise ValueError("invalid_coord_type")
    return normalized


def haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
    """Great-circle distance in meters."""
    phi1 = math.radians(lat1)
    phi2 = math.radians(lat2)
    d_phi = math.radians(lat2 - lat1)
    d_lam = math.radians(lon2 - lon1)
    a = (
        math.sin(d_phi / 2.0) ** 2
        + math.cos(phi1) * math.cos(phi2) * math.sin(d_lam / 2.0) ** 2
    )
    c = 2.0 * math.atan2(math.sqrt(a), math.sqrt(1.0 - a))
    return EARTH_RADIUS_M * c


def clamp(n: int, lo: int, hi: int) -> int:
    return max(lo, min(hi, n))


def nearest_point_index(
    points_latlon: Iterable[Tuple[float, float]],
    lat: float,
    lon: float,
    *,
    start_idx: Optional[int] = None,
    window: int = 80,
) -> Tuple[int, float]:
    """Return (idx, distance_m) of the nearest point.

    Kept for compatibility with older call sites.
    """
    pts = list(points_latlon)
    if not pts:
        raise ValueError("empty points")

    if start_idx is None:
        search_lo = 0
        search_hi = len(pts) - 1
    else:
        search_lo = clamp(start_idx - window, 0, len(pts) - 1)
        search_hi = clamp(start_idx + window, 0, len(pts) - 1)

    best_i = search_lo
    best_d = float("inf")
    for i in range(search_lo, search_hi + 1):
        p_lat, p_lon = pts[i]
        d = haversine_m(lat, lon, p_lat, p_lon)
        if d < best_d:
            best_d = d
            best_i = i
    return best_i, best_d


def wgs84_to_bd09(lat: float, lon: float) -> Tuple[float, float]:
    gcj_lat, gcj_lon = _wgs84_to_gcj02(lat, lon)
    return _gcj02_to_bd09(gcj_lat, gcj_lon)


def wgs84_to_gcj02(lat: float, lon: float) -> Tuple[float, float]:
    return _wgs84_to_gcj02(lat, lon)


def gcj02_to_wgs84(lat: float, lon: float) -> Tuple[float, float]:
    if _out_of_china(lat, lon):
        return lat, lon

    wgs_lat = lat
    wgs_lon = lon
    for _ in range(8):
        guessed_lat, guessed_lon = _wgs84_to_gcj02(wgs_lat, wgs_lon)
        delta_lat = guessed_lat - lat
        delta_lon = guessed_lon - lon
        wgs_lat -= delta_lat
        wgs_lon -= delta_lon
        if abs(delta_lat) < 1e-7 and abs(delta_lon) < 1e-7:
            break
    return wgs_lat, wgs_lon


def bd09_to_gcj02(lat: float, lon: float) -> Tuple[float, float]:
    x = lon - 0.0065
    y = lat - 0.006
    z = math.sqrt(x * x + y * y) - 0.00002 * math.sin(y * _X_PI)
    theta = math.atan2(y, x) - 0.000003 * math.cos(x * _X_PI)
    gcj_lon = z * math.cos(theta)
    gcj_lat = z * math.sin(theta)
    return gcj_lat, gcj_lon


def bd09_to_wgs84(lat: float, lon: float) -> Tuple[float, float]:
    gcj_lat, gcj_lon = bd09_to_gcj02(lat, lon)
    return gcj02_to_wgs84(gcj_lat, gcj_lon)


def convert_to_wgs84(
    lat: float,
    lon: float,
    coord_type: object,
) -> Tuple[float, float]:
    normalized = normalize_coord_type(coord_type, default="WGS84")
    if normalized == "WGS84":
        return lat, lon
    if normalized == "GCJ02":
        return gcj02_to_wgs84(lat, lon)
    return bd09_to_wgs84(lat, lon)


def convert_from_wgs84(
    lat: float,
    lon: float,
    coord_type: object,
) -> Tuple[float, float]:
    normalized = normalize_coord_type(coord_type, default="WGS84")
    if normalized == "WGS84":
        return lat, lon
    if normalized == "GCJ02":
        return wgs84_to_gcj02(lat, lon)
    return wgs84_to_bd09(lat, lon)


def crosses_alert_line(
    prev_lat: float,
    prev_lon: float,
    curr_lat: float,
    curr_lon: float,
    start_lat: float,
    start_lon: float,
    end_lat: float,
    end_lon: float,
    *,
    buffer_m: float = 2.5,
) -> bool:
    ref_lat = (prev_lat + curr_lat + start_lat + end_lat) / 4.0
    ref_lon = (prev_lon + curr_lon + start_lon + end_lon) / 4.0

    p0 = _latlon_to_xy(prev_lat, prev_lon, ref_lat, ref_lon)
    p1 = _latlon_to_xy(curr_lat, curr_lon, ref_lat, ref_lon)
    a = _latlon_to_xy(start_lat, start_lon, ref_lat, ref_lon)
    b = _latlon_to_xy(end_lat, end_lon, ref_lat, ref_lon)

    if _segment_length_m(a, b) < 0.5:
        return False

    if _segments_intersect(p0, p1, a, b):
        return True

    side0 = _orientation(a, b, p0)
    side1 = _orientation(a, b, p1)
    if side0 * side1 > 0:
        return False

    return _segment_distance_m(p0, p1, a, b) <= max(0.5, buffer_m)


def _out_of_china(lat: float, lon: float) -> bool:
    return lon < 72.004 or lon > 137.8347 or lat < 0.8293 or lat > 55.8271


def _transform_lat(x: float, y: float) -> float:
    ret = (
        -100.0
        + 2.0 * x
        + 3.0 * y
        + 0.2 * y * y
        + 0.1 * x * y
        + 0.2 * math.sqrt(abs(x))
    )
    ret += (
        (20.0 * math.sin(6.0 * x * _PI) + 20.0 * math.sin(2.0 * x * _PI))
        * 2.0
        / 3.0
    )
    ret += (
        (20.0 * math.sin(y * _PI) + 40.0 * math.sin(y / 3.0 * _PI))
        * 2.0
        / 3.0
    )
    ret += (
        (160.0 * math.sin(y / 12.0 * _PI) + 320 * math.sin(y * _PI / 30.0))
        * 2.0
        / 3.0
    )
    return ret


def _transform_lon(x: float, y: float) -> float:
    ret = (
        300.0
        + x
        + 2.0 * y
        + 0.1 * x * x
        + 0.1 * x * y
        + 0.1 * math.sqrt(abs(x))
    )
    ret += (
        (20.0 * math.sin(6.0 * x * _PI) + 20.0 * math.sin(2.0 * x * _PI))
        * 2.0
        / 3.0
    )
    ret += (
        (20.0 * math.sin(x * _PI) + 40.0 * math.sin(x / 3.0 * _PI))
        * 2.0
        / 3.0
    )
    ret += (
        (150.0 * math.sin(x / 12.0 * _PI) + 300.0 * math.sin(x / 30.0 * _PI))
        * 2.0
        / 3.0
    )
    return ret


def _wgs84_to_gcj02(lat: float, lon: float) -> Tuple[float, float]:
    if _out_of_china(lat, lon):
        return lat, lon

    d_lat = _transform_lat(lon - 105.0, lat - 35.0)
    d_lon = _transform_lon(lon - 105.0, lat - 35.0)
    rad_lat = lat / 180.0 * _PI
    magic = math.sin(rad_lat)
    magic = 1 - _EE * magic * magic
    sqrt_magic = math.sqrt(magic)
    d_lat = (d_lat * 180.0) / (((_A * (1 - _EE)) / (magic * sqrt_magic)) * _PI)
    d_lon = (d_lon * 180.0) / ((_A / sqrt_magic * math.cos(rad_lat)) * _PI)
    return lat + d_lat, lon + d_lon


def _gcj02_to_bd09(lat: float, lon: float) -> Tuple[float, float]:
    z = math.sqrt(lon * lon + lat * lat) + 0.00002 * math.sin(lat * _X_PI)
    theta = math.atan2(lat, lon) + 0.000003 * math.cos(lon * _X_PI)
    bd_lon = z * math.cos(theta) + 0.0065
    bd_lat = z * math.sin(theta) + 0.006
    return bd_lat, bd_lon


def _latlon_to_xy(
    lat: float,
    lon: float,
    ref_lat: float,
    ref_lon: float,
) -> Tuple[float, float]:
    x = math.radians(lon - ref_lon) * EARTH_RADIUS_M * math.cos(math.radians(ref_lat))
    y = math.radians(lat - ref_lat) * EARTH_RADIUS_M
    return x, y


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


def _orientation(
    a: Tuple[float, float],
    b: Tuple[float, float],
    c: Tuple[float, float],
) -> float:
    return (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0])


def _on_segment(
    a: Tuple[float, float],
    b: Tuple[float, float],
    p: Tuple[float, float],
    *,
    eps: float = 1e-6,
) -> bool:
    return (
        min(a[0], b[0]) - eps <= p[0] <= max(a[0], b[0]) + eps
        and min(a[1], b[1]) - eps <= p[1] <= max(a[1], b[1]) + eps
    )


def _segments_intersect(
    p1: Tuple[float, float],
    q1: Tuple[float, float],
    p2: Tuple[float, float],
    q2: Tuple[float, float],
    *,
    eps: float = 1e-6,
) -> bool:
    o1 = _orientation(p1, q1, p2)
    o2 = _orientation(p1, q1, q2)
    o3 = _orientation(p2, q2, p1)
    o4 = _orientation(p2, q2, q1)

    if (o1 > eps and o2 < -eps or o1 < -eps and o2 > eps) and (
        o3 > eps and o4 < -eps or o3 < -eps and o4 > eps
    ):
        return True

    if abs(o1) <= eps and _on_segment(p1, q1, p2, eps=eps):
        return True
    if abs(o2) <= eps and _on_segment(p1, q1, q2, eps=eps):
        return True
    if abs(o3) <= eps and _on_segment(p2, q2, p1, eps=eps):
        return True
    if abs(o4) <= eps and _on_segment(p2, q2, q1, eps=eps):
        return True
    return False


def _point_to_segment_distance_m(
    p: Tuple[float, float],
    a: Tuple[float, float],
    b: Tuple[float, float],
) -> float:
    ax, ay = a
    bx, by = b
    px, py = p
    dx = bx - ax
    dy = by - ay
    if abs(dx) < 1e-9 and abs(dy) < 1e-9:
        return math.hypot(px - ax, py - ay)
    t = ((px - ax) * dx + (py - ay) * dy) / (dx * dx + dy * dy)
    t = max(0.0, min(1.0, t))
    proj_x = ax + t * dx
    proj_y = ay + t * dy
    return math.hypot(px - proj_x, py - proj_y)


def _segment_distance_m(
    p1: Tuple[float, float],
    q1: Tuple[float, float],
    p2: Tuple[float, float],
    q2: Tuple[float, float],
) -> float:
    if _segments_intersect(p1, q1, p2, q2):
        return 0.0
    return min(
        _point_to_segment_distance_m(p1, p2, q2),
        _point_to_segment_distance_m(q1, p2, q2),
        _point_to_segment_distance_m(p2, p1, q1),
        _point_to_segment_distance_m(q2, p1, q1),
    )