Shuvit game master repo. http://shuvit.org
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

astar.py 7.4KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244
  1. import bge, json, mathutils, math
  2. from timeit import default_timer as timer
  3. def chrono(function):
  4. def wrapper(*args, **kargs):
  5. before = timer()
  6. value = function(*args, **kargs)
  7. print(function.__qualname__, 'took', (timer() - before) * 100, 'to return', value)
  8. return value
  9. return wrapper
  10. def draw_all(self):
  11. for x in self.points:
  12. pm = bge.logic.getCurrentScene().addObject('path_marker')
  13. pm.worldPosition = x.loc
  14. pm.color = [1,1,0,1]
  15. pm.worldScale = [.25, .26, .25]
  16. class IterRegistry(type):
  17. def __iter__(cls):
  18. return iter(cls._registry)
  19. def load_points(points_file):
  20. mainDir = bge.logic.expandPath("//assets/")
  21. fileName = mainDir + points_file
  22. with open(fileName, 'r') as filehandle:
  23. return json.load(filehandle)
  24. def build_tree(points):
  25. kd = mathutils.kdtree.KDTree(len(points))
  26. id = 0
  27. for p in points:
  28. p = mathutils.Vector(p[0])
  29. kd.insert(p, id)
  30. id += 1
  31. kd.balance()
  32. return kd
  33. def get_distance(start, end):
  34. length = (end - start).magnitude
  35. return length
  36. class Point(object):
  37. __metaclass__ = IterRegistry
  38. _registry = []
  39. def __init__(self, list_point, parent, iter):
  40. self.l = list_point[0]
  41. self.id = iter
  42. self.parent = parent
  43. self.loc = mathutils.Vector(list_point[0])
  44. self.lneighbors = list_point[1]
  45. self.neighbors = []
  46. self._registry.append(self)
  47. self.g = 100000
  48. self.h = 100000
  49. self.f = 100000
  50. self.prev = None
  51. def get_neighbors(self):
  52. for p in self.lneighbors:
  53. for p_ in self.parent.points:
  54. if p_.loc == mathutils.Vector(p):
  55. self.neighbors.append(p_)
  56. self.neighbors = set(self.neighbors)
  57. def init_points(self):
  58. points = []
  59. iter = 0
  60. for x in self.points_list:
  61. name = 'p' + str(iter)
  62. y = Point(x, self, iter)
  63. points.append(y)
  64. iter += 1
  65. return points
  66. class Astar:
  67. def __init__(self, points_file, messenger):
  68. self.state = 'idle'
  69. self.points_list = load_points(points_file)
  70. self.points = init_points(self)
  71. self.start = []
  72. self.end = []
  73. self.kdtree = build_tree(self.points_list)
  74. self.life = 0
  75. self.open = []
  76. self.closed = []
  77. self.searched = []
  78. self.current = []
  79. self.fscores = []
  80. self.gscores = []
  81. self.lps = []
  82. self.messenger = messenger
  83. self.queue = []
  84. self.current_searcher = None
  85. for x in self.points: x.get_neighbors()
  86. def update(self):
  87. self.life += 1
  88. if self.state == 'searching':
  89. self.search()
  90. #print('open', len(self.open))
  91. if self.life > 2000:
  92. print('path failed', self.current_searcher)
  93. self.messenger.dispatch('path found', ['failed', self.current_searcher, []])
  94. self.state = 'idle'
  95. else:
  96. self.update_queue()
  97. def get_results(self):
  98. result_locs = []
  99. results = []
  100. cp = self.end
  101. breaker = 0
  102. while cp != self.start:
  103. result_locs.append(cp.id)
  104. results.append(cp.loc)
  105. if cp.prev == None:
  106. cp = self.current
  107. else:
  108. cp = cp.prev
  109. breaker += 1
  110. if breaker > 2000:
  111. break
  112. results.reverse()
  113. self.messenger.dispatch('path found', ['path', self.current_searcher, results])
  114. #print('path found', self.current_searcher)
  115. def smallest_f(self):
  116. if len(self.open) >= 1:
  117. l = []
  118. lfs = []
  119. for x in self.open:
  120. l.append(x)
  121. lfs.append(x.f)
  122. ind = lfs.index(min(lfs))
  123. return l[ind]
  124. else:
  125. return self.start
  126. #@chrono
  127. def search(self):
  128. tries = 0
  129. if self.open == []:
  130. self.state = 'inactive'
  131. while self.open != []:
  132. try:
  133. self.open.remove(self.current)
  134. except:
  135. pass
  136. current_node = self.smallest_f()
  137. self.current = current_node
  138. self.searched.append(current_node)
  139. self.closed.append(current_node) #add to closed
  140. if current_node == self.end:
  141. path = self.get_results()
  142. self.state = 'inactive'
  143. self.open = []
  144. break
  145. else:
  146. children = current_node.neighbors
  147. for child in children:
  148. self.searched.append(child)
  149. p = current_node
  150. g = get_distance(current_node.loc, child.loc) + current_node.g
  151. h = get_distance(child.loc, self.end.loc)
  152. f = g + h
  153. if child not in self.open and child not in self.closed:
  154. if g >= child.g:
  155. pass
  156. else:
  157. child.g = g
  158. child.h = h
  159. child.f = f
  160. child.prev = p
  161. self.open[:] = [x for x in self.open if x != child]
  162. self.closed[:] = [x for x in self.closed if x != child]
  163. self.open.append(child)
  164. tries += 1
  165. if tries >= 5:
  166. break
  167. def get_neighbor_id(self, id):
  168. iter = 0
  169. for p in self.points:
  170. if p[0] == id:
  171. num = iter
  172. else:
  173. pass
  174. iter += 1
  175. def set_gscore(self):
  176. self.gscores = []
  177. self.fscores = []
  178. self.lps = []
  179. for x in self.points:
  180. self.gscores.append(100000)
  181. self.fscores.append(100000)
  182. self.lps.append(None)
  183. x.f = 100000
  184. x.h = 100000
  185. x.g = 100000
  186. def set_fscore(self):
  187. self.fscores = []
  188. for x in self.points:
  189. self.fscores.append(100000)
  190. def queue_path(self, start_in, end_in, obj):
  191. self.queue.append([start_in, end_in, obj])
  192. def update_queue(self):
  193. if self.queue != []:
  194. self.get_path(self.queue[0][0], self.queue[0][1], self.queue[0][2])
  195. self.queue.remove(self.queue[0])
  196. def get_path(self, start_in, end_in, obj):
  197. stree = self.kdtree.find_n(start_in, 1)
  198. start_out = self.points[stree[0][1]]
  199. otree = self.kdtree.find_n(end_in, 1)
  200. end_out = self.points[otree[0][1]]
  201. self.life = 0
  202. self.current_searcher = obj
  203. self.start = start_out
  204. self.end = end_out
  205. self.state = 'searching'
  206. self.open.append(self.start)
  207. self.set_gscore()
  208. self.current = self.start
  209. self.open = []
  210. self.closed = []
  211. self.searched = []
  212. self.open.append(self.start)
  213. self.current = self.start
  214. self.current.g = 0
  215. self.current.f = 0
  216. self.current.h = get_distance(self.current.loc, self.end.loc)