forked from sbochkar/distributed_coverage_planner
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmin_alt_discrt.py
158 lines (120 loc) · 4.44 KB
/
min_alt_discrt.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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
from shapely.geometry import LinearRing
from shapely.geometry import LineString
from shapely.geometry import Polygon
from shapely import affinity
def discritize_set(D, width):
"""
Wrapper for discritization. Given a set of polygon in standard form.
Generate segments
"""
segments = []
for poly in D:
segments.extend(discritize(poly, width))
return segments
def discritize(P, width):
"""
Function will discritize the free space of a given polygon with minimum
number of lines
:param P: polygon in standard form
:param width: distance between lines
:return lines: a set of segments which could be lines of points
"""
altitude, theta = alt.get_min_altitude(P)
segments = populate_with_lines(P, width, theta)
return segments
def populate_with_lines(P, width, theta):
"""
Populate the free space of P with lines oriented at theta space width appart
Approach:
Utilize from shapely:
Polygon, parallel_offset
Get the chains of exterior and holes
Parallel offset them by width in appropriate directions
Create a polygon with the new chains
If valid, generate lines on this
Return coordinates
Thigns to watch out for:
What to do with invalid polygons
Narrow corridors between holes may result in no lines even when
there is space for it. Because of equidistance between lines
We can get disconnected polygons if there narrow corridors
The uncovered area in the end of the polygon can be handled easily here
Returns a set of lines and their coordinates
"""
P = rotation.rotate_polygon(P, -theta)
# Parallel offset the boundaries of ext and holes
lr_ext = LinearRing(P[0])
offset_ext = lr_ext.parallel_offset(width/2, side='left', join_style=1)
lr_new_holes = []
for hole in P[1]:
lr_hole = LinearRing(hole)
offset_hole = lr_hole.parallel_offset(width/2, side='left', join_style=1)
lr_new_holes.append(offset_hole)
if offset_ext.is_empty:
print offset_ext
print("Line generation ERROR: Shrunk polygon is not valid")
return []
shrunk_polygon = Polygon(offset_ext, lr_new_holes)
if not shrunk_polygon.is_valid:
print("Line generation ERROR: Shrunk polygon is not valid")
return []
minx, miny, maxx, maxy = shrunk_polygon.bounds
# Start generating lines from left to right equidistance from each other
segments = []
cur_x = minx
finishing_touches = False
while (cur_x <= maxx) or (finishing_touches):
if finishing_touches:
cur_x = maxx-0.001
# Crudely deal with the very first line
if cur_x == minx:
test_line = LineString([(cur_x+0.001, miny), (cur_x+0.001, maxy)])
else:
test_line = LineString([(cur_x, miny), (cur_x, maxy)])
intersection = shrunk_polygon.intersection(test_line)
# Handle each type of intersection separatly
if intersection.geom_type == "Point":
new_coord = rotation.rotate_points(intersection.coords[:], theta)
segments.append(classes.PointSegment(*new_coord))
elif intersection.geom_type == "LineString":
# Rotate back and append
new_coords = rotation.rotate_points(intersection.coords[:], theta)
segments.append(classes.LineSegment(new_coords))
elif intersection.geom_type == "MultiLineString":
for line in intersection:
new_coords = rotation.rotate_points(line.coords[:], theta)
segments.append(classes.LineSegment(new_coords))
elif intersection.geom_type == "GeometricCollection":
for element in intersection:
if element.geom_type == "Point":
new_coord = rotation.rotate_points(element.coords[:], theta)
segments.append(classes.PointSegment(*new_coord))
elif element.geom_type == "LineString":
new_coords = rotation.rotate_points(element.coords[:], theta)
segments.append(classes.LineSegment(new_coords))
elif element.geom_type == "MultiLineString":
for line in element:
new_coords = rotation.rotate_points(line.coords[:], theta)
segments.append(classes.LineSegment(new_coords))
cur_x += width
if (cur_x > maxx) and (cur_x <= maxx+width/2):
finishing_touches = True
else:
finishing_touches = False
# Process the last segment
return segments
if __name__ == '__main__':
if __package__ is None:
import os, sys
sys.path.insert(0, os.path.abspath("../.."))
from aux.geometry import edges
from aux.altitudes import altitude as alt
from aux.geometry import rotation
sys.path.insert(0, os.path.abspath(".."))
import classes
else:
import rotation
import altitude as alt
import directions
import classes
#print populate_with_lines(([[(0,0),(10,0),(4.8,5),(10,10),(0,10)], []]), 0.1, 0)