1

src/mcm26b/belief/__init__.py

@@ -1,24 +1,38 @@
 """P3 逐频道信念状态：候选集合 K_c 维护与频道处理状态。
 
-现状：骨架（P3 起填充）。接口与不变式先行固定，防止后续实现漂移。
-
 核心不变式（验收标准）：
-- 保守性：真源永远属于维护中的候选集合；任何更新不得因误差、
-  圆弧离散或浮点容差把真点排除（自建真值案例中真源始终保留）；
-- direction 读数：K_c ⊆ Ω ∩ W(s, θ±δ) ∩ B(s, 1500)（首次测向构造见
-  geometry.planning.first_bearing_candidate_disk 的解析证书）；
-- no_signal 读数（P4，全向源）：仅可排除 B(s, 1000)；用于定向源时
-  是"位置×朝向"联合约束，单独维护空间候选会误删真源（P5 才启用）；
+- 保守性：真源永远属于维护中的候选集合。候选集用"圆盘外切多边形 ∩
+  测向角域半平面"的凸多边形表示——外切多边形严格包含圆盘，半平面裁剪
+  带正容差，因此每步更新只会放大而不会缩小真实候选集；
+- direction 读数：K_c ⊆ Ω ∩ W(s, θ±δ_eff) ∩ B(s, 1500)，后续读数再交
+  同样的约束；δ_eff = 误差半角 + 示向度舍入裕量（服务器保留两位小数，
+  舍入本身引入 ≤0.005° 偏差）；
+- no_signal 读数（P4，全向源）：基线不据此删空间候选——对定向源它是
+  "位置×朝向"联合约束，单独维护空间候选会误删真源（P5 才启用）；
+  "该频道不存在源"的证明由覆盖账本（coverage.CoverageLedger）给出；
+- near 读数：源在 5m 内，候选集收缩为 B(s, 5)，可直接产生清除证书；
 - /clear 成功：状态转 CLEARED；失败只说明"该频道 20m 内无可清除源"，
-  不得直接删除超出 20m 的候选。
+  不得直接删除超出 20m 的候选，必须先补测再重试。
 """
 
 from __future__ import annotations
 
+import math
 from dataclasses import dataclass, field
 from enum import Enum
 
-from ..config import ProblemConfig
+from ..config import ProblemConfig, Vec2
+from ..geometry.localization import smallest_enclosing_circle
+from ..protocol.rules import DIRECTION, NEAR, NO_SIGNAL
+
+# 半平面裁剪的正容差（米）。坐标量级 ~2100，浮点误差远小于它；
+# 容差只向"保留更多候选"的方向倾斜，不破坏保守性。
+_CLIP_TOL_M = 1e-6
+_DEDUPE_TOL_M = 1e-7
+
+
+class InconsistentBelief(RuntimeError):
+    """读数把候选集剪空等违背保守性的情形；须停机核查而非继续跑。"""
 
 
 class ChannelStatus(Enum):
@@ -29,11 +43,185 @@ class ChannelStatus(Enum):
     ABSENT = "absent"            # 覆盖证明不存在（P4）
 
 
+def circumscribed_polygon(center: Vec2, radius_m: float, sides: int) -> list[Vec2]:
+    """圆的外切正多边形顶点（外接半径 r/cos(π/n)），严格包含圆盘。
+
+    近似误差（外接圆超出圆盘的部分）为 r(1/cos(π/n) - 1) ≤ r(1-cos(π/n))·sec²，
+    调用方用 (1-cos(π/n)) 口径记账即可（略保守偏大）。
+    """
+    if sides < 8:
+        raise ValueError("外切多边形边数过少，近似误差不可控")
+    circumradius = radius_m / math.cos(math.pi / sides)
+    step = 2 * math.pi / sides
+    return [(center[0] + circumradius * math.cos(step * i + step / 2),
+             center[1] + circumradius * math.sin(step * i + step / 2))
+            for i in range(sides)]
+
+
+def polygon_halfplanes(polygon: list[Vec2]) -> list[tuple[float, float, float]]:
+    """CCW 多边形 -> 外法线半平面列表 (a, b, c)，含义 a·x + b·y <= c。"""
+    planes = []
+    count = len(polygon)
+    for index in range(count):
+        p, q = polygon[index], polygon[(index + 1) % count]
+        dx, dy = q[0] - p[0], q[1] - p[1]
+        norm = math.hypot(dx, dy)
+        if norm <= 1e-12:
+            continue
+        a, b = dy / norm, -dx / norm      # CCW 的右法线朝外
+        planes.append((a, b, a * p[0] + b * p[1]))
+    return planes
+
+
+def clip_halfplane(polygon: list[Vec2], a: float, b: float, c: float,
+                   tol: float = _CLIP_TOL_M) -> list[Vec2]:
+    """Sutherland–Hodgman 凸多边形半平面裁剪，保留 a·x + b·y <= c + tol。
+
+    边界交点取在无容差直线上；进入判定带正容差，保证"贴边"的真点不丢。
+    """
+    if not polygon:
+        return []
+    output: list[Vec2] = []
+    count = len(polygon)
+    for index in range(count):
+        p, q = polygon[index], polygon[(index + 1) % count]
+        fp = a * p[0] + b * p[1] - c
+        fq = a * q[0] + b * q[1] - c
+        p_in, q_in = fp <= tol, fq <= tol
+        if p_in:
+            output.append(p)
+        if p_in != q_in:
+            ratio = fp / (fp - fq)
+            output.append((p[0] + ratio * (q[0] - p[0]),
+                           p[1] + ratio * (q[1] - p[1])))
+    return _dedupe(output)
+
+
+def _dedupe(points: list[Vec2], tol: float = _DEDUPE_TOL_M) -> list[Vec2]:
+    unique: list[Vec2] = []
+    for point in points:
+        if not unique or math.dist(point, unique[-1]) > tol:
+            unique.append(point)
+    if len(unique) > 1 and math.dist(unique[0], unique[-1]) <= tol:
+        unique.pop()
+    return unique
+
+
+class CandidateRegion:
+    """某频道候选集合的保守凸外包多边形（顶点按 CCW；允许退化为 1-2 点）。"""
+
+    def __init__(self, polygon: list[Vec2], slack_m: float = 0.0):
+        self.polygon = list(polygon)
+        self.slack_m = float(slack_m)   # 外包近似引入的半径向误差记账
+        self._mec: tuple[Vec2, float] | None = None
+
+    # ------------------------------------------------------------ 构造
+    @classmethod
+    def disk(cls, center: Vec2, radius_m: float, sides: int,
+             slack_m: float | None = None) -> "CandidateRegion":
+        if slack_m is None:
+            slack_m = radius_m * (1 - math.cos(math.pi / sides))
+        return cls(circumscribed_polygon(center, radius_m, sides), slack_m)
+
+    @classmethod
+    def initial(cls, station: Vec2, config: ProblemConfig) -> "CandidateRegion":
+        """首个 direction 后的基础集：Ω ∩ B(station, r_max) 的保守外包。"""
+        field, sides = config.field, config.search.polygon_sides
+        slack = field.omega_radius_m * (1 - math.cos(math.pi / sides))
+        region = cls.disk((0.0, 0.0), field.omega_radius_m, sides, slack)
+        region.clip_disk(station, field.r_eff_max_m, sides)
+        return region
+
+    # ------------------------------------------------------------ 收缩
+    def _clip(self, a: float, b: float, c: float) -> None:
+        polygon = clip_halfplane(self.polygon, a, b, c)
+        if not polygon:
+            raise InconsistentBelief(
+                f"候选区域被半平面 ({a:.6f},{b:.6f},{c:.6f}) 剪空：读数与保守集矛盾")
+        self.polygon = polygon
+        self._mec = None
+
+    def clip_disk(self, center: Vec2, radius_m: float, sides: int) -> None:
+        """与圆盘 B(center, radius) 的外切多边形求交（近似误差入账）。"""
+        for a, b, c in polygon_halfplanes(circumscribed_polygon(center, radius_m, sides)):
+            self._clip(a, b, c)
+        self.slack_m += radius_m * (1 - math.cos(math.pi / sides))
+
+    def clip_wedge(self, station: Vec2, bearing_deg: float,
+                   half_angle_deg: float) -> None:
+        """与测向角域 W(station, bearing±half_angle) 求交（半角须 <90°）。
+
+        两个边界半平面与 localization.halfplanes 同口径，0°/360° 跨界由
+        sin/cos 直接处理，无需分支。
+        """
+        if not 0 < half_angle_deg < 90:
+            raise ValueError("角域半角须在 (0, 90) 度")
+        lower = math.radians(bearing_deg - half_angle_deg)
+        upper = math.radians(bearing_deg + half_angle_deg)
+        sx, sy = float(station[0]), float(station[1])
+        self._clip(math.sin(lower), -math.cos(lower),
+                   math.sin(lower) * sx - math.cos(lower) * sy)
+        self._clip(-math.sin(upper), math.cos(upper),
+                   -math.sin(upper) * sx + math.cos(upper) * sy)
+
+    # ------------------------------------------------------------ 查询
+    def mec(self) -> tuple[Vec2, float]:
+        """最小包围圆（圆心, 半径）。半径是真实候选集 ρ 的上界。"""
+        if self._mec is None:
+            if not self.polygon:
+                raise InconsistentBelief("候选区域为空")
+            self._mec = smallest_enclosing_circle(self.polygon)
+        return self._mec
+
+    def witnesses(self, spacing_m: float) -> list[Vec2]:
+        """顶点 + 边界按间距细分的采样点（最坏后验评估用）。"""
+        if len(self.polygon) < 3:
+            return list(self.polygon)
+        points = list(self.polygon)
+        count = len(self.polygon)
+        for index in range(count):
+            p, q = self.polygon[index], self.polygon[(index + 1) % count]
+            length = math.dist(p, q)
+            steps = int(length // spacing_m)
+            for k in range(1, steps + 1):
+                ratio = k / (steps + 1)
+                points.append((p[0] + ratio * (q[0] - p[0]),
+                               p[1] + ratio * (q[1] - p[1])))
+        return _dedupe(points, tol=1e-9)
+
+    def contains(self, point: Vec2, tol_m: float = 0.05) -> bool:
+        """点是否在候选集内（带容差；用于真源保留性检验，不做删点依据）。"""
+        if not self.polygon:
+            return False
+        if len(self.polygon) == 1:
+            return math.dist(point, self.polygon[0]) <= tol_m + self.slack_m
+        if len(self.polygon) == 2:
+            p, q = self.polygon
+            return _point_segment_distance(point, p, q) <= tol_m + self.slack_m
+        for index in range(len(self.polygon)):
+            p = self.polygon[index]
+            q = self.polygon[(index + 1) % len(self.polygon)]
+            if (q[0] - p[0]) * (point[1] - p[1]) - (q[1] - p[1]) * (point[0] - p[0]) \
+                    < -tol_m - self.slack_m:
+                return False
+        return True
+
+
+def _point_segment_distance(point: Vec2, p: Vec2, q: Vec2) -> float:
+    vx, vy = q[0] - p[0], q[1] - p[1]
+    length_sq = vx * vx + vy * vy
+    if length_sq <= 1e-18:
+        return math.dist(point, p)
+    t = max(0.0, min(1.0, ((point[0] - p[0]) * vx + (point[1] - p[1]) * vy) / length_sq))
+    return math.dist(point, (p[0] + t * vx, p[1] + t * vy))
+
+
 @dataclass
 class ChannelBelief:
     channel: int
     status: ChannelStatus = ChannelStatus.UNKNOWN
-    measurements: list = field(default_factory=list)   # (位置, 结果, svd_deg) 记录
+    region: CandidateRegion | None = None
+    measurements: list = field(default_factory=list)   # 逐步观测记录（审计用）
 
 
 class BeliefState:
@@ -45,14 +233,53 @@ class BeliefState:
                          for channel in range(config.field.channel_min,
                                               config.field.channel_max + 1)}
 
-    def update_measure(self, channel: int, position, result: str,
-                       svd_deg: float | None) -> None:
-        """按读数收敛候选集合。未实现：需要几何模块的在线圆盘角域交。"""
-        raise NotImplementedError("P3：接入 geometry 的圆盘约束角域交后实现")
+    @property
+    def effective_half_angle_deg(self) -> float:
+        """示向度误差半角 + 舍入裕量：服务器保留两位小数引入 ≤0.005° 偏差。"""
+        return (self.config.field.bearing_error_half_deg
+                + self.config.search.bearing_rounding_margin_deg)
 
-    def candidate_set(self, channel: int):
-        """返回该频道当前候选集合（保守外包形式）。未实现。"""
-        raise NotImplementedError("P3")
+    def update_measure(self, channel: int, position: Vec2, result: str,
+                       svd_deg: float | None) -> None:
+        """按读数收敛候选集合；真源保留性由保守表示保证。"""
+        belief = self.channels[channel]
+        belief.measurements.append({"position": tuple(position), "result": result,
+                                    "svd_deg": svd_deg})
+        if result == DIRECTION:
+            if svd_deg is None:
+                raise ValueError("direction 读数必须携带 svd_deg")
+            sides = self.config.search.polygon_sides
+            if belief.region is None:
+                belief.region = CandidateRegion.initial(position, self.config)
+            else:
+                # 后续 direction 同样隐含距离 <= r_max [题面]
+                belief.region.clip_disk(position, self.config.field.r_eff_max_m, sides)
+            belief.region.clip_wedge(position, svd_deg, self.effective_half_angle_deg)
+            belief.status = ChannelStatus.DETECTED
+        elif result == NEAR:
+            sides = self.config.search.polygon_sides
+            if belief.region is None:
+                belief.region = CandidateRegion.disk(
+                    tuple(position), self.config.field.near_radius_m, sides)
+            else:
+                belief.region.clip_disk(position, self.config.field.near_radius_m, sides)
+            belief.status = ChannelStatus.DETECTED
+        elif result == NO_SIGNAL:
+            # P3/P4 基线：不据此删空间候选（缺席证明由覆盖账本给出）。
+            pass
+        else:
+            raise ValueError(f"未知测向结果: {result!r}")
+
+    def candidate_set(self, channel: int) -> CandidateRegion | None:
+        """该频道当前候选集合（保守外包形式）；尚无 direction/near 时为 None。"""
+        return self.channels[channel].region
 
     def mark_cleared(self, channel: int) -> None:
         self.channels[channel].status = ChannelStatus.CLEARED
+
+    def mark_absent(self, channel: int) -> None:
+        self.channels[channel].status = ChannelStatus.ABSENT
+
+    def channels_by_status(self, *statuses: ChannelStatus) -> list[int]:
+        return sorted(channel for channel, belief in self.channels.items()
+                      if belief.status in statuses)