123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252 |
- import bge
- scene = bge.logic.getCurrentScene()
- import math
- import mathutils
-
- lVector = []
- lOpen = []
- lClosed = []
- lShortest_Dist = []
- lPrevious_Vert = []
- lFrom_Start = []
- lHur = []
- lFval = []
- lDistances = []
-
- def build_lists(own):
- mesh = own.meshes
- size = own.meshes[0].getVertexArrayLength(0)
- kd = mathutils.kdtree.KDTree(size)
- lVector_tmp = []
- for mesh in own.meshes:
- for m_index in range(len(mesh.materials)):
- for v_index in range(mesh.getVertexArrayLength(m_index)):
- vertex = mesh.getVertex(m_index, v_index)
- lVector_tmp.append(vertex.XYZ)
- id = 0
- for x in lVector_tmp:
- if x not in lVector:
- lVector.append(x)
- lShortest_Dist.append(100000)
- lHur.append(100000)
- lFval.append(100000)
- lFrom_Start.append(100000)
- lPrevious_Vert.append(None)
- kd.insert(x, id)
- id += 1
-
- kd.balance()
- own['kd'] = kd
-
-
- def mouse2world():
- cam = bge.logic.getCurrentScene().active_camera
- vec = cam.getScreenVect(*bge.logic.mouse.position)
- camPos = cam.worldPosition
- projectedPos = [0,0,0]
- z = 60 # user-set depth
- projectedPos[0] = camPos[0] - vec[0] * z
- projectedPos[1] = camPos[1] - vec[1] * z
- projectedPos[2] = camPos[2] - vec[2] * z
-
- return projectedPos
-
- def clear_points():
- scene = bge.logic.getCurrentScene()
- for x in scene.objects:
- if 'point' in x:
- x.endObject()
-
- def mouse(cont):
- own = cont.owner
- keyboard = bge.logic.mouse
- JUST_ACTIVATED = bge.logic.KX_INPUT_JUST_ACTIVATED
- if keyboard.events[bge.events.LEFTMOUSE] == JUST_ACTIVATED:
- mouse_pos = mouse2world()
- print('mouse clicked on', mouse_pos)
- end_pos = mouse_pos
- if 'end_pos' in own:
- own['start_pos'] = own['end_pos']
- own['start_index'] = own['end_index']
- own['end_pos'] = mouse_pos
- kd = own['kd']
- end_pos = mathutils.Vector((end_pos[0], end_pos[1], 0))
- co_find = end_pos
- co, index, dist = kd.find(co_find)
- own['end_pos'] = co
- own['end_index'] = index
- own['end_index'] = lVector.index(co)
- draw(own)
-
- def draw_path(own):
- start_index = own['start_index']
- end_index = own['end_index']
- breaker = 0
- current_pindex = end_index
- pos = scene.objects['addEmpty']
- starPath = []
- while current_pindex != start_index and breaker < 1600:
- try:
- current_pindex = lPrevious_Vert[current_pindex]
- if lVector[current_pindex] != own['start_pos']:
- pos.worldPosition = lVector[current_pindex]
- starPath.append(lVector[current_pindex])
- else:
- current_pindex = start_index
- except:
- current_pindex = start_index
- print('no current index')
- breaker += 1
- return starPath
- def draw_point(drawList, own, starPath):
- id = 0
- obj = scene.objects['addEmpty']
- for x in drawList:
- if x != own['start_pos'] and x != own['end_pos']:
- obj.worldPosition = x
- obj.worldPosition.z = .001
- point = scene.addObject('point', obj)
- point.color = [.5,.5,.05,0]
- point.scaling = [.5,.5,.5]
- point['id'] = id
- id += 1
-
- starPath = starPath[::-1]
- for x in starPath:
- obj.worldPosition = x
- obj.worldPosition.z = .002
- point = scene.addObject('point', obj)
- point.color = [0,1,1,0]
- point.scaling = [1,1,1]
- point['star'] = True
- point['id'] = id
- id += 1
-
- def draw(own):
- clear_points()
- get_start_end_verts(own)
- pos = scene.objects['addEmpty']
- pos.worldPosition = own['start_pos']
- start_pos = own['start_pos']
- point = scene.addObject('point', pos)
- point.color = [0,1,0,1]
- point.scaling = [2,2,2]
- point['ticker'] = 2.0
- if 'end_pos' in own:
- pos.worldPosition = own['end_pos']
- point = scene.addObject('point', pos)
- point.color = [1,0,0,1]
- point.scaling = [2,2,2]
- point['ticker'] = 2.0
- get_start_end_verts(own)
- dijk8(own)
-
- def get_neighbors(own, start):
- sv = lVector.index(start)
- kd = own['kd']
- neighbor_list = []
- neighbor_length_list = []
- for (co, index, dist4) in kd.find_n(start, 9):
- if co != start:
- neighbor_list.append(co)
- neighbor_length_list.append(dist4)
- min_dist = min(neighbor_length_list) * 1.5
- id = 0
- tnl = []
- for x in neighbor_list:
- if neighbor_length_list[id] > min_dist:
- neighbor_list.remove(x)
- else:
- tnl.append(x)
- id += 1
- neighbor_list = tnl
- return neighbor_list
-
- def dijk8(own):
- lOpen = []
- drawList = []
- for x in lVector:
- lShortest_Dist[lVector.index(x)] = 100000
- lPrevious_Vert[lVector.index(x)] = None
- lOpen.append(x)
- lFval[lVector.index(x)] = 1000000
- lHur = lDistances[own['end_index']]
- lShortest_Dist[own['start_index']] = 0
- lFval[own['start_index']] = lHur[own['start_index']]
- breaker = 0
-
- while lOpen != []:
- start = own['start_pos']
- tmp_dst = []
- id = 0
- for x in lFval:
- if lVector[id] in lOpen:
- tmp_dst.append(x)
- else:
- tmp_dst.append(50000000)
- id += 1
- parent_id = tmp_dst.index(min(tmp_dst))
- cur_vec = lVector[parent_id]
- cost = lShortest_Dist[parent_id]
- cur_Vid = parent_id
- cur_id = parent_id
- drawList.append(cur_vec)
- if cur_vec == own['end_pos']:
- print('path found')
- break
- neighbor_list = get_neighbors(own, cur_vec)
- from_start = lShortest_Dist[cur_Vid]
- for nb in neighbor_list:
- cv = lVector.index(nb)
- total_dist = ((cur_vec - nb).length) + from_start
- if lShortest_Dist[cv] > total_dist:
- lShortest_Dist[cv] = total_dist
- lPrevious_Vert[cv] = parent_id
-
- fval = lHur[cv] + lShortest_Dist[cv]
- if lFval[cv] > fval:
- lFval[cv] = fval
-
- lOpen.remove(cur_vec)
- if lOpen == []:
- print('list emptied')
- break
- breaker += 1
- if breaker > 4000:
- print('breaker tripped')
- break
- starPath = draw_path(own)
- draw_point(drawList, own, starPath)
-
- def get_start_end_verts(own):
- kd = own['kd']
- start = own['start_pos']
- end = own['end_pos']
- co, start_index, dist = kd.find(start)
- co, end_index, dist = kd.find(end)
- own['start_index'] = start_index
- own['end_index'] = end_index
- lShortest_Dist[start_index] = 0
-
- def pre_compute_dist(own):
- id = 0
- while id < len(lVector):
- end = lVector[id]
- lst = []
- for y in lVector:
- dist = ((end - y).length)
- lst.append(dist)
- lDistances.append(lst)
- id += 1
-
- def main(cont):
- own = cont.owner
- if 'inited' not in own:
- own['start_pos'] = mathutils.Vector((0.0, 0.0, 0.0))
- own['end_pos'] = mathutils.Vector((4.0, 8.0, 0.0))
- build_lists(own)
- pre_compute_dist(own)
- get_start_end_verts(own)
- own['inited'] = True
- draw(own)
- mouse(cont)
|