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.

StatesCar.py 15KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439
  1. import utils
  2. import bge
  3. import random
  4. from mathutils import Vector
  5. #====================================
  6. def mark_path(path, y):
  7. iter_ = 0
  8. for x in path:
  9. pm = bge.logic.getCurrentScene().addObject('path_marker', y.obj, 0)
  10. pm.worldPosition = path[iter_]
  11. iter_ += 1
  12. if iter_ == 1:
  13. pm.color = [0,1,0,.4]
  14. if iter_ == (len(path) ):
  15. pm.color = [1,0,0,.4]
  16. if iter_ == (len(path) +1):
  17. pm.color = [1,0,1,.4]
  18. y.path_display.append(pm)
  19. def clear_markers(self):
  20. # for x in bge.logic.getCurrentScene().objects:
  21. # if 'path_marker' in x.name:
  22. # x.endObject()
  23. for x in self.FSM.owner.path_display:
  24. try:
  25. x.endObject()
  26. except:
  27. pass
  28. def get_ground_ray(self):
  29. Axis = 2
  30. Distance = -10
  31. end = self.obj.worldPosition + (self.obj.worldOrientation.col[Axis]*Distance)
  32. start = self.obj.worldPosition.copy()
  33. ground_ray = self.obj.rayCast(end, start, 6,'', 1, 0)
  34. return ground_ray
  35. def set_height(self):
  36. ground_ray = get_ground_ray(self)
  37. target_height = 0.9
  38. hitpoint = ground_ray[1]
  39. try:
  40. dist = self.obj.getDistanceTo(hitpoint)
  41. if dist < target_height:
  42. self.obj.worldPosition.z += target_height - dist
  43. self.obj.linearVelocity.z = 0
  44. self.obj.linearVelocity.y *= .1
  45. except:
  46. pass
  47. def align_to_road(self):
  48. ground_ray = get_ground_ray(self)
  49. try:
  50. self.obj.alignAxisToVect(ground_ray[2], 2, .15)
  51. except:
  52. pass
  53. def find_new_parking(self):
  54. potentials = []
  55. for x in self.manager.parking_spots:
  56. if x.status == 'available':
  57. potentials.append(x)
  58. for x in potentials:
  59. min_dist = 100
  60. dist = self.obj.getDistanceTo(x.obj)
  61. if dist < min_dist:
  62. potentials.remove(x)
  63. print('----removing potential')
  64. if len(potentials) > 0:
  65. new_parking = random.choice(potentials)
  66. path = self.manager.navmesh.findPath(self.start_empty.obj.worldPosition, new_parking.obj.worldPosition)
  67. print('parking added at', new_parking.obj.worldPosition)
  68. mark_path(path, self)
  69. return new_parking, path
  70. else:
  71. print('cant find parking for', self)
  72. self.FSM.FSM.ToTransition('toEnterParallelPark')
  73. def get_lane_point(self):
  74. self.point = self.path[self.path_index]
  75. #print(self.path_index, 'path index')
  76. if self.point != self.last_lane_point:
  77. v = Vector([self.last_lane_point.x - self.point.x, self.last_lane_point.y - self.point.y, 0])
  78. tv = v.normalized()
  79. nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees
  80. self.last_lane_point = self.lane_point
  81. self.lane_point = self.point + self.manager.lane_position * nv
  82. def update_point(self):
  83. if self.path_index >= (len(self.path) ):
  84. self.FSM.FSM.ToTransition('toEnterParallelPark')
  85. else:
  86. dist = self.obj.getDistanceTo(self.lane_point)
  87. #print(dist, self.path_index)
  88. self.point = self.path[self.path_index]
  89. if dist < 2.5:
  90. get_lane_point(self)
  91. if self.path_index > (len(self.path)):
  92. pass
  93. else:
  94. self.path_index += 1
  95. def align_to_point(self):
  96. v = self.obj.getVectTo(self.lane_point)[1]
  97. v.z = 0
  98. self.obj.alignAxisToVect(v, 0, .1)
  99. def delta_to_vect(self):
  100. v = self.obj.getVectTo(self.lane_point)[1]
  101. #vt = Vector([self.last_lane_point.x - self.lane_point.x, self.last_lane_point.y - self.lane_point.y, 0])
  102. delta = self.last_lane_point - self.lane_point
  103. delta = delta.cross(v)
  104. delta_mult = -.1
  105. mult = 1.0
  106. deltamove = delta[2] * delta_mult
  107. #self.obj.applyMovement([0, deltamove, 0], True)
  108. f = deltamove * 5000
  109. self.obj.applyForce([0, f, 0], True)
  110. def apply_gas(self):
  111. if self.obj.linearVelocity.x < self.speed_targ:
  112. self.obj.applyForce([self.speed_inc, 0, 0], True)
  113. #====================================
  114. State = type("State", (object,), {})
  115. #====================================
  116. class State(object):
  117. def __init__(self, FSM):
  118. self.FSM = FSM
  119. self.timer = 0
  120. self.startTime = 0
  121. def Enter(self):
  122. self.timer = 0
  123. self.startTime = 0
  124. def Execute(self):
  125. print('Executing')
  126. def Exit(self):
  127. print('Exiting')
  128. #====================================
  129. class Example(State):
  130. def __init__(self,FSM):
  131. super(Example, self).__init__(FSM)
  132. def Enter(self):
  133. self.FSM.stateLife = 1
  134. self.FSM.owner.resumePhysics()
  135. self.FSM.owner.resumeDynamics()
  136. print('physics resumed')
  137. super(Example, self).Enter()
  138. def Execute(self):
  139. self.FSM.stateLife += 1
  140. print('doing example')
  141. #o = self.FSM.owner
  142. #g = o.me['game']
  143. #self.FSM.ToTransition('toActivate')
  144. #print(self.FSM.owner)
  145. #self.FSM.owner.applyRotation([0,0,.01], True)
  146. #self.FSM.owner.worldPosition.z = 0
  147. #self.FSM.owner.worldPosition.z += 0.1
  148. #self.FSM.owner.worldPosition.x += 1.1
  149. #print(self.FSM.owner.children)
  150. def Exit(self):
  151. pass
  152. class ExitParallelPark(State):
  153. def __init__(self,FSM):
  154. super(ExitParallelPark, self).__init__(FSM)
  155. def Enter(self):
  156. self.FSM.stateLife = 1
  157. self.FSM.owner.obj.restorePhysics()
  158. self.FSM.owner.obj.restoreDynamics()
  159. self.FSM.owner.obj.linearVelocity = [0,0,0]
  160. self.FSM.owner.target, self.FSM.owner.path = find_new_parking(self.FSM.owner)
  161. self.FSM.owner.path_index = 0
  162. self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index]
  163. self.FSM.owner.target.status = 'targetted'
  164. self.FSM.owner.start_empty.status = 'available'
  165. #print('target is', self.FSM.owner.target)
  166. print('physics resumed')
  167. super(ExitParallelPark, self).Enter()
  168. def Execute(self):
  169. self.FSM.stateLife += 1
  170. #print('doing ExitParallelPark')
  171. v = self.FSM.owner.obj.getVectTo(self.FSM.owner.path[0])
  172. #v.z = 0
  173. self.FSM.owner.obj.alignAxisToVect(v[1], 0, .01)
  174. self.FSM.owner.obj.alignAxisToVect([0,0,1], 2, 1)
  175. if self.FSM.stateLife > 220:
  176. self.FSM.ToTransition('toNavigateToTarget')
  177. #self.FSM.owner.obj.applyForce([6, 0, 0], True)
  178. def Exit(self):
  179. pass
  180. #====================================
  181. class EnterParallelPark(State):
  182. def __init__(self,FSM):
  183. super(EnterParallelPark, self).__init__(FSM)
  184. def Enter(self):
  185. self.FSM.stateLife = 1
  186. print('entering parallel park')
  187. self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition
  188. self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation
  189. self.FSM.owner.obj.applyMovement([0, -6, 0], True)
  190. self.FSM.owner.target.status = 'in_use'
  191. self.FSM.owner.obj.worldPosition.z += .9
  192. self.FSM.owner.active = False
  193. self.FSM.owner.start_empty = self.FSM.owner.target
  194. self.FSM.owner.last_point = self.FSM.owner.target.obj.worldPosition.copy()
  195. self.FSM.owner.last__lane_point = self.FSM.owner.obj.worldPosition
  196. self.FSM.owner.point = self.FSM.owner.target.obj.worldPosition.copy()
  197. clear_markers(self)
  198. self.FSM.owner.obj.suspendDynamics()
  199. self.FSM.owner.obj.suspendPhysics()
  200. super(EnterParallelPark, self).Enter()
  201. def Execute(self):
  202. self.FSM.stateLife += 1
  203. if self.FSM.stateLife == 2:
  204. self.FSM.owner.manager.cars_active.remove(self.FSM.owner)
  205. #self.FSM.ToTransition('toActivate')
  206. def Exit(self):
  207. pass
  208. #====================================
  209. class NavigateToTarget(State):
  210. def __init__(self,FSM):
  211. super(NavigateToTarget, self).__init__(FSM)
  212. def Enter(self):
  213. self.FSM.stateLife = 1
  214. super(NavigateToTarget, self).Enter()
  215. def Execute(self):
  216. self.FSM.stateLife += 1
  217. update_point(self.FSM.owner)
  218. align_to_point(self.FSM.owner)
  219. align_to_road(self.FSM.owner)
  220. set_height(self.FSM.owner)
  221. delta_to_vect(self.FSM.owner)
  222. apply_gas(self.FSM.owner)
  223. #print('target', self.FSM.owner.target.obj.worldPosition)
  224. #emergency exit
  225. if self.FSM.stateLife > 30 * 60:
  226. self.FSM.ToTransition('toEnterParallelPark')
  227. #print('doing NavigateToTarget')
  228. #self.FSM.owner.obj.applyForce([6, 0, 0], True)
  229. def Exit(self):
  230. pass
  231. #====================================
  232. class Activate(State):
  233. def __init__(self,FSM):
  234. super(Activate, self).__init__(FSM)
  235. def Enter(self):
  236. self.FSM.stateLife = 1
  237. # self.FSM.owner.restorePhysics()
  238. # self.FSM.owner.restoreDynamics()
  239. # self.FSM.owner.worldPosition.z = 2
  240. # self.FSM.curTarget = None
  241. # self.FSM.last_target = None
  242. # #self.FSM.path = None
  243. # self.point = None
  244. # self.last_point = self.FSM.owner.worldPosition.copy()
  245. # self.last_pos = self.FSM.owner.worldPosition.copy()
  246. # self.backup = 0
  247. # self.lane_point = None
  248. # self.last_lane_point = self.last_pos
  249. # self.lane_position = 1.75
  250. # self.pos_his = []
  251. # self.max_speed = 5.0
  252. # self.FSM.path = None
  253. # print('physics resumed')
  254. super(Activate, self).Enter()
  255. def find_target(self):
  256. pass
  257. # if self.FSM.curTarget == None:
  258. # choices = self.FSM.owner['manager'].targets
  259. # if self.FSM.last_target in choices:
  260. # choices.remove(self.FSM.last_target)
  261. # if choices:
  262. # self.FSM.curTarget = random.choice(choices)
  263. # self.FSM.path = self.FSM.owner['manager'].navmesh.findPath(self.FSM.owner.worldPosition, self.FSM.curTarget.worldPosition)
  264. # self.FSM.path.remove(self.FSM.path[0])
  265. # self.point = self.FSM.path[0]
  266. # vt = Vector([self.last_point.x - self.point.x, self.last_point.y - self.point.y, 0])
  267. # tv = vt.normalized()
  268. # #rotate 90 degrees
  269. # nv = Vector([-tv.y, tv.x, 0])
  270. # distance = 3.5
  271. # self.lane_point = self.point + self.lane_position * nv
  272. # else:
  273. # print('no fucking choices')
  274. # return False
  275. # else:
  276. # return True
  277. def drive_to_point(self):
  278. if self.FSM.path:
  279. ground_ray = get_ground_ray(self)
  280. if ground_ray[0]:
  281. set_height(self, ground_ray)
  282. align_to_road(self, ground_ray)
  283. v = self.FSM.owner.getVectTo(self.lane_point)
  284. speed_force = 800.0
  285. max_speed = 5.0
  286. behind = False
  287. local = self.FSM.owner.worldOrientation.inverted() * (self.lane_point - self.FSM.owner.worldPosition)
  288. v2 = v[1].copy()
  289. v2.z = 0
  290. delta = self.last_lane_point - self.lane_point
  291. delta = delta.cross(v[1])
  292. delta_mult = -.1
  293. mult = 1.0
  294. backup_time = 20
  295. #change max speed
  296. if self.FSM.stateLife % 180 == 0:
  297. print('change speed force')
  298. self.speed_force = random.choice([max_speed * 1.3, max_speed * 1.6, max_speed * .8, max_speed * .6, max_speed])
  299. if local.x > 0 and self.backup == 0:
  300. if self.FSM.owner.linearVelocity.x < self.max_speed:
  301. self.FSM.owner.applyForce([speed_force, 0, 0], True)
  302. deltamove = delta[2] * delta_mult
  303. #self.FSM.owner.applyMovement([0, deltamove, 0], True)
  304. f = deltamove * 5000
  305. self.FSM.owner.applyForce([0, f, 0], True)
  306. v = self.FSM.owner.getVectTo(self.lane_point)
  307. v2 = v[1].copy()
  308. v2.z = 0
  309. self.FSM.owner.alignAxisToVect(v2, 0, .05)
  310. else:
  311. if local.x < 0:
  312. self.backup = backup_time
  313. if self.backup > 0:
  314. print('backing up')
  315. v = self.FSM.owner.getVectTo(self.FSM.path[0])
  316. v2 = v[1].copy()
  317. v2.z = 0
  318. self.FSM.owner.alignAxisToVect(v2, 0, .02)
  319. if self.FSM.owner.linearVelocity.x > -max_speed / 2:
  320. self.FSM.owner.applyForce([-speed_force * .8, 0, 0], True)
  321. self.backup -= 1
  322. dist = self.FSM.owner.getDistanceTo(self.lane_point)
  323. if dist < 2.5 and (len(self.FSM.path) > 0):
  324. #print(self.FSM.path,'this is the path')
  325. #print('navmesh point removed')
  326. self.last_point = self.point
  327. self.last_lane_point = self.lane_point
  328. self.FSM.path.remove(self.FSM.path[0])
  329. if len(self.FSM.path) > 0:
  330. self.point = self.FSM.path[0]
  331. v = Vector([self.last_point.x - self.point.x, self.last_point.y - self.point.y, 0])
  332. tv = v.normalized()
  333. nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees
  334. self.lane_point = self.point + self.lane_position * nv
  335. else:
  336. self.point = None
  337. if self.FSM.path == []:
  338. self.FSM.curTarget = None
  339. #progress
  340. self.pos_his.append(self.FSM.owner.worldPosition.copy())
  341. pos_his_len = len(self.pos_his)
  342. if pos_his_len > 200:
  343. #sum_ = abs(self.FSM.owner.worldPosition.x) + abs(self.pos_his[-1][0]) + abs(self.FSM.owner.worldPosition.y) + abs(self.pos_his[-1][1])
  344. sum_ = abs(self.FSM.owner.worldPosition.x - self.pos_his[0][0]) + abs(self.FSM.owner.worldPosition.y - self.pos_his[0][1])
  345. #print(sum_, 'sum')
  346. if sum_ < .05:
  347. self.backup = backup_time
  348. print('progress stopped')
  349. del self.pos_his[0]
  350. def Execute(self):
  351. self.FSM.stateLife += 1
  352. #if self.find_target():
  353. #self.drive_to_point()
  354. #self.FSM.owner.linearVelocity.y = 0
  355. #self.last_pos = self.FSM.owner.worldPosition.copy()
  356. #self.FSM.owner['manager'].target_loc.worldPosition = self.lane_point
  357. def Exit(self):
  358. pass
  359. #====================================