shuvit/scripts/astar.py

import bge, json, mathutils, math
from timeit import default_timer as timer

def chrono(function):
    def wrapper(*args, **kargs):
        before = timer()
        value = function(*args, **kargs)
        print(function.__qualname__, 'took', (timer() - before) * 100, 'to return', value)
        return value
    return wrapper

def draw(locs, self):
    for x in locs:
        b = bge.logic.getCurrentScene().addObject('ball')
        b.worldPosition = self.points[x].loc
        b.color = [1,0,0,1]
        
def draw_mesh(self):
    self.searched = set(self.searched)
    for x in self.searched:
        b = bge.logic.getCurrentScene().addObject('ball')
        b.worldPosition = x.loc
        b.color = [0,0,1,1]
        b.worldScale = [.5, .5, .5] 
    
def draw_searched(self):
    pass

def draw_se(self):        
    b = bge.logic.getCurrentScene().addObject('ball')
    b.worldPosition = self.start.loc
    b.color = [0,1,0,1]
    b.worldScale = [.75, .75, .75] 
    #----    
    b = bge.logic.getCurrentScene().addObject('ball')
    b.worldPosition = self.end.loc
    b.color = [1,1,0,1]
    b.worldScale = [2, 2, 2]  

def draw_all(self):
    for x in self.points:

        pm = bge.logic.getCurrentScene().addObject('path_marker')
        pm.worldPosition = x.loc
        pm.color = [1,1,0,1]
        pm.worldScale = [.25, .26, .25]  
            
class IterRegistry(type):
    def __iter__(cls):
        return iter(cls._registry)

def load_points(points_file):        
    #mainDir = bge.logic.expandPath("//")
    mainDir = bge.logic.expandPath("//assets/")
    fileName = mainDir + points_file    
    with open(fileName, 'r') as filehandle:        
        return json.load(filehandle)

def build_tree(points):
    kd = mathutils.kdtree.KDTree(len(points))
    id = 0
    for p in points:
        p = mathutils.Vector(p[0])
        kd.insert(p, id)
        id += 1
    kd.balance()
    return kd

def get_distance(start, end):
    length = (end - start).magnitude
    return length



class Point(object):
    __metaclass__ = IterRegistry
    _registry = []

    def __init__(self, list_point, parent, iter):
        self.l = list_point[0]
        self.id = iter
        self.parent = parent
        self.loc = mathutils.Vector(list_point[0])
        self.lneighbors = list_point[1]
        self.neighbors = []
        self._registry.append(self)  
        self.g = 100000
        self.h = 100000
        self.f = 100000 
        self.prev = None  
        
    def get_neighbors(self):
        for p in self.lneighbors:
            for p_ in self.parent.points:
                if p_.loc == mathutils.Vector(p):
                    self.neighbors.append(p_)
        self.neighbors = set(self.neighbors)            
                 
def init_points(self):
    points = []
    iter = 0
    for x in self.points_list:
        name = 'p' + str(iter)
        y = Point(x, self, iter)
        points.append(y)
        iter += 1
    return points    
    
class Astar:
    def __init__(self, points_file, messenger):
        self.state = 'idle'
        self.points_list = load_points(points_file)
        self.points = init_points(self) 
        self.start = []
        self.end = []
        self.kdtree = build_tree(self.points_list)
        self.life = 0
        self.open = []
        self.closed = []
        self.searched = []
        self.current = []
        self.fscores = []
        self.gscores = []
        self.lps = []
        self.messenger = messenger
        self.queue = []
        self.current_searcher = None
        for x in self.points: x.get_neighbors()
        
    def update(self):
        self.life += 1
        if self.state == 'searching':
            self.search()
            #print('life', self.life)
            if self.life > 5000:
                #print('path failed')
                self.state = 'idle'
        else:
            #print('updating pathfinder queue')
            self.update_queue()        
             
    def get_results(self):
        result_locs = []
        results = []
        #print('self.end', self.end, self.end.id)
        cp = self.end   
        breaker = 0
        while cp != self.start:
            result_locs.append(cp.id)
            results.append(cp.loc)
            if cp.prev == None:
                cp = self.current
            else:
                cp = cp.prev
            breaker += 1
            if breaker > 2000:
                #print('breaking results')
                break
        #if cp == self.start:
            #print('start found!!')
        #print('sp', self.start.id, 'ep', self.end.id)
        #print('final path', result_locs, len(result_locs)) 
        #draw_mesh(self)
        #draw(result_locs, self)  
        #publish
        results.reverse()
        self.messenger.dispatch('path found', ['found path for object', self.current_searcher, results])
        #draw_all(self)

    def smallest_f(self):
        if len(self.open) > 1:
            l = []
            lfs = []
            for x in self.open:
                l.append(x)
                lfs.append(x.f)
            ind = lfs.index(min(lfs))  
            #print(ind, lfs, l[ind]) 
            return l[ind]
        else:
            return self.start
            
    #@chrono    
    def search(self):
        #Loop until you find the end
        tries = 0
        if self.open == []:
            draw_mesh(self)
            self.state = 'inactive'

        while self.open != []:
            #Get the current node from open list with smallest fval
            try:
                self.open.remove(self.current)
            except:
                pass
                #print('current is not in open, cant remove')
            current_node = self.smallest_f()
            self.current = current_node
            self.searched.append(current_node)
             #remove from open
            self.closed.append(current_node) #add to closed
            if current_node == self.end:
                #print('path found')
                path = self.get_results()
                #print('p', path)
                self.state = 'inactive'
                self.open = []
                break                
            else:
                children = current_node.neighbors
                for child in children:
                    self.searched.append(child)
                    p = current_node
                    g = get_distance(current_node.loc, child.loc) + current_node.g
                    h = get_distance(child.loc, self.end.loc)
                    f = g + h   

                    if child not in self.open and child not in self.closed:
                        if g >= child.g:
                            pass
                        else:
                            child.g = g
                            child.h = h
                            child.f = f
                            child.prev = p
                            
                        self.open[:] = [x for x in self.open if x != child]  
                        self.closed[:] = [x for x in self.closed if x != child]    
                        self.open.append(child)

#            print('trying', current_node.id, current_node.f, len(self.open))
#            nei = []
#            for z in current_node.neighbors:
#                nei.append(z.id)
            tries += 1
            if tries >= 5:
                break                      
                
    def get_neighbor_id(self, id):
        iter = 0
        for p in self.points:
            if p[0] == id:
                num = iter
            else:
                pass
                #print('cant get navmesh neighbor id')    
            iter += 1    
        #print(num, 'id')
        
    def set_gscore(self):
        self.gscores = []
        self.fscores = []
        self.lps = []
        for x in self.points:
            self.gscores.append(100000)    
            self.fscores.append(100000)  
            self.lps.append(None)   
            x.f = 100000
            x.h = 100000
            x.g = 100000  
                   
    def set_fscore(self):
        self.fscores = []
        for x in self.points:
            self.fscores.append(100000) 

    def queue_path(self, start_in, end_in, obj):
        self.queue.append([start_in, end_in, obj])
        #obj.manager.pub.register("path found", obj)
        #print('queueing path', len(self.queue))

    def update_queue(self):
        if self.queue != []:
            #print('--getting next path in queue')
            self.get_path(self.queue[0][0], self.queue[0][1], self.queue[0][2])
            self.queue.remove(self.queue[0])           
                         
    def get_path(self, start_in, end_in, obj):
        stree = self.kdtree.find_n(start_in, 1)
        start_out = self.points[stree[0][1]]
        otree = self.kdtree.find_n(end_in, 1)
        end_out = self.points[otree[0][1]]
        self.life = 0
        self.current_searcher = obj
        self.start = start_out
        self.end = end_out
        self.state = 'searching'
        self.open.append(self.start)
        self.set_gscore()
        self.current = self.start
        #beginning search
        #Initialize both open and closed list
        self.open = []
        self.closed = []
        self.searched = []

        #Add the start node
        self.open.append(self.start)
        self.current = self.start
        self.current.g = 0
        self.current.f = 0
        self.current.h = get_distance(self.current.loc, self.end.loc)
        #draw_se(self)
        print('start', self.start.id, 'end', self.end.id)
        
# def main(cont):
#     own = cont.owner
#     if 'inited' not in own:
#         own['inited'] = True
#         own['ped_astar'] = Astar('nav_points')
#         own['ped_astar'].get_path([5,-12,1] , [16,16,1])
        
#     own['ped_astar'].update()