123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239 |
- 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)
-
|