270 lines
12 KiB
Python
270 lines
12 KiB
Python
"""Intersect forward bearing wedges without an artificial bounding box.
|
|
|
|
All calculations are floating point with normalized station coordinates.
|
|
The reference algorithm deliberately favors auditability over asymptotic speed.
|
|
"""
|
|
|
|
from dataclasses import dataclass
|
|
from itertools import combinations
|
|
from math import cos, sin, radians, hypot, isfinite
|
|
from random import Random
|
|
|
|
Point = tuple[float, float]
|
|
EPS = 1e-9
|
|
PARALLEL_EPS = 1e-12
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Measurement:
|
|
x: float
|
|
y: float
|
|
bearing_deg: float
|
|
half_angle_deg: float = 1.0
|
|
|
|
def __post_init__(self):
|
|
values = (self.x, self.y, self.bearing_deg, self.half_angle_deg)
|
|
if any(isinstance(v, bool) or not isinstance(v, (int, float))
|
|
or not isfinite(v) for v in values):
|
|
raise ValueError("坐标、示向度和误差半角必须是有限数值")
|
|
if not 0 <= self.half_angle_deg < 90:
|
|
raise ValueError("误差半角必须在 [0, 90) 度；题设为1度")
|
|
|
|
|
|
def cross(a: Point, b: Point) -> float:
|
|
return a[0] * b[1] - a[1] * b[0]
|
|
|
|
|
|
def distance(a: Point, b: Point) -> float:
|
|
return hypot(a[0] - b[0], a[1] - b[1])
|
|
|
|
|
|
def halfplanes(m: Measurement) -> list[tuple[float, float, float]]:
|
|
"""Return a*x+b*y<=c with unit normals, including the forward constraint."""
|
|
theta = m.bearing_deg % 360
|
|
lower, upper = radians(theta - m.half_angle_deg), radians(theta + m.half_angle_deg)
|
|
middle = radians(theta)
|
|
normals = [(sin(lower), -cos(lower)), (-sin(upper), cos(upper)),
|
|
(-cos(middle), -sin(middle))]
|
|
return [(a, b, a * m.x + b * m.y) for a, b in normals]
|
|
|
|
|
|
def convex_hull(points: list[Point], eps: float = EPS) -> list[Point]:
|
|
"""Monotone chain, CCW, no repeated final vertex; retain segment endpoints."""
|
|
ordered = sorted(points)
|
|
unique = []
|
|
for p in ordered:
|
|
if not unique or distance(p, unique[-1]) > eps:
|
|
unique.append(p)
|
|
if len(unique) <= 2:
|
|
return unique
|
|
|
|
def chain(seq):
|
|
out = []
|
|
for p in seq:
|
|
while len(out) >= 2:
|
|
a = (out[-1][0] - out[-2][0], out[-1][1] - out[-2][1])
|
|
b = (p[0] - out[-1][0], p[1] - out[-1][1])
|
|
# Distance-to-line tolerance, not an unscaled area tolerance.
|
|
if cross(a, b) > eps * max(hypot(*a), hypot(*b), eps):
|
|
break
|
|
out.pop()
|
|
out.append(p)
|
|
return out
|
|
|
|
return chain(unique)[:-1] + chain(reversed(unique))[:-1]
|
|
|
|
|
|
def diameter(points: list[Point]):
|
|
if not points:
|
|
raise ValueError("空点集没有本程序定义下的直径")
|
|
a = b = points[0]
|
|
value = 0.0
|
|
for p, q in combinations(points, 2):
|
|
d = distance(p, q)
|
|
if d > value:
|
|
value, a, b = d, p, q
|
|
return value, (a, b)
|
|
|
|
|
|
def _circumcircle(a: Point, b: Point, c: Point):
|
|
u = (b[0] - a[0], b[1] - a[1])
|
|
v = (c[0] - a[0], c[1] - a[1])
|
|
det = 2 * cross(u, v)
|
|
if abs(det) <= PARALLEL_EPS * max(hypot(*u) * hypot(*v), 1e-300):
|
|
return None
|
|
uu, vv = u[0] ** 2 + u[1] ** 2, v[0] ** 2 + v[1] ** 2
|
|
center = (a[0] + (uu * v[1] - vv * u[1]) / det,
|
|
a[1] + (u[0] * vv - v[0] * uu) / det)
|
|
return center, distance(center, a)
|
|
|
|
|
|
def _enclosing_circle_reference(points: list[Point], eps: float = EPS):
|
|
"""Enumerate all 1/2/3-point supports; O(m^4), appropriate for small m.
|
|
|
|
Radius is recomputed against every vertex to preserve numerical containment.
|
|
Coordinates should be normalized when there are very large offsets.
|
|
"""
|
|
if not points:
|
|
raise ValueError("空集不计算最小包围圆")
|
|
xs, ys = [p[0] for p in points], [p[1] for p in points]
|
|
best_c = ((min(xs) + max(xs)) / 2, (min(ys) + max(ys)) / 2)
|
|
best_r = max(distance(best_c, p) for p in points)
|
|
|
|
def consider(center, radius):
|
|
nonlocal best_c, best_r
|
|
if radius > best_r + eps:
|
|
return
|
|
actual = max(distance(center, p) for p in points)
|
|
if actual <= radius + eps and actual < best_r:
|
|
best_c, best_r = center, actual
|
|
|
|
for p in points:
|
|
consider(p, 0.0)
|
|
for a, b in combinations(points, 2):
|
|
consider(((a[0] + b[0]) / 2, (a[1] + b[1]) / 2), distance(a, b) / 2)
|
|
for a, b, c in combinations(points, 3):
|
|
circle = _circumcircle(a, b, c)
|
|
if circle is not None:
|
|
consider(*circle)
|
|
return best_c, best_r
|
|
|
|
|
|
def smallest_enclosing_circle(points: list[Point], eps: float = EPS):
|
|
"""Welzl's randomized incremental algorithm, using an explicit call stack.
|
|
|
|
The fixed shuffle makes results reproducible. Expected linear work refers
|
|
to randomized input order, not a worst-case guarantee for this fixed seed.
|
|
Normalize internally so this public helper also handles translated inputs.
|
|
The exhaustive reference is retained for independent regression comparisons.
|
|
"""
|
|
if not points:
|
|
raise ValueError("空集不计算最小包围圆")
|
|
if not isfinite(eps) or eps <= 0:
|
|
raise ValueError("eps必须为有限正数")
|
|
if any(len(p) != 2 or any(isinstance(x, bool) or not isinstance(x, (int, float))
|
|
or not isfinite(x) for x in p) for p in points):
|
|
raise ValueError("点集必须由有限二维坐标组成")
|
|
origin = points[0]
|
|
scale = max(distance(origin, p) for p in points)
|
|
if scale == 0:
|
|
return tuple(origin), 0.0
|
|
local = [((p[0]-origin[0])/scale, (p[1]-origin[1])/scale) for p in points]
|
|
tolerance = min(eps/scale, EPS)
|
|
Random(20260910).shuffle(local)
|
|
# Frame = (remaining prefix length, forced boundary, returning from child).
|
|
stack = [(len(local), (), False)]
|
|
circle = None
|
|
while stack:
|
|
n, boundary, returning = stack.pop()
|
|
if n == 0 or len(boundary) == 3:
|
|
# Forced support points must lie ON the boundary. For three points
|
|
# use their circumcircle, even when their unconstrained MEC is smaller.
|
|
circle = _circumcircle(*boundary) if len(boundary) == 3 else None
|
|
if circle is None and boundary:
|
|
circle = _enclosing_circle_reference(list(boundary), tolerance)
|
|
elif not returning:
|
|
stack.append((n, boundary, True))
|
|
stack.append((n-1, boundary, False))
|
|
elif circle is None or distance(circle[0], local[n-1]) > circle[1] + tolerance:
|
|
stack.append((n-1, boundary + (local[n-1],), False))
|
|
center = (origin[0]+circle[0][0]*scale, origin[1]+circle[0][1]*scale)
|
|
# Recompute after transforming back: returned circle contains all input points
|
|
# to distance() precision, including any roundoff from the large translation.
|
|
return center, max(distance(center, p) for p in points)
|
|
|
|
|
|
def solve(measurements: list[Measurement], eps: float = EPS) -> dict:
|
|
"""Solve Q1's pure angular intersection (no disk/range prior added).
|
|
|
|
Nonempty intersections of <180-degree forward wedges are pointed polyhedra,
|
|
so they possess a vertex, even if unbounded. Feasibility follows from vertex
|
|
enumeration; unboundedness follows from a nonzero recession direction.
|
|
Empty diameter is null, unbounded diameter is null with diameter_infinite=true.
|
|
"""
|
|
if not isfinite(eps) or eps <= 0:
|
|
raise ValueError("eps必须为有限正数")
|
|
if not measurements:
|
|
return {"status": "unbounded", "vertices": [], "diameter_m": None,
|
|
"diameter_infinite": True, "recession_direction": [1.0, 0.0],
|
|
"message": "没有观测，整个平面均可行"}
|
|
if len(measurements) > 100:
|
|
raise ValueError("参考实现最多接收100个检测点；更大规模应使用半平面交算法")
|
|
ox = sum(m.x for m in measurements) / len(measurements)
|
|
oy = sum(m.y for m in measurements) / len(measurements)
|
|
scale = max(1.0, max(hypot(m.x - ox, m.y - oy) for m in measurements))
|
|
local = [Measurement((m.x - ox) / scale, (m.y - oy) / scale,
|
|
m.bearing_deg, m.half_angle_deg) for m in measurements]
|
|
planes = [h for m in local for h in halfplanes(m)]
|
|
|
|
def inside(p):
|
|
return all(a * p[0] + b * p[1] <= c + eps for a, b, c in planes)
|
|
|
|
feasible_vertices = []
|
|
for (a, b, c), (d, e, f) in combinations(planes, 2):
|
|
det = a * e - b * d
|
|
if abs(det) <= PARALLEL_EPS:
|
|
continue
|
|
p = ((c * e - b * f) / det, (a * f - c * d) / det)
|
|
if inside(p):
|
|
feasible_vertices.append(p)
|
|
|
|
if not feasible_vertices:
|
|
return {"status": "empty", "vertices": [], "diameter_m": None,
|
|
"diameter_infinite": False, "tolerance_m": eps * scale,
|
|
"message": "测向角域没有公共点；检查频道、数据及误差界"}
|
|
|
|
# A nontrivial 2D recession cone has a ray tangent to an input halfplane.
|
|
# Use a much tighter angular threshold than the positional feasibility epsilon.
|
|
ray = None
|
|
for a, b, _ in planes:
|
|
for direction in ((b, -a), (-b, a)):
|
|
if all(d * direction[0] + e * direction[1] <= 2e-14
|
|
for d, e, _ in planes):
|
|
ray = direction
|
|
break
|
|
if ray is not None:
|
|
break
|
|
|
|
hull = convex_hull(feasible_vertices, eps)
|
|
|
|
def world(p):
|
|
return [ox + p[0] * scale, oy + p[1] * scale]
|
|
|
|
result = {"status": "unbounded" if ray else
|
|
("point" if len(hull) == 1 else "segment" if len(hull) == 2 else "polygon"),
|
|
"vertices": [world(p) for p in hull], "tolerance_m": eps * scale,
|
|
"diameter_infinite": ray is not None}
|
|
if ray is not None:
|
|
result.update(diameter_m=None, recession_direction=list(ray),
|
|
message="存在非零衰退方向，区域无界，直径为无穷大")
|
|
return result
|
|
|
|
d, pair = diameter(hull)
|
|
midpoint = ((pair[0][0] + pair[1][0]) / 2, (pair[0][1] + pair[1][1]) / 2)
|
|
distances = [distance(midpoint, p) for p in hull]
|
|
witness_index = max(range(len(hull)), key=distances.__getitem__)
|
|
max_distance = distances[witness_index]
|
|
center, radius = smallest_enclosing_circle(hull, eps)
|
|
area = abs(sum(cross(hull[i], hull[(i + 1) % len(hull)])
|
|
for i in range(len(hull)))) / 2
|
|
result.update(diameter_m=d * scale, diameter_pair=[world(p) for p in pair],
|
|
area_m2=area * scale ** 2,
|
|
diameter_circle={"center": world(midpoint), "radius_m": d * scale / 2,
|
|
"max_vertex_distance_m": max_distance * scale,
|
|
"covers": max_distance <= d / 2 + eps * 4,
|
|
"coverage_tolerance_m": 4 * eps * scale,
|
|
"max_excess_m": (max_distance-d/2) * scale,
|
|
"outside_vertex_indices": [i for i, value in enumerate(distances)
|
|
if value > d/2 + 4*eps],
|
|
"witness_vertex": world(hull[witness_index])
|
|
if max_distance > d/2 + 4*eps else None},
|
|
minimum_enclosing_circle={"center": world(center), "radius_m": radius * scale},
|
|
guaranteed_clearable_at_mec_center=radius * scale <= 20 - eps * scale * 4,
|
|
clearance_certificate={"model": "angular_intersection_only",
|
|
"clear_radius_m": 20.0,
|
|
"numerical_margin_m": 4*eps*scale,
|
|
"radius_with_margin_m": (radius+4*eps)*scale,
|
|
"remaining_margin_m": 20-(radius+4*eps)*scale,
|
|
"center": world(center)})
|
|
return result
|