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_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("//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()
            if self.life > 5000:
                self.state = 'idle'
        else:
            self.update_queue()        
             
    def get_results(self):
        result_locs = []
        results = []
        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:
                break
        results.reverse()
        self.messenger.dispatch('path found', ['found path for object', self.current_searcher, results])

    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))  
            return l[ind]
        else:
            return self.start
            
    #@chrono    
    def search(self):
        tries = 0
        if self.open == []:
            self.state = 'inactive'

        while self.open != []:
            try:
                self.open.remove(self.current)
            except:
                pass
            current_node = self.smallest_f()
            self.current = current_node
            self.searched.append(current_node)
            self.closed.append(current_node) #add to closed
            if current_node == self.end:
                path = self.get_results()
                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)

            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
            iter += 1    
        
    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])

    def update_queue(self):
        if self.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
        self.open = []
        self.closed = []
        self.searched = []
        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)