dsh/dual_camera_system/core/coord_utils.py

"""球面坐标与 PTZ pan/tilt 角度换算工具."""
import math
from typing import Tuple


def pan_tilt_to_vector(pan: float, tilt: float) -> Tuple[float, float, float]:
    """把 pan/tilt（度）转成单位向量 (x, y, z)。

    坐标系：x 向右，y 向上，z 向前；pan 从正前方 z 轴开始顺时针；
    tilt 0 为水平，+90 为天顶。
    """
    pan_rad = math.radians(pan)
    tilt_rad = math.radians(tilt)
    x = math.cos(tilt_rad) * math.sin(pan_rad)
    y = math.sin(tilt_rad)
    z = math.cos(tilt_rad) * math.cos(pan_rad)
    return x, y, z


def spherical_to_pan_tilt(x: float, y: float, z: float) -> Tuple[float, float]:
    """把单位向量 (x, y, z) 转成 pan/tilt（度）。"""
    norm = math.sqrt(x * x + y * y + z * z)
    if norm < 1e-9:
        return 0.0, 0.0
    x, y, z = x / norm, y / norm, z / norm
    tilt = math.degrees(math.asin(max(-1.0, min(1.0, y))))
    pan = math.degrees(math.atan2(x, z))
    if pan < 0:
        pan += 360.0
    return pan, tilt


def compute_sample_grid(
    pan_range: Tuple[float, float] = (0.0, 360.0),
    tilt_layers: Tuple[float, ...] = (-20.0, 0.0, 20.0),
    pan_step: float = 30.0,
) -> list[tuple[float, float]]:
    """生成扫描采样网格 [(pan, tilt), ...]。"""
    if pan_step <= 0:
        raise ValueError("pan_step must be positive")
    if not tilt_layers:
        raise ValueError("tilt_layers must not be empty")
    if pan_range[0] >= pan_range[1]:
        raise ValueError("pan_range start must be less than end")
    points = []
    pan_start, pan_end = pan_range
    pan = pan_start
    while pan < pan_end - 1e-6:
        for tilt in tilt_layers:
            points.append((round(pan, 2), round(tilt, 2)))
        pan += pan_step
    return points