astar/astar.py

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)