1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
|
from functools import reduce
from math import pi
import math
def merge_intervals(intervals):
sorted_by_lower_bound = sorted(intervals, key=lambda tup: tup[0])
merged = []
for higher in sorted_by_lower_bound:
if not merged:
merged.append(higher)
else:
lower = merged[-1]
# test for intersection between lower and higher:
# we know via sorting that lower[0] <= higher[0]
if higher[0] <= lower[1]:
upper_bound = max(lower[1], higher[1])
merged[-1] = (lower[0], upper_bound) # replace by merged interval
else:
merged.append(higher)
return merged
def clean_intervals(intervals):
return list(filter(lambda i : i[0]!=i[1],intervals))
def canonicalize_angle_interval(i):
a=i[0] % (2*pi)
b=i[1] % (2*pi)
if a <= b:
return [(a,b)]
else:
return [(0,b),(a,2*pi)]
def invert_angle_intervals(intervals):
flattened = reduce(lambda a,b:a+b, (map(lambda a: [a[0],a[1]], intervals)))
return clean_intervals(list(zip([0]+flattened, flattened+[2*pi]))[::2])
def find_largest_angle_interval(intervals):
if (intervals[0][0]==0 and intervals[-1][1]==2*pi):
# these two intervals actually are one.
intervals_ = intervals[1:-1] + [(intervals[-1][0], intervals[0][1]+2*pi)]
else:
intervals_ = intervals
return max(intervals_, key=lambda p:p[1]-p[0])
def get_point_angle(origin, p):
dx = p[0] - origin[0]
dy = p[1] - origin[1]
return math.atan2(dy,dx) % (2*pi)
def check_point_in_interval(origin, p, interval):
ang = get_point_angle(origin, p)
a,b = interval[0]%(2*pi), interval[1]%(2*pi)
if a <= b:
return (a <= ang and ang <= b)
else:
return (a <= ang or ang <= b)
def intervals_intersect(int1_, int2_):
int1s = canonicalize_angle_interval(int1_)
int2s = canonicalize_angle_interval(int2_)
for int1 in int1s:
for int2 in int2s:
if (max(int1[0],int2[0]) <= min(int1[1],int2[1])):
return True
return False
def intersection(int1s, int2s):
#expects merged, canonicalized intervals, returns overlap-free canonicalized intervals
result = []
for int1 in int1s:
for int2 in int2s:
if (max(int1[0],int2[0]) <= min(int1[1],int2[1])):
result += [(max(int1[0],int2[0]), min(int1[1],int2[1]))]
return result
def interval_area(ints):
result = 0
for i in merge_intervals(ints):
result += i[1]-i[0]
return result
def interval_occupied_by_cell(origin, cell):
ang = get_point_angle(origin, cell.pos)
dist = math.sqrt( (cell.pos[0]-origin[0])**2 + (cell.pos[1]-origin[1])**2 )
if cell.size >= dist:
corridor_halfwidth = math.pi/2
else:
corridor_halfwidth = math.asin(cell.size / dist)
return (ang-corridor_halfwidth, ang+corridor_halfwidth)
def check_cell_in_interval(origin, cell, interval):
return intervals_intersect(interval, interval_occupied_by_cell(origin,cell))
def get_cells_in_interval(origin, interval, cells):
return list(filter(lambda x: check_point_in_interval(origin, x.pos, interval), cells))
|
