-
Notifications
You must be signed in to change notification settings - Fork 0
/
class_highway.py
92 lines (74 loc) · 1.91 KB
/
class_highway.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
from math import *
from class_robot import *
from class_point import *
from class_circular import *
class Route():
points=None
dist=0
def __init__(self):
self.points=[]
class Highway():
points=CircularList(
[
Point(415,-1000,"A"),
Point(415,-730, "B"),
Point(415,-500, "C"),
Point(415,-250, "D"),
Point(415,+000, "E"),
Point(415,+250, "F"),
Point(415,+500, "G"),
Point(415,+730, "H"),
# Point(500,+1000,"I"),
Point(750, +1000,"J"),
Point(1000,+1000,"K"),
Point(1250,+1000,"L"),
# Point(1500,+1000,"M"),
Point(1500,+730, "N"),
Point(1500,+500, "O"),
Point(1500,+250, "P"),
Point(1500,+000, "Q"),
Point(1500,-250, "R"),
Point(1500,-500, "S"),
Point(1500,-730, "T"),
# Point(1500,-1000,"U"),
Point(1500,-1000,"V"),
Point(1250,-1000,"W"),
Point(1000,-1000,"X"),
Point(750, -1000,"Y"),
]
)
def findClosest(self,source):
minDist=9999999
for point in self.points:
dist=self.dist(source,point)
if (dist<minDist):
minDist=dist
res=point
return res
def dist(self,a,b):
return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y))
def findRoute(self,src,dest):
start = self.findClosest(src)
end = self.findClosest(dest)
print("start at "+start.name)
print("end at "+end.name)
route1=Route()
route2=Route()
index = self.points.index(start)-1
while self.points[index+1]!=end :
route1.points.append(self.points[index+1])
index+=1
route1.points.append(self.points[index+1])
index = self.points.index(start)+1
while self.points[index-1]!=end :
route2.points.append(self.points[index-1])
index-=1
route2.points.append(self.points[index-1])
for i in range(0,len(route1.points)-1):
route1.dist+=self.dist(route1.points[i],route1.points[i+1])
for i in range(0,len(route2.points)-1):
route2.dist+=self.dist(route2.points[i],route2.points[i+1])
if route2.dist<route1.dist:
return route2,route1
else:
return route1,route2