"""位置×朝向联合可行集（第四问定向源）。 现状问题（docs/第三四问架构优化与运行提速方案.md §1.3 F3）：第四问的失败**全部** 来自定向源（评测机两局：全向 17/17 清除，定向仅 2/6）。原因不是覆盖不足，而是 策略只知道"源可能在某个空间区域"，不知道"它朝着哪边"——于是选点只能靠固定 45° 阶梯试探，一旦试探失败就退化到 ≤400 格 20 m 盲清。 本模块把"看不看得见"变成可推理的量：为已在 Ω 上维护的网格幸存集 （:class:`~mcm26b.particles.ChannelParticles`）附加每格 24 档朝向可行性掩码， 得到 (位置 × 朝向) 联合可行集。观测语义严格对齐评测机 ``referee.simulator._observe``： - 距离 > r_eff → no_signal；定向且源→读点方向与朝向夹角 > 90° → no_signal； - 距离 ≤ 5 m → near（**仍需可见**：3 m 处背向的定向源同样返回 no_signal）； - 其余 → direction（读数 = 源→读点方位 + 有界误差）。 更新规则（全部保守，只删"必然不可能"的格/档）： =============== ============================================ ================== 观测 位置 朝向 =============== ============================================ ================== ``direction``@s 距离 ≤ 1500、角向 |gap| ≤ δ_eff + 格裕量 必须**可能**覆盖 s ``near``@s 距离 ≤ 5 必须**可能**覆盖 s ``no_signal``@s **不删**（可能是朝向遮挡或距离不足） 必须**确定不**覆盖 s （仅当整格必在 r_eff_min 内才约束朝向） =============== ============================================ ================== 阈值全部取保守侧，并计入**格内角裕量** ``asin(margin / max(dist − margin, 1))``（格内任一点相对格心方位偏差上界）： - "必须可能覆盖"：``gap ≤ 90° + 半档 + 查表舍入 + 格内角裕量``（宁可保留）； - "必须确定不覆盖"：先删去 ``gap ≤ 90° − 半档 − 查表舍入 − 格内角裕量`` 的档， 只保留确定看不见 s 的档（同样宁可保留）。 因此真源 (位置, 朝向) 永不被删——这是网格幸存集真源保留定理的二维推广。 （2026-09-12 修正：no_signal 分支原先误用"确定覆盖"掩码做交集，等价于把 规则反向；keep=True 分支也漏计了格内角裕量。两处都会删掉真源朝向档， 现由 ``tests/test_joint_belief.py`` 的随机观测序列保留性测试固化。） 用途： 1. ``prob_detect``：该点能看见该频道任一可行 (位置, 朝向) 的概率质量， 用于**可见性选点**（取代固定阶梯）——只影响选点顺序，不承担安全责任； 2. ``expected_posterior_radius``：一次测量后的期望后验 MEC 半径（D1 值函数）； 3. ``is_absent``：联合集空 ⟹ 该频道不存在源（严格，与覆盖证明同一条件）； 4. 位置边际仍由凸证书负责 20 m 清除判定，本模块不改变任何证书口径。 """ from __future__ import annotations import math from ..config import ProblemConfig, Vec2 from ..particles import ChannelParticles, _branch_radius_array from ..protocol.rules import DIRECTION, NEAR, NO_SIGNAL HEADING_BINS = 24 # 15° 一档 _BIN_HALF_DEG = 180.0 / HEADING_BINS # 7.5° _FULL_MASK = (1 << HEADING_BINS) - 1 _GRID_DEG = 3.0 # 角度查表分辨率 _GRID_N = int(round(360.0 / _GRID_DEG)) try: # numpy 仅作可选加速后端（与 particles 一致） import numpy as _np from ..particles import _np as _particles_np if _particles_np is None: # MCM26B_NO_NUMPY 或缺失：强制纯 Python _np = None except Exception: # pragma: no cover - 环境无 numpy _np = None def heading_gap(a_deg: float, b_deg: float) -> float: """两个朝向角的最小夹角（度）。""" return abs((a_deg - b_deg + 180.0) % 360.0 - 180.0) def _cover_mask(bearing_deg: float, half_deg: float) -> int: """与该方位夹角不超过 ``half_deg`` 的朝向档掩码。""" mask = 0 for k in range(HEADING_BINS): if heading_gap(360.0 * k / HEADING_BINS, bearing_deg) <= half_deg: mask |= 1 << k return mask # 查表：以 bearing 为轴、半角 90° 的可见半平面。 # - COVER：**可能**覆盖该方位（含半档与查表舍入的保守侧）； # - SILENT：**确定**覆盖该方位（内缩半档与舍入）——离散对照表，供审计与 # 文档比对；实际裁剪走 _cover_mask / _far_mask，因为还必须逐格计入 # "格内角裕量"（格心到格内任一点的方位偏差上界），无法预先制表。 _COVER_TABLE = tuple( _cover_mask(_GRID_DEG * i, 90.0 + _BIN_HALF_DEG + _GRID_DEG / 2) for i in range(_GRID_N)) _SILENT_TABLE = tuple( _cover_mask(_GRID_DEG * i, 90.0 - _BIN_HALF_DEG - _GRID_DEG / 2) for i in range(_GRID_N)) def _far_mask(bearing_deg: float, exclude_half_deg: float) -> int: """与方位夹角**大于** ``exclude_half_deg`` 的朝向档掩码（no_signal 用）。 与 :func:`_cover_mask` 互补：no_signal 只允许保留"确定看不见"的朝向档。 """ mask = 0 for k in range(HEADING_BINS): if heading_gap(360.0 * k / HEADING_BINS, bearing_deg) > exclude_half_deg: mask |= 1 << k return mask if _np is not None: # 12 位半字 popcount 查表（朝向掩码 24 位）与半平面查表数组 _POP16 = _np.array([bin(value).count("1") for value in range(1 << 12)], dtype=_np.int64) _COVER_ARR = _np.array(_COVER_TABLE, dtype=_np.int64) else: # pragma: no cover - 无 numpy 分支 _POP16 = _COVER_ARR = None def _popcount(mask: int) -> int: return bin(mask).count("1") def _table_index(bearing_deg: float) -> int: return int(round((bearing_deg % 360.0) / _GRID_DEG)) % _GRID_N class JointBelief: """单频道 (位置 × 朝向) 联合可行集。""" def __init__(self, config: ProblemConfig = ProblemConfig()): self.config = config self.field = config.field self.position = ChannelParticles(config) self.masks: dict[int, int] = {} # 格索引 -> 朝向掩码（缺省为全档） self._sample_cache: tuple[int, list[int]] | None = None # ------------------------------------------------------------ 查询 def __len__(self) -> int: return len(self.position) def is_absent(self) -> bool: """联合集空 ⟹ 该频道不存在源（真源保留定理的二维推广）。""" return not self.position.alive def _mask(self, index: int) -> int: return self.masks.get(index, _FULL_MASK) def sample(self, size: int = 256) -> list[int]: """均匀抽样幸存格（评分用；证书始终用完整集合）。""" cached = self._sample_cache if cached is not None and cached[0] == self.position.version: return cached[1] alive = sorted(self.position.alive) if len(alive) > size: step = len(alive) / size alive = [alive[int(k * step) % len(alive)] for k in range(size)] self._sample_cache = (self.position.version, alive) return alive def _mask_popcount(self, index: int) -> int: return _popcount(self._mask(index)) def expected_posterior_radius(self, reader: Vec2, size: int | None = None) -> float: """一次测量后的期望后验 MEC 半径（米）；D1 值函数。 与 :meth:`ChannelParticles.expected_posterior_radius` 同族，差别只在 no_signal 分支概率：定向源不仅会因距离 > r_eff 静默，还会因朝向遮挡 静默，因此 p_vis(p) = P(r_eff ≥ |p−q|) × cover_ratio(p, q) 其中 ``cover_ratio`` 是该格朝向掩码中"可能覆盖 q 方位"的档位占比。 分支半径语义： - direction（可见，非 near）：读数为 q 处的方位 ±δ，后验收缩到该方位 楔形与当前可行集之交，半径取幸存子采样的质心最大距离； - no_signal：定向源的静默**不删空间候选**，位置后验半径保持当前值； - near：后验为 5m 盘。 返回值只用于**选点排序**，不参与任何安全判定；证书仍由凸/网格口径给出。 """ pf = self.position if not pf.alive: return 0.0 if size is None: size = self.config.bayes.score_subsample indices = self.sample(max(2, size)) if not indices: return 0.0 stride = max(len(indices) // max(self.config.bayes.mc_representatives, 1), 1) representatives = indices[::stride] if _np is not None: return self._epr_vector(reader, indices, representatives) return self._epr_python(reader, indices, representatives) # pragma: no cover def _epr_vector(self, reader: Vec2, indices: list[int], representatives: list[int]) -> float: pf = self.position points = _np.array([pf.cell_center(index) for index in indices], dtype=float) masks = _np.array([self._mask(index) for index in indices], dtype=_np.int64) reps = _np.array([pf.cell_center(index) for index in representatives], dtype=float) rep_masks = _np.array([self._mask(index) for index in representatives], dtype=_np.int64) dx, dy = points[:, 0] - reader[0], points[:, 1] - reader[1] dist = _np.hypot(dx, dy) bearing = _np.degrees(_np.arctan2(dy, dx)) rdx, rdy = reps[:, 0] - reader[0], reps[:, 1] - reader[1] rdist = _np.hypot(rdx, rdy) rbearing = _np.degrees(_np.arctan2(rdy, rdx)) cover = _COVER_ARR[_np.round(_np.mod(rbearing, 360.0) / _GRID_DEG) .astype(_np.int64) % _GRID_N] visible_mask = rep_masks & cover bits = (_POP16[visible_mask & 0xFFF] + _POP16[(visible_mask >> 12) & 0xFFF]) cover_ratio = bits / float(HEADING_BINS) p_silent_range = _np.clip((rdist - self.field.r_eff_min_m) / (self.field.r_eff_max_m - self.field.r_eff_min_m), 0.0, 1.0) p_vis = (1.0 - p_silent_range) * cover_ratio p_ns = 1.0 - p_vis gaps = _np.abs((bearing[None, :] - rbearing[:, None] + 180.0) % 360.0 - 180.0) in_wedge = ((gaps <= self.config.field.bearing_error_half_deg + self.config.search.bearing_rounding_margin_deg + 1e-9) & (dist[None, :] <= self.field.r_eff_max_m + pf.margin_m)) eps_radius = pf.spacing_m / 2.0 radii = [] for row in in_wedge: member = points[row] radii.append(_branch_radius_array(member) if len(member) else eps_radius) radii = _np.array(radii, dtype=float) current = pf.mec()[1] near = rdist <= self.field.near_radius_m + pf.margin_m values = _np.where(near, self.field.near_radius_m + pf.margin_m, p_ns * current + p_vis * radii) return float(values.mean()) def _epr_python(self, reader: Vec2, indices: list[int], representatives: list[int]) -> float: # pragma: no cover """纯 Python 回退（与向量版逐分支同语义，供无 numpy 环境与对拍）。""" pf = self.position points = [pf.cell_center(index) for index in indices] masks = [self._mask(index) for index in indices] eps_radius = pf.spacing_m / 2.0 current = pf.mec()[1] half_angle = (self.config.field.bearing_error_half_deg + self.config.search.bearing_rounding_margin_deg) total = 0.0 for rep_index in representatives: rx, ry = pf.cell_center(rep_index) distance = math.hypot(rx - reader[0], ry - reader[1]) if distance <= self.field.near_radius_m + pf.margin_m: total += self.field.near_radius_m + pf.margin_m continue reading = math.degrees(math.atan2(ry - reader[1], rx - reader[0])) cover = _COVER_TABLE[_table_index(reading)] bits = _popcount(self._mask(rep_index) & cover) ratio = bits / float(HEADING_BINS) if distance <= self.field.r_eff_min_m: p_range = 1.0 elif distance >= self.field.r_eff_max_m: p_range = 0.0 else: p_range = ((self.field.r_eff_max_m - distance) / (self.field.r_eff_max_m - self.field.r_eff_min_m)) p_vis = p_range * ratio members = [] for point, mask in zip(points, masks): if math.dist(point, reader) > self.field.r_eff_max_m + pf.margin_m: continue bearing = math.degrees(math.atan2(point[1] - reader[1], point[0] - reader[0])) if heading_gap(bearing, reading) <= half_angle + 1e-9: members.append(point) if members: mx = sum(p[0] for p in members) / len(members) my = sum(p[1] for p in members) / len(members) radius = max(math.hypot(p[0] - mx, p[1] - my) for p in members) else: radius = eps_radius total += (1.0 - p_vis) * current + p_vis * radius return total / max(len(representatives), 1) def prob_detect(self, reader: Vec2, size: int = 256) -> float: """在 ``reader`` 处能看见任一可行 (位置, 朝向) 的概率质量（启发式）。 r_eff ~ U[1000, 1500] 解析积分；朝向按可行档位占比计权。 只用于选点排序，不参与任何安全判定。 """ cells = self.sample(size) if not cells: return 0.0 total = 0.0 span = self.field.r_eff_max_m - self.field.r_eff_min_m for index in cells: gx, gy = self.position.cell_center(index) distance = math.hypot(reader[0] - gx, reader[1] - gy) if distance > self.field.r_eff_max_m: continue if distance <= self.field.r_eff_min_m: p_range = 1.0 else: p_range = (self.field.r_eff_max_m - distance) / span if p_range <= 0.0: continue bearing = math.degrees(math.atan2(reader[1] - gy, reader[0] - gx)) cover = _COVER_TABLE[_table_index(bearing)] bits = bin(self._mask(index) & cover).count("1") if bits: total += p_range * bits / HEADING_BINS return total / len(cells) # ------------------------------------------------------------ 观测更新 def _apply_heading(self, station: Vec2, keep: bool, within_m: float | None = None) -> None: """按可见性约束裁剪朝向档（保守：只删"必然不可能"的档）。 ``keep=True``（direction / near）⟹ 朝向必须**可能**覆盖 station： 保留与方位夹角 ≤ 90° + 半档 + 查表舍入 + 格内角裕量的档。 ``keep=False``（no_signal 且整格必在 r_eff_min 内）⟹ 朝向必须 **确定不覆盖** station：只保留与方位夹角 > 90° − 半档 − 查表舍入 − 格内角裕量的档。 ``格内角裕量 = asin(margin / max(dist − margin, 1))``：格内任一点 相对格心的方位偏差上界。两个方向都必须计入它，否则会误删真源的 朝向档（keep=True 漏计）或保留必然可见的档（keep=False 漏计）。 """ margin_m = self.position.margin_m doomed: list[int] = [] for index in self.position.alive: gx, gy = self.position.cell_center(index) dx, dy = station[0] - gx, station[1] - gy distance = math.hypot(dx, dy) if within_m is not None and distance > within_m: continue bearing = math.degrees(math.atan2(dy, dx)) if distance <= margin_m + 1.0: # 格贴测点：格内方位可指向任意方向，任何一侧都不可据此裁剪 continue angular_margin = math.degrees( math.asin(min(0.9999, margin_m / (distance - margin_m)))) if keep: updated = self._mask(index) & _cover_mask( bearing, 90.0 + _BIN_HALF_DEG + _GRID_DEG / 2 + angular_margin) else: updated = self._mask(index) & _far_mask( bearing, 90.0 - _BIN_HALF_DEG - _GRID_DEG / 2 - angular_margin) if updated == 0: doomed.append(index) self.masks.pop(index, None) elif updated != _FULL_MASK: self.masks[index] = updated if doomed: self.position.discard(doomed) def apply(self, station: Vec2, result: str, svd_deg: float | None = None) -> None: """按读数更新联合可行集（保守，真源 (位置, 朝向) 永不被删）。""" if result == DIRECTION: self.position.apply_measure(station, DIRECTION, svd_deg, directional=True) self._apply_heading(station, keep=True) elif result == NEAR: # near 同样要求可见（评测机先判朝向再判距离） self.position.apply_measure(station, NEAR, directional=True) self._apply_heading(station, keep=True) elif result == NO_SIGNAL: # 距离阈值必须取**保守侧**：只有"整格都必然落在 r_eff_min 圆盘内" # 时，no_signal 才一定由朝向遮挡解释。格心到格内任一点 ≤ margin， # 故阈值取 r_eff_min − margin；取 + margin 会把 1000~1010m 的模糊带 # 也当成"必须不覆盖"，从而可能删掉真源的朝向档（破坏真源保留性）。 self._apply_heading(station, keep=False, within_m=max( 0.0, self.field.r_eff_min_m - self.position.margin_m)) else: raise ValueError(f"未知测向结果: {result!r}") # ------------------------------------------------------------ 诊断 def summary(self) -> dict: """联合集规模快照（写入策略 summary，供审计与论文复算）。""" cells = self.sample(256) bins = [bin(self._mask(index)).count("1") for index in cells] return {"cells": len(self.position), "sampled_mean_heading_bins": (round(sum(bins) / len(bins), 2) if bins else 0.0), "heading_bins_total": HEADING_BINS}