-
Notifications
You must be signed in to change notification settings - Fork 0
/
graph.py
157 lines (133 loc) · 4.31 KB
/
graph.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
import matplotlib.pyplot as plt
import sys, math
from string import split
R_DIM = (10,10) #example, robot that is 10cm x 10cm
class Obstacle:
v_num = 0
vertices = []
grown = False
def __init__(self, v_num, vertices, grown):
self.v_num = v_num
self.vertices = vertices
self.grown = False
#grows the obstacle by the size of the robot
def grow(self):
for a in range(0, self.v_num):
x,y = self.vertices[a]
self.vertices.append([x+R_DIM[0], y+R_DIM[1]])
self.vertices.append([x+R_DIM[0], y])
self.vertices.append([x, y+R_DIM[1]])
self.grown = True
#place vertices in the order the graham scan will analyze them in
def sort(self):
#initial sort by lowest y-val to highest, if conflict then lowest x first
self.vertices = list(set(tuple(a) for a in self.vertices))
self.vertices.sort(key=lambda x: (x[1], x[0]))
self.vertices = (list(a) for a in self.vertices)
ang_table = []
temp_table = []
v = list(self.vertices)
#find the angle for each point compared to origin
for i in range(1,len(v)):
ang = 0
if v[i][0]-v[0][0] == 0:
ang = -math.pi/2
else:
ang = math.atan(float(v[i][1]-v[0][1])/float(v[i][0]-v[0][0]))
ang_table.append([ang, v[i]])
temp_pos = []
temp_neg = []
#lowest -> highest positive, then greatest neg number to lowest
for a,b in ang_table:
if a>= 0:
temp_pos.append([a,b])
else:
temp_neg.append([a,b])
reversed(temp_neg)
temp_pos.extend(temp_neg)
temp_table.append(v[0])
for a,b in temp_pos:
temp_table.append(b)
self.vertices = temp_table
def o_print(self):
for a in self.vertices:
print a
#ccw if >0, cw if <0, colinear if == 0
def ccw(self, p1, p2, p3):
# print p1
# print p2
# print p3
return (p2[0]-p1[0])*(p3[1]-p1[1]) - (p2[1]-p1[1])*(p3[0]-p1[0])
def graham_scan(self):
temp_table = []
v_num = len(self.vertices)
v = list(self.vertices)
for i in v:
while v_num > 1 and (self.ccw(v[-2], v[-1], i) >= 0):
v.pop()
temp_table.append(i)
# print temp_table
# print len(temp_table)
# v.insert(0, v.pop(-1))
# # print v
# # print len(v)
#
# p = 1 #number of points on convex hull
# for i in range(2, self.v_num):
# while self.ccw(v[p-1], v[p], v[p+1]) <= 0:
# if p > 1:
# p -= 1
# continue
# elif i == p:
# break
# else:
# i += 1
# p += 1
# temp = v[p]
# v[p] = v[i]
# v[i] = temp
# print len(v)
# print v
def main():
test_obs = Obstacle(4, [[0,0], [0,10], [10,0], [10,10]], False)
test_obs.grow()
test_obs.sort()
test_obs.graham_scan()
# test_obs.o_print()
# input_table = []
# obstacle_table = []
# input_file = sys.argv[1]
#
# #read in data from input file
# with open(input_file, 'r') as fp:
# parsed_input = fp.readlines()
# fp.close()
#
# #place data into a list of floats
# parsed_input = [x.strip() for x in parsed_input]
# for x in parsed_input:
# x = map(float, x.split())
# input_table.append(x)
#
# start_coord = input_table.pop(0)
# goal_coord = input_table.pop(0)
# env_coord = input_table.pop(0)
# num_of_obstacles = int(input_table[0][0])
# input_table.pop(0)
#
# for a in range(0, num_of_obstacles):
# temp_table = []
# num_of_vertices = int(input_table[0][0])
# input_table.pop(0)
#
# for b in range(0, num_of_vertices):
# temp = input_table.pop(0)
# temp_table.append(temp)
# obstacle_table.append(Obstacle(num_of_vertices, temp_table, False))
# obstacle_table[a].grow()
# obstacle_table[a].sort()
# temp_table = []
# # obstacle_table[1].o_print()
# obstacle_table[0].graham_scan()
if __name__ == '__main__':
main()