forked from sbochkar/distributed_coverage_planner
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathdecomposition_processing.py
83 lines (62 loc) · 2.3 KB
/
decomposition_processing.py
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
from shapely.geometry import Polygon
from shapely.geometry import LineString
from shapely.geometry import Point
def collinear_correction(decomp):
for poly in decomp:
boundary = poly[0]
i = 0
while i < len(boundary):
p1 = boundary[i]
p2 = boundary[(i+1)%len(boundary)]
p3 = boundary[(i+2)%len(boundary)]
if cuts.collinear(p1, p2, p3):
del boundary[(i+1)%len(boundary)]
i += 1
def compute_adjacency(decomposition=[]):
"""
Computes an adjacency relation for the polygons in the decomposition.
In the effect, computes a graph where the nodes are polygons and the edges
represent adjacency between polygons.
Assumption:
The polygons are considered adjacent if their boundaries intersect at an
edge. If they only touch at a point, then will not be considered
adjacent.
Params:
decomposition: A list of polygons in the canonical form.
Returns:
A 2D list representing adjacency relation between polygons.
"""
# Initialize the 2D matric with None values.
adjMatrix = [[False for i in range(len(decomposition))] for i in range(len(decomposition))]
for polyAIdx in range(len(decomposition)):
for polyBIdx in range(polyAIdx + 1, len(decomposition)):
try:
polyAShapely = Polygon(*decomposition[polyAIdx])
polyBShapely = Polygon(*decomposition[polyBIdx])
except:
print("Computing adjacency but decomposition contains invalid polygons")
return []
if not polyAShapely.touches(polyBShapely):
continue
intersection = polyAShapely.intersection(polyBShapely)
if type(intersection) is Point:
continue
else:
adjMatrix[polyAIdx][polyBIdx] = True
adjMatrix[polyBIdx][polyAIdx] = True
return adjMatrix
if __name__ == '__main__':
# If package is launched from cmd line, run sanity checks
global DEBUG_LEVEL
DEBUG_LEVEL = 0 #0x8+0x4
print("\nSanity tests for pairwise reoptimization.\n")
polySet = [[[(0, 0), (1, 0), (1, 1), (0, 1)], []],
[[(1, 0), (2, 0), (2, 1), (1, 1)], []],
[[(1, 1), (2, 1), (2, 2), (1, 2)], []],
[[(0, 1), (1, 1), (1, 2), (0, 2)], []]]
print(compute_adjacency(polySet))
polySet = [[[(0, 0), (1, 0), (1, 1), (0.5, 0.5), (0, 1)], []],
[[(1, 0), (2, 0), (2, 1), (1, 1)], []],
[[(1, 1), (2, 1), (2, 2), (1, 2)], []],
[[(0, 1), (0.5, 0.5), (1, 1), (1, 2), (0, 2)], []]]
print(compute_adjacency(polySet))