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)