|
@@ -0,0 +1,252 @@
|
|
1
|
+import bge
|
|
2
|
+scene = bge.logic.getCurrentScene()
|
|
3
|
+import math
|
|
4
|
+import mathutils
|
|
5
|
+
|
|
6
|
+lVector = []
|
|
7
|
+lOpen = []
|
|
8
|
+lClosed = []
|
|
9
|
+lShortest_Dist = []
|
|
10
|
+lPrevious_Vert = []
|
|
11
|
+lFrom_Start = []
|
|
12
|
+lHur = []
|
|
13
|
+lFval = []
|
|
14
|
+lDistances = []
|
|
15
|
+
|
|
16
|
+def build_lists(own):
|
|
17
|
+ mesh = own.meshes
|
|
18
|
+ size = own.meshes[0].getVertexArrayLength(0)
|
|
19
|
+ kd = mathutils.kdtree.KDTree(size)
|
|
20
|
+ lVector_tmp = []
|
|
21
|
+ for mesh in own.meshes:
|
|
22
|
+ for m_index in range(len(mesh.materials)):
|
|
23
|
+ for v_index in range(mesh.getVertexArrayLength(m_index)):
|
|
24
|
+ vertex = mesh.getVertex(m_index, v_index)
|
|
25
|
+ lVector_tmp.append(vertex.XYZ)
|
|
26
|
+ id = 0
|
|
27
|
+ for x in lVector_tmp:
|
|
28
|
+ if x not in lVector:
|
|
29
|
+ lVector.append(x)
|
|
30
|
+ lShortest_Dist.append(100000)
|
|
31
|
+ lHur.append(100000)
|
|
32
|
+ lFval.append(100000)
|
|
33
|
+ lFrom_Start.append(100000)
|
|
34
|
+ lPrevious_Vert.append(None)
|
|
35
|
+ kd.insert(x, id)
|
|
36
|
+ id += 1
|
|
37
|
+
|
|
38
|
+ kd.balance()
|
|
39
|
+ own['kd'] = kd
|
|
40
|
+
|
|
41
|
+
|
|
42
|
+def mouse2world():
|
|
43
|
+ cam = bge.logic.getCurrentScene().active_camera
|
|
44
|
+ vec = cam.getScreenVect(*bge.logic.mouse.position)
|
|
45
|
+ camPos = cam.worldPosition
|
|
46
|
+ projectedPos = [0,0,0]
|
|
47
|
+ z = 60 # user-set depth
|
|
48
|
+ projectedPos[0] = camPos[0] - vec[0] * z
|
|
49
|
+ projectedPos[1] = camPos[1] - vec[1] * z
|
|
50
|
+ projectedPos[2] = camPos[2] - vec[2] * z
|
|
51
|
+
|
|
52
|
+ return projectedPos
|
|
53
|
+
|
|
54
|
+def clear_points():
|
|
55
|
+ scene = bge.logic.getCurrentScene()
|
|
56
|
+ for x in scene.objects:
|
|
57
|
+ if 'point' in x:
|
|
58
|
+ x.endObject()
|
|
59
|
+
|
|
60
|
+def mouse(cont):
|
|
61
|
+ own = cont.owner
|
|
62
|
+ keyboard = bge.logic.mouse
|
|
63
|
+ JUST_ACTIVATED = bge.logic.KX_INPUT_JUST_ACTIVATED
|
|
64
|
+ if keyboard.events[bge.events.LEFTMOUSE] == JUST_ACTIVATED:
|
|
65
|
+ mouse_pos = mouse2world()
|
|
66
|
+ print('mouse clicked on', mouse_pos)
|
|
67
|
+ end_pos = mouse_pos
|
|
68
|
+ if 'end_pos' in own:
|
|
69
|
+ own['start_pos'] = own['end_pos']
|
|
70
|
+ own['start_index'] = own['end_index']
|
|
71
|
+ own['end_pos'] = mouse_pos
|
|
72
|
+ kd = own['kd']
|
|
73
|
+ end_pos = mathutils.Vector((end_pos[0], end_pos[1], 0))
|
|
74
|
+ co_find = end_pos
|
|
75
|
+ co, index, dist = kd.find(co_find)
|
|
76
|
+ own['end_pos'] = co
|
|
77
|
+ own['end_index'] = index
|
|
78
|
+ own['end_index'] = lVector.index(co)
|
|
79
|
+ draw(own)
|
|
80
|
+
|
|
81
|
+def draw_path(own):
|
|
82
|
+ start_index = own['start_index']
|
|
83
|
+ end_index = own['end_index']
|
|
84
|
+ breaker = 0
|
|
85
|
+ current_pindex = end_index
|
|
86
|
+ pos = scene.objects['addEmpty']
|
|
87
|
+ starPath = []
|
|
88
|
+ while current_pindex != start_index and breaker < 1600:
|
|
89
|
+ try:
|
|
90
|
+ current_pindex = lPrevious_Vert[current_pindex]
|
|
91
|
+ if lVector[current_pindex] != own['start_pos']:
|
|
92
|
+ pos.worldPosition = lVector[current_pindex]
|
|
93
|
+ starPath.append(lVector[current_pindex])
|
|
94
|
+ else:
|
|
95
|
+ current_pindex = start_index
|
|
96
|
+ except:
|
|
97
|
+ current_pindex = start_index
|
|
98
|
+ print('no current index')
|
|
99
|
+ breaker += 1
|
|
100
|
+ return starPath
|
|
101
|
+def draw_point(drawList, own, starPath):
|
|
102
|
+ id = 0
|
|
103
|
+ obj = scene.objects['addEmpty']
|
|
104
|
+ for x in drawList:
|
|
105
|
+ if x != own['start_pos'] and x != own['end_pos']:
|
|
106
|
+ obj.worldPosition = x
|
|
107
|
+ obj.worldPosition.z = .001
|
|
108
|
+ point = scene.addObject('point', obj)
|
|
109
|
+ point.color = [.5,.5,.05,0]
|
|
110
|
+ point.scaling = [.5,.5,.5]
|
|
111
|
+ point['id'] = id
|
|
112
|
+ id += 1
|
|
113
|
+
|
|
114
|
+ starPath = starPath[::-1]
|
|
115
|
+ for x in starPath:
|
|
116
|
+ obj.worldPosition = x
|
|
117
|
+ obj.worldPosition.z = .002
|
|
118
|
+ point = scene.addObject('point', obj)
|
|
119
|
+ point.color = [0,1,1,0]
|
|
120
|
+ point.scaling = [1,1,1]
|
|
121
|
+ point['star'] = True
|
|
122
|
+ point['id'] = id
|
|
123
|
+ id += 1
|
|
124
|
+
|
|
125
|
+def draw(own):
|
|
126
|
+ clear_points()
|
|
127
|
+ get_start_end_verts(own)
|
|
128
|
+ pos = scene.objects['addEmpty']
|
|
129
|
+ pos.worldPosition = own['start_pos']
|
|
130
|
+ start_pos = own['start_pos']
|
|
131
|
+ point = scene.addObject('point', pos)
|
|
132
|
+ point.color = [0,1,0,1]
|
|
133
|
+ point.scaling = [2,2,2]
|
|
134
|
+ point['ticker'] = 2.0
|
|
135
|
+ if 'end_pos' in own:
|
|
136
|
+ pos.worldPosition = own['end_pos']
|
|
137
|
+ point = scene.addObject('point', pos)
|
|
138
|
+ point.color = [1,0,0,1]
|
|
139
|
+ point.scaling = [2,2,2]
|
|
140
|
+ point['ticker'] = 2.0
|
|
141
|
+ get_start_end_verts(own)
|
|
142
|
+ dijk8(own)
|
|
143
|
+
|
|
144
|
+def get_neighbors(own, start):
|
|
145
|
+ sv = lVector.index(start)
|
|
146
|
+ kd = own['kd']
|
|
147
|
+ neighbor_list = []
|
|
148
|
+ neighbor_length_list = []
|
|
149
|
+ for (co, index, dist4) in kd.find_n(start, 9):
|
|
150
|
+ if co != start:
|
|
151
|
+ neighbor_list.append(co)
|
|
152
|
+ neighbor_length_list.append(dist4)
|
|
153
|
+ min_dist = min(neighbor_length_list) * 1.5
|
|
154
|
+ id = 0
|
|
155
|
+ tnl = []
|
|
156
|
+ for x in neighbor_list:
|
|
157
|
+ if neighbor_length_list[id] > min_dist:
|
|
158
|
+ neighbor_list.remove(x)
|
|
159
|
+ else:
|
|
160
|
+ tnl.append(x)
|
|
161
|
+ id += 1
|
|
162
|
+ neighbor_list = tnl
|
|
163
|
+ return neighbor_list
|
|
164
|
+
|
|
165
|
+def dijk8(own):
|
|
166
|
+ lOpen = []
|
|
167
|
+ drawList = []
|
|
168
|
+ for x in lVector:
|
|
169
|
+ lShortest_Dist[lVector.index(x)] = 100000
|
|
170
|
+ lPrevious_Vert[lVector.index(x)] = None
|
|
171
|
+ lOpen.append(x)
|
|
172
|
+ lFval[lVector.index(x)] = 1000000
|
|
173
|
+ lHur = lDistances[own['end_index']]
|
|
174
|
+ lShortest_Dist[own['start_index']] = 0
|
|
175
|
+ lFval[own['start_index']] = lHur[own['start_index']]
|
|
176
|
+ breaker = 0
|
|
177
|
+
|
|
178
|
+ while lOpen != []:
|
|
179
|
+ start = own['start_pos']
|
|
180
|
+ tmp_dst = []
|
|
181
|
+ id = 0
|
|
182
|
+ for x in lFval:
|
|
183
|
+ if lVector[id] in lOpen:
|
|
184
|
+ tmp_dst.append(x)
|
|
185
|
+ else:
|
|
186
|
+ tmp_dst.append(50000000)
|
|
187
|
+ id += 1
|
|
188
|
+ parent_id = tmp_dst.index(min(tmp_dst))
|
|
189
|
+ cur_vec = lVector[parent_id]
|
|
190
|
+ cost = lShortest_Dist[parent_id]
|
|
191
|
+ cur_Vid = parent_id
|
|
192
|
+ cur_id = parent_id
|
|
193
|
+ drawList.append(cur_vec)
|
|
194
|
+ if cur_vec == own['end_pos']:
|
|
195
|
+ print('path found')
|
|
196
|
+ break
|
|
197
|
+ neighbor_list = get_neighbors(own, cur_vec)
|
|
198
|
+ from_start = lShortest_Dist[cur_Vid]
|
|
199
|
+ for nb in neighbor_list:
|
|
200
|
+ cv = lVector.index(nb)
|
|
201
|
+ total_dist = ((cur_vec - nb).length) + from_start
|
|
202
|
+ if lShortest_Dist[cv] > total_dist:
|
|
203
|
+ lShortest_Dist[cv] = total_dist
|
|
204
|
+ lPrevious_Vert[cv] = parent_id
|
|
205
|
+
|
|
206
|
+ fval = lHur[cv] + lShortest_Dist[cv]
|
|
207
|
+ if lFval[cv] > fval:
|
|
208
|
+ lFval[cv] = fval
|
|
209
|
+
|
|
210
|
+ lOpen.remove(cur_vec)
|
|
211
|
+ if lOpen == []:
|
|
212
|
+ print('list emptied')
|
|
213
|
+ break
|
|
214
|
+ breaker += 1
|
|
215
|
+ if breaker > 4000:
|
|
216
|
+ print('breaker tripped')
|
|
217
|
+ break
|
|
218
|
+ starPath = draw_path(own)
|
|
219
|
+ draw_point(drawList, own, starPath)
|
|
220
|
+
|
|
221
|
+def get_start_end_verts(own):
|
|
222
|
+ kd = own['kd']
|
|
223
|
+ start = own['start_pos']
|
|
224
|
+ end = own['end_pos']
|
|
225
|
+ co, start_index, dist = kd.find(start)
|
|
226
|
+ co, end_index, dist = kd.find(end)
|
|
227
|
+ own['start_index'] = start_index
|
|
228
|
+ own['end_index'] = end_index
|
|
229
|
+ lShortest_Dist[start_index] = 0
|
|
230
|
+
|
|
231
|
+def pre_compute_dist(own):
|
|
232
|
+ id = 0
|
|
233
|
+ while id < len(lVector):
|
|
234
|
+ end = lVector[id]
|
|
235
|
+ lst = []
|
|
236
|
+ for y in lVector:
|
|
237
|
+ dist = ((end - y).length)
|
|
238
|
+ lst.append(dist)
|
|
239
|
+ lDistances.append(lst)
|
|
240
|
+ id += 1
|
|
241
|
+
|
|
242
|
+def main(cont):
|
|
243
|
+ own = cont.owner
|
|
244
|
+ if 'inited' not in own:
|
|
245
|
+ own['start_pos'] = mathutils.Vector((0.0, 0.0, 0.0))
|
|
246
|
+ own['end_pos'] = mathutils.Vector((4.0, 8.0, 0.0))
|
|
247
|
+ build_lists(own)
|
|
248
|
+ pre_compute_dist(own)
|
|
249
|
+ get_start_end_verts(own)
|
|
250
|
+ own['inited'] = True
|
|
251
|
+ draw(own)
|
|
252
|
+ mouse(cont)
|