150 lines
7.1 KiB
Python
150 lines
7.1 KiB
Python
import math
|
|
import random
|
|
import unittest
|
|
|
|
from mcm26b.geometry import Measurement, solve, smallest_enclosing_circle
|
|
from mcm26b.geometry.localization import halfplanes, distance
|
|
|
|
|
|
class GeometryTests(unittest.TestCase):
|
|
def test_equilateral_from_legal_bearings(self):
|
|
h = 20 * math.sqrt(3)
|
|
stations = [Measurement(-1000, 0, 1),
|
|
Measurement(540, -500 * math.sqrt(3), 121),
|
|
Measurement(520, h + 500 * math.sqrt(3), 241)]
|
|
result = solve(stations)
|
|
self.assertEqual(result['status'], 'polygon')
|
|
self.assertEqual(len(result['vertices']), 3)
|
|
self.assertAlmostEqual(result['diameter_m'], 40, places=7)
|
|
self.assertAlmostEqual(result['minimum_enclosing_circle']['radius_m'],
|
|
40/math.sqrt(3), places=7)
|
|
self.assertFalse(result['diameter_circle']['covers'])
|
|
self.assertFalse(result['guaranteed_clearable_at_mec_center'])
|
|
|
|
def test_no_observations_and_single_bearing(self):
|
|
self.assertEqual(solve([])['status'], 'unbounded')
|
|
r = solve([Measurement(20, 30, 359.8)])
|
|
self.assertEqual(r['status'], 'unbounded')
|
|
self.assertTrue(r['diameter_infinite'])
|
|
self.assertIsNone(r['diameter_m'])
|
|
|
|
def test_inconsistent(self):
|
|
result = solve([Measurement(-100, 0, 180), Measurement(100, 0, 0)])
|
|
self.assertEqual(result['status'], 'empty')
|
|
|
|
def test_exact_segment_and_point(self):
|
|
segment = [Measurement(0, 0, 0, 0), Measurement(100, 0, 180, 0)]
|
|
r = solve(segment)
|
|
self.assertEqual(r['status'], 'segment')
|
|
self.assertAlmostEqual(r['diameter_m'], 100)
|
|
self.assertTrue(r['diameter_circle']['covers'])
|
|
r = solve(segment + [Measurement(30, -30, 90, 0)])
|
|
self.assertEqual(r['status'], 'point')
|
|
self.assertAlmostEqual(r['vertices'][0][0], 30)
|
|
self.assertAlmostEqual(r['diameter_m'], 0)
|
|
|
|
def test_forward_ray_not_line(self):
|
|
r = solve([Measurement(0, 0, 0, 0), Measurement(-1, 0, 180, 0)])
|
|
self.assertEqual(r['status'], 'empty')
|
|
|
|
def test_angle_wrap_and_repeated_station(self):
|
|
a = [Measurement(-500, -1, 359.9), Measurement(0, -500, 90)]
|
|
b = [Measurement(-500, -1, -.1), Measurement(0, -500, 450)]
|
|
self.assertAlmostEqual(solve(a)['diameter_m'], solve(b)['diameter_m'])
|
|
self.assertAlmostEqual(solve(a+a)['diameter_m'], solve(a)['diameter_m'])
|
|
|
|
def test_close_parallel_remains_finite(self):
|
|
# Two nearly parallel wedges can still be bounded, very far away.
|
|
r = solve([Measurement(0, 0, 1), Measurement(0, 100, -1.000001)])
|
|
self.assertEqual(r['status'], 'polygon')
|
|
self.assertGreater(r['diameter_m'], 1e9)
|
|
|
|
def test_near_antiparallel_thin_wedge_keeps_far_vertex(self):
|
|
"""近平行细长区域不得丢失远端顶点（审核复现案例）。
|
|
|
|
测站相距 1cm、示向度 1 与 358.99999999999：两条近平行边界相交出
|
|
~5.7e10 m 处的远端顶点。旧实现按绝对阈值 PARALLEL_EPS 跳过该平面
|
|
对，漏掉顶点后错误报出 0.143m 直径并给出清除保证。
|
|
"""
|
|
r = solve([Measurement(0, 0, 1), Measurement(0, 0.01, 358.99999999999)])
|
|
self.assertEqual(r['status'], 'polygon')
|
|
self.assertGreater(r['diameter_m'], 5.7e10)
|
|
self.assertFalse(r['guaranteed_clearable_at_mec_center'],
|
|
"细长区域的清除保证必须被拒绝")
|
|
# 远端方向上的可行源 (1000, 0) 必须仍落在最小包围圆内
|
|
mec = r['minimum_enclosing_circle']
|
|
self.assertLessEqual(distance(mec['center'], (1000.0, 0.0)),
|
|
mec['radius_m'] + 1e-6)
|
|
|
|
def test_circle_known_shapes(self):
|
|
c, r = smallest_enclosing_circle([(0,0),(2,0),(2,2),(0,2)])
|
|
self.assertEqual(c, (1,1))
|
|
self.assertAlmostEqual(r, math.sqrt(2))
|
|
c, r = smallest_enclosing_circle([(0,0),(2,0),(.4,.2)])
|
|
self.assertAlmostEqual(r, 1)
|
|
c, r = smallest_enclosing_circle([(0,0),(1,0),(3,0)])
|
|
self.assertAlmostEqual(r, 1.5)
|
|
|
|
def test_random_truth_containment_and_monotonic(self):
|
|
rng = random.Random(20260910)
|
|
for _ in range(100):
|
|
target = (rng.uniform(-300,300), rng.uniform(-300,300))
|
|
ms = []
|
|
for i in range(4):
|
|
ang = (i*90 + rng.uniform(-20,20))*math.pi/180
|
|
radius = rng.uniform(300,1200)
|
|
p = (target[0]+radius*math.cos(ang), target[1]+radius*math.sin(ang))
|
|
bearing = math.degrees(math.atan2(target[1]-p[1],target[0]-p[0]))
|
|
ms.append(Measurement(*p, bearing+rng.uniform(-.99,.99)))
|
|
r3, r4 = solve(ms[:3]), solve(ms)
|
|
self.assertEqual(r4['status'], 'polygon')
|
|
self.assertLessEqual(r4['diameter_m'], r3['diameter_m']+1e-5)
|
|
for m in ms:
|
|
for a,b,c in halfplanes(m):
|
|
self.assertLessEqual(a*target[0]+b*target[1], c+1e-7)
|
|
mec = r4['minimum_enclosing_circle']
|
|
self.assertLessEqual(distance(mec['center'], target), mec['radius_m']+1e-5)
|
|
for v in r4['vertices']:
|
|
self.assertLessEqual(distance(mec['center'], v), mec['radius_m']+1e-5)
|
|
|
|
def test_translation_rotation_scaling(self):
|
|
ms = [Measurement(-500,0,1), Measurement(0,-500,89), Measurement(400,400,225)]
|
|
original = solve(ms)
|
|
a=.37
|
|
changed = [Measurement(1e6+3*(m.x*math.cos(a)-m.y*math.sin(a)),
|
|
-2e6+3*(m.x*math.sin(a)+m.y*math.cos(a)),
|
|
m.bearing_deg+math.degrees(a)) for m in ms]
|
|
moved=solve(changed)
|
|
self.assertAlmostEqual(moved['diameter_m'], original['diameter_m']*3, places=6)
|
|
|
|
def test_invalid_input(self):
|
|
for args in [(float('nan'),0,0),(0,0,float('inf')),(0,0,0,90),(0,0,0,-1),(True,0,0)]:
|
|
with self.assertRaises(ValueError): Measurement(*args)
|
|
|
|
def test_independent_rectangle_clipping_reference(self):
|
|
# A second construction: clip a demonstrably inactive finite box.
|
|
# The production algorithm never uses this box.
|
|
rng=random.Random(4001)
|
|
for _ in range(60):
|
|
ms=[]
|
|
for x,y in [(-500,-300),(600,-200),(0,700)]:
|
|
ms.append(Measurement(x,y,math.degrees(math.atan2(-y,-x))+rng.uniform(-.9,.9)))
|
|
poly=[(-2000.,-2000.),(2000.,-2000.),(2000.,2000.),(-2000.,2000.)]
|
|
for m in ms:
|
|
for a,b,c in halfplanes(m):
|
|
out=[]
|
|
for p,q in zip(poly,poly[1:]+poly[:1]):
|
|
f=a*p[0]+b*p[1]-c;g=a*q[0]+b*q[1]-c
|
|
if f<=0:out.append(p)
|
|
if f*g<0:
|
|
t=f/(f-g);out.append((p[0]+t*(q[0]-p[0]),p[1]+t*(q[1]-p[1])))
|
|
poly=out
|
|
self.assertTrue(poly)
|
|
self.assertTrue(all(max(abs(x),abs(y))<1900 for x,y in poly))
|
|
expected=max(math.dist(p,q) for p in poly for q in poly)
|
|
self.assertAlmostEqual(solve(ms)['diameter_m'],expected,places=6)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main()
|