|
|
from __future__ import annotations
|
|
|
from typing import Tuple, Optional
|
|
|
from . import config
|
|
|
|
|
|
# ---------------- Базовая геометрия углов ----------------
|
|
|
|
|
|
def normalize360(a: float) -> float:
|
|
|
"""Нормирует угол в диапазон [0, 360)."""
|
|
|
a = a % 360.0
|
|
|
return a if a >= 0.0 else a + 360.0
|
|
|
|
|
|
def ang_diff_signed(a: float, b: float) -> float:
|
|
|
"""
|
|
|
Подписанная кратчайшая разница (a - b) в диапазоне [-180, +180).
|
|
|
Знак >0 означает поворот по ЧС от b к a.
|
|
|
"""
|
|
|
return (a - b + 540.0) % 360.0 - 180.0
|
|
|
|
|
|
def _wrap_deg_pm180(a: float) -> float:
|
|
|
"""Нормировка в диапазон [-180, +180)."""
|
|
|
return (a + 180.0) % 360.0 - 180.0
|
|
|
|
|
|
# ---------------- Перевод внутренних углов PTZ в гео ----------------
|
|
|
|
|
|
def pan_to_geo(cam_id: int, pan_deg: float | None) -> float | None:
|
|
|
"""
|
|
|
Переводит «внутренний» pan PTZ (0..360, как отдаёт камера) в гео-азимут (0°=С, 90°=В).
|
|
|
Гео = normalize360(north_offset_deg + pan).
|
|
|
Если pan_deg=None или north неизвестен — возвращает None (или 0.0 по желанию).
|
|
|
"""
|
|
|
if pan_deg is None:
|
|
|
return None
|
|
|
north = config.CAMERA_CONFIG.get(cam_id, {}).get("north_offset_deg")
|
|
|
if not isinstance(north, (int, float)):
|
|
|
return None
|
|
|
return normalize360(float(north) + float(pan_deg))
|
|
|
|
|
|
# ---------------- Сектора (мягкие/жёсткие края) ----------------
|
|
|
|
|
|
def sector_contains(az: float, dmin: float, dmax: float) -> bool:
|
|
|
"""
|
|
|
Проверяет, лежит ли азимут az внутри сектора [dmin, dmax] с учётом жёсткого эпсилона.
|
|
|
Работает и для «завёрнутых» интервалов (например, [320°, 40°]).
|
|
|
"""
|
|
|
az = normalize360(az); dmin = normalize360(dmin); dmax = normalize360(dmax)
|
|
|
if dmin <= dmax:
|
|
|
return dmin - config.EDGE_EPS_HARD <= az <= dmax + config.EDGE_EPS_HARD
|
|
|
return az >= dmin - config.EDGE_EPS_HARD or az <= dmax + config.EDGE_EPS_HARD
|
|
|
|
|
|
def sector_distances(az: float, dmin: float, dmax: float) -> Tuple[float, float, bool]:
|
|
|
"""
|
|
|
Возвращает (to_left, to_right, inside), где:
|
|
|
to_left = на сколько градусов до левой границы (>=0, если внутри)
|
|
|
to_right = на сколько градусов до правой границы (>=0, если внутри)
|
|
|
inside = флаг попадания в сектор (по sector_contains)
|
|
|
Расстояния считаются по кратчайшей дуге.
|
|
|
"""
|
|
|
az_n = normalize360(az)
|
|
|
dmin_n = normalize360(dmin)
|
|
|
dmax_n = normalize360(dmax)
|
|
|
|
|
|
inside = sector_contains(az_n, dmin_n, dmax_n)
|
|
|
|
|
|
# расстояние до границы — модуль подписанной разницы
|
|
|
to_left = abs(ang_diff_signed(dmin_n, az_n))
|
|
|
to_right = abs(ang_diff_signed(dmax_n, az_n))
|
|
|
|
|
|
return to_left, to_right, inside
|
|
|
|
|
|
def clamp_sector_speed(pan_i: int, az: Optional[float], dmin: Optional[float], dmax: Optional[float],
|
|
|
pan_sign: int = 1) -> int:
|
|
|
"""
|
|
|
Притормаживает/блокирует скорость панорамирования у краёв сектора.
|
|
|
Работает с целочисленной шкалой Hik (-100..+100). Совместимо с логикой в ptz_io.
|
|
|
|
|
|
- Если az неизвестен, возврат pan_i без изменений (внешний код сам решает, можно ли двигаться).
|
|
|
- Если вне сектора — блокируем.
|
|
|
- Если внутри и «толкаем» в закрытую сторону — блокируем.
|
|
|
- Если внутри — плавно замедляем вблизи ближайшей границы.
|
|
|
"""
|
|
|
if az is None or dmin is None or dmax is None:
|
|
|
return pan_i
|
|
|
to_left, to_right, inside = sector_distances(az, dmin, dmax)
|
|
|
if not inside:
|
|
|
return 0
|
|
|
|
|
|
# направление «вправо/влево» с учётом знака головы
|
|
|
pushing_right = (pan_i * pan_sign) > 0 and ang_diff_signed(dmax, az) <= 0.0
|
|
|
pushing_left = (pan_i * pan_sign) < 0 and ang_diff_signed(dmin, az) >= 0.0
|
|
|
if pushing_right or pushing_left:
|
|
|
return 0
|
|
|
|
|
|
if pan_i == 0:
|
|
|
return 0
|
|
|
|
|
|
# плавное замедление у ближайшей границы
|
|
|
dist_min = min(to_left, to_right)
|
|
|
edge_slow = getattr(config, "TRACK_EDGE_SLOWDOWN_DEG", 6.0)
|
|
|
edge_min_k = getattr(config, "TRACK_EDGE_MIN_SCALE", 0.25)
|
|
|
if dist_min < edge_slow:
|
|
|
k = max(edge_min_k, dist_min / max(1e-6, edge_slow))
|
|
|
pan_i = int(pan_i * k)
|
|
|
|
|
|
return pan_i
|
|
|
|
|
|
# ---------------- Прочие утилиты ----------------
|
|
|
|
|
|
def is_near(a: float, b: float, tol: float = 0.25) -> bool:
|
|
|
"""Близость углов по модулю подписанной разницы."""
|
|
|
return abs(ang_diff_signed(a, b)) <= tol
|
|
|
|
|
|
def nearest_preset_token(cam_idx: int, az: float) -> str | None:
|
|
|
"""
|
|
|
Возвращает token ближайшего пресета (preset1|preset2) по текущему азимуту.
|
|
|
Если нет калиброванных preset*_deg — вернёт None.
|
|
|
"""
|
|
|
cfg = config.CAMERA_CONFIG[cam_idx]
|
|
|
p1, p2 = cfg.get("preset1"), cfg.get("preset2")
|
|
|
a1, a2 = cfg.get("preset1_deg"), cfg.get("preset2_deg")
|
|
|
if not (p1 and p2 and isinstance(a1, (int, float)) and isinstance(a2, (int, float))):
|
|
|
return None
|
|
|
d1 = abs(ang_diff_signed(az, a1))
|
|
|
d2 = abs(ang_diff_signed(az, a2))
|
|
|
return p1 if d1 <= d2 else p2
|
|
|
|
|
|
def project_x_to_bearing(base_az: float, hfov_deg: float, x_norm: float) -> float:
|
|
|
"""
|
|
|
Быстрый расчёт истинного азимута объекта на панораме:
|
|
|
base_az — гео-азимут центра кадра (0°=С, 90°=В),
|
|
|
hfov_deg — горизонтальный угол поля зрения,
|
|
|
x_norm — нормированная x-координата бокса [0..1].
|
|
|
Возвращает bearing в диапазоне [-180, +180).
|
|
|
"""
|
|
|
# Важно: сначала нормируем базовый азимут, затем добавляем смещение,
|
|
|
# после чего сворачиваем в [-180, +180) для «относительного» румба.
|
|
|
az_center = normalize360(float(base_az))
|
|
|
delta = (float(x_norm) - 0.5) * float(hfov_deg)
|
|
|
return _wrap_deg_pm180(az_center + delta)
|