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 17KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524
  1. import utils
  2. import bge
  3. import random
  4. from mathutils import Vector
  5. import mathutils
  6. #import sound_man
  7. #import math
  8. #====================================
  9. def mark_path(path, y):
  10. iter_ = 0
  11. for x in path:
  12. pm = bge.logic.getCurrentScene().addObject('path_marker', y.obj, 0)
  13. pm.worldPosition = path[iter_]
  14. iter_ += 1
  15. if iter_ == 1:
  16. pm.color = [0,1,0,.4]
  17. if iter_ == (len(path) ):
  18. pm.color = [1,0,0,.4]
  19. if iter_ == (len(path) +1):
  20. pm.color = [1,0,1,.4]
  21. y.path_display.append(pm)
  22. def single_marker(pos, col):
  23. pm = bge.logic.getCurrentScene().addObject('path_marker', None, 0)
  24. pm.worldPosition = pos
  25. pm.color = col
  26. def clear_markers(self):
  27. for x in self.FSM.owner.path_display:
  28. try:
  29. x.endObject()
  30. except:
  31. pass
  32. def get_ground_ray(self):
  33. Axis = 2
  34. Distance = -10
  35. end = self.obj.worldPosition + (self.obj.worldOrientation.col[Axis]*Distance)
  36. start = self.obj.worldPosition.copy()
  37. ground_ray = self.obj.rayCast(end, start, 6,'', 1, 0)
  38. return ground_ray
  39. def set_height(self):
  40. ground_ray = get_ground_ray(self)
  41. target_height = 0.9
  42. hitpoint = ground_ray[1]
  43. try:
  44. dist = self.obj.getDistanceTo(hitpoint)
  45. if dist < target_height:
  46. self.obj.worldPosition.z += target_height - dist
  47. self.obj.linearVelocity.z = 0
  48. self.obj.linearVelocity.y *= .1
  49. except:
  50. pass
  51. def align_to_road(self):
  52. ground_ray = get_ground_ray(self)
  53. try:
  54. self.obj.alignAxisToVect(ground_ray[2], 2, .15)
  55. except:
  56. pass
  57. def find_new_parking(self):
  58. potentials = []
  59. for x in self.manager.parking_spots:
  60. if x.status == 'available':
  61. potentials.append(x)
  62. for x in potentials:
  63. min_dist = 45
  64. dist = self.obj.getDistanceTo(x.obj)
  65. if dist < min_dist:
  66. potentials.remove(x)
  67. if len(potentials) > 0:
  68. new_parking = random.choice(potentials)
  69. path2 = self.manager.navmesh2.queue_path(self.start_empty.obj.worldPosition, new_parking.obj.worldPosition, self)
  70. #print('astar request')
  71. #if path2 is not None:
  72. return new_parking, path2
  73. else:
  74. get_parking_type(self)
  75. #self.FSM.FSM.ToTransition('toEnterParallelPark')
  76. def get_lane_point(self):
  77. self.point = self.path[self.path_index]
  78. if self.point != self.last_lane_point:
  79. v = Vector([self.last_lane_point.x - self.point.x, self.last_lane_point.y - self.point.y, 0])
  80. tv = v.normalized()
  81. nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees
  82. self.last_lane_point = self.lane_point
  83. self.lane_point = self.point + self.manager.lane_position * nv
  84. else:
  85. pass
  86. def update_point(self):
  87. if self.path_index >= (len(self.path) ):
  88. get_parking_type(self)
  89. #self.FSM.FSM.ToTransition('toEnterParallelPark')
  90. else:
  91. dist = self.obj.getDistanceTo(self.lane_point)
  92. self.point = self.path[self.path_index]
  93. if dist < 2.5:
  94. get_lane_point(self)
  95. if self.path_index > (len(self.path)):
  96. pass
  97. else:
  98. self.path_index += 1
  99. def align_to_point(self, mult):
  100. v = self.obj.getVectTo(self.lane_point)[1]
  101. v.z = 0
  102. self.obj.alignAxisToVect(v, 0, .05 * mult)
  103. def align_to_target(self, mult):
  104. v = self.obj.getVectTo(self.FSM.FSM.curState.park_loc)[1]
  105. v.z = 0
  106. self.obj.alignAxisToVect(v, 0, .05 * mult)
  107. def delta_to_vect(self):
  108. v = self.obj.getVectTo(self.lane_point)[1]
  109. delta = self.last_lane_point - self.lane_point
  110. delta = delta.cross(v)
  111. delta_mult = -.1
  112. mult = 1.0
  113. deltamove = delta[2] * delta_mult
  114. #check if in front
  115. local = self.obj.worldOrientation.inverted() * (self.lane_point - self.obj.worldPosition)
  116. f = deltamove * 500
  117. if local < 0:
  118. f *= -1
  119. self.obj.applyForce([0, f, 0], True)
  120. def apply_gas(self, mult):
  121. if self.obj.linearVelocity.x < self.speed_targ * mult:
  122. self.obj.applyForce([self.speed_inc * mult, 0, 0], True)
  123. def starting_mod(self):
  124. path2 = self.path
  125. #path2.append(self.target.obj.worldPosition) #add endpoint
  126. #path2.append(self.target.obj.worldPosition)
  127. def check_front(self): #
  128. obj_vec = self.obj.getAxisVect([1,0,0])
  129. targ_vec = self.obj.getVectTo(self.path[self.path_index] +1)[1].normalized()
  130. dot_ = obj_vec.dot(targ_vec)
  131. if dot_ > 0:
  132. print('in front', self)
  133. else:
  134. print('behind', self)
  135. if dot_ > 0:
  136. return True
  137. else:
  138. return False
  139. def get_parking_type(self):
  140. if self.target.type == 'perp':
  141. self.FSM.FSM.ToTransition('toEnterPerpPark')
  142. else:
  143. self.FSM.FSM.ToTransition('toEnterParallelPark')
  144. def get_parking_point(self):
  145. v = Vector([self.last_lane_point.x - self.point.x, self.last_lane_point.y - self.point.y, 0])
  146. tv = v.normalized()
  147. #nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees
  148. #self.last_lane_point = self.lane_point
  149. local = Vector([7,0,0])
  150. return Vector(self.target.obj.worldPosition + (self.target.obj.worldOrientation * local))
  151. #return Vector(self.target.obj.worldPosition) - 7 * tv
  152. def move_to_point(self):
  153. #print(self.v_to_target[0])
  154. if self.v_to_target[0] < .95:
  155. #print('reached')
  156. return True
  157. else:
  158. vectTo = Vector(self.v_to_target[1])
  159. vectTo.magnitude = 4.01
  160. self.FSM.owner.obj.linearVelocity = vectTo
  161. return False
  162. def forward_ray(self):
  163. Axis = 0
  164. Distance = 30
  165. End = self.obj.worldPosition + (self.obj.worldOrientation.col[Axis]*Distance)
  166. Start = self.obj.worldPosition.copy()
  167. return self.obj.rayCast(End, Start, 10,'traffic', 1, 0, 0)
  168. #dist = v1[0]
  169. #print(dist)
  170. #====================================
  171. State = type("State", (object,), {})
  172. #====================================
  173. class State(object):
  174. def __init__(self, FSM):
  175. self.FSM = FSM
  176. self.timer = 0
  177. self.startTime = 0
  178. def Enter(self):
  179. self.timer = 0
  180. self.startTime = 0
  181. def Execute(self):
  182. print('Executing')
  183. def Exit(self):
  184. print('Exiting')
  185. #====================================
  186. class Example(State):
  187. def __init__(self,FSM):
  188. super(Example, self).__init__(FSM)
  189. def Enter(self):
  190. self.FSM.stateLife = 1
  191. self.FSM.owner.resumePhysics()
  192. self.FSM.owner.resumeDynamics()
  193. print('physics resumed')
  194. super(Example, self).Enter()
  195. def Execute(self):
  196. self.FSM.stateLife += 1
  197. print('doing example')
  198. def Exit(self):
  199. pass
  200. class ExitParallelPark(State):
  201. def __init__(self,FSM):
  202. super(ExitParallelPark, self).__init__(FSM)
  203. def Enter(self):
  204. self.FSM.stateLife = 1
  205. self.FSM.owner.obj.restorePhysics()
  206. self.FSM.owner.obj.restoreDynamics()
  207. self.FSM.owner.obj.linearVelocity = [0,0,0]
  208. self.FSM.owner.path_index = 1
  209. try:
  210. self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index]
  211. get_lane_point(self.FSM.owner)
  212. except:
  213. print('no path')
  214. self.FSM.owner.target.status = 'targetted'
  215. self.FSM.owner.start_empty.status = 'available'
  216. starting_mod(self.FSM.owner)
  217. super(ExitParallelPark, self).Enter()
  218. def Execute(self):
  219. self.FSM.stateLife += 1
  220. set_height(self.FSM.owner)
  221. if self.FSM.owner.path != []:
  222. update_point(self.FSM.owner)
  223. #v = self.FSM.owner.obj.getVectTo(self.FSM.owner.path[0])
  224. #v = self.FSM.owner.obj.getVectTo(self.FSM.owner.lane_point)
  225. #print(v[0])
  226. #self.FSM.owner.obj.alignAxisToVect(v[1], 0, .02)
  227. #self.FSM.owner.obj.alignAxisToVect([0,0,1], 2, 1)
  228. #if self.FSM.stateLife > 160 or self.FSM.owner.obj.getDistanceTo(self.FSM.owner.lane_point) < 2:
  229. #if self.FSM.owner.obj.getDistanceTo(self.FSM.owner.path[0]) < 3.5:# or (self.FSM.stateLife > 30 and check_front(self.FSM.owner)):
  230. if self.FSM.owner.path_index > 1:
  231. #print('exiting park')
  232. #self.FSM.owner.path_index = 2
  233. self.FSM.ToTransition('toNavigateToTarget')
  234. align_to_point(self.FSM.owner, .65)
  235. align_to_road(self.FSM.owner)
  236. #set_height(self.FSM.owner)
  237. delta_to_vect(self.FSM.owner)
  238. apply_gas(self.FSM.owner, .5)
  239. #if self.FSM.stateLife < 30:
  240. #self.FSM.owner.obj.applyRotation([0,0,.0075], True)
  241. def Exit(self):
  242. pass
  243. #====================================
  244. class ExitPerpPark(State):
  245. def __init__(self,FSM):
  246. super(ExitPerpPark, self).__init__(FSM)
  247. def Enter(self):
  248. self.FSM.stateLife = 1
  249. self.FSM.owner.obj.restorePhysics()
  250. self.FSM.owner.obj.restoreDynamics()
  251. self.FSM.owner.obj.linearVelocity = [0,0,0]
  252. self.FSM.owner.path_index = 1
  253. try:
  254. self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index]
  255. get_lane_point(self.FSM.owner)
  256. except:
  257. print('no path')
  258. self.FSM.owner.target.status = 'targetted'
  259. self.FSM.owner.start_empty.status = 'available'
  260. starting_mod(self.FSM.owner)
  261. super(ExitPerpPark, self).Enter()
  262. def Execute(self):
  263. #print('exiting perp park', self.FSM.owner.start_empty.type)
  264. self.FSM.stateLife += 1
  265. set_height(self.FSM.owner)
  266. if self.FSM.owner.path != []:
  267. update_point(self.FSM.owner)
  268. if self.FSM.owner.path_index > 1 or self.FSM.stateLife > 300:
  269. self.FSM.ToTransition('toNavigateToTarget')
  270. #align_to_point(self.FSM.owner, .65)
  271. align_to_road(self.FSM.owner)
  272. #set_height(self.FSM.owner)
  273. #delta_to_vect(self.FSM.owner)
  274. #apply_gas(self.FSM.owner, .5)
  275. if self.FSM.stateLife < 120:
  276. self.FSM.owner.obj.applyForce([-50,0,0], True)
  277. def Exit(self):
  278. pass
  279. #====================================
  280. class EnterParallelPark(State):
  281. def __init__(self,FSM):
  282. super(EnterParallelPark, self).__init__(FSM)
  283. def Enter(self):
  284. self.FSM.stateLife = 1
  285. clear_markers(self)
  286. super(EnterParallelPark, self).Enter()
  287. def cleanup(self):
  288. self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition.copy()
  289. self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation.copy()
  290. self.FSM.owner.obj.applyMovement([0, -6, 0], True)
  291. self.FSM.owner.target.status = 'in_use'
  292. self.FSM.owner.obj.worldPosition.z += .9
  293. self.FSM.owner.active = False
  294. self.FSM.owner.start_empty = self.FSM.owner.target
  295. self.FSM.owner.last_point = self.FSM.owner.target.obj.worldPosition.copy()
  296. self.FSM.owner.lane_point = self.FSM.owner.target.obj.worldPosition.copy()
  297. self.FSM.owner.last_lane_point = self.FSM.owner.target.obj.worldPosition.copy()
  298. self.FSM.owner.point = self.FSM.owner.target.obj.worldPosition.copy()
  299. self.FSM.owner.path_index = 0
  300. self.FSM.owner.path = None
  301. self.FSM.owner.manager.cars_active.remove(self.FSM.owner)
  302. self.FSM.owner.obj.suspendDynamics()
  303. def Execute(self):
  304. self.FSM.stateLife += 1
  305. if self.FSM.stateLife == 240:
  306. self.cleanup()
  307. #self.FSM.owner.obj.suspendPhysics()
  308. def Exit(self):
  309. pass
  310. class EnterPerpPark(State):
  311. def __init__(self,FSM):
  312. super(EnterPerpPark, self).__init__(FSM)
  313. def Enter(self):
  314. self.FSM.stateLife = 1
  315. self.park_loc = get_parking_point(self.FSM.owner)
  316. single_marker(self.park_loc, [1,0,0,.3])
  317. clear_markers(self)
  318. super(EnterPerpPark, self).Enter()
  319. def cleanup(self):
  320. self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition.copy()
  321. self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation.copy()
  322. self.FSM.owner.obj.applyMovement([7, 0, 0], True)
  323. self.FSM.owner.target.status = 'in_use'
  324. self.FSM.owner.obj.worldPosition.z += .9
  325. self.FSM.owner.active = False
  326. self.FSM.owner.start_empty = self.FSM.owner.target
  327. self.FSM.owner.last_point = self.FSM.owner.target.obj.worldPosition.copy()
  328. self.FSM.owner.lane_point = self.FSM.owner.target.obj.worldPosition.copy()
  329. self.FSM.owner.last_lane_point = self.FSM.owner.target.obj.worldPosition.copy()
  330. self.FSM.owner.point = self.FSM.owner.target.obj.worldPosition.copy()
  331. self.FSM.owner.path_index = 0
  332. self.FSM.owner.path = None
  333. self.FSM.owner.manager.cars_active.remove(self.FSM.owner)
  334. self.FSM.owner.obj.suspendDynamics()
  335. def Execute(self):
  336. self.FSM.stateLife += 1
  337. self.v_to_target = self.FSM.owner.obj.getVectTo(self.park_loc)
  338. goal_met = move_to_point(self)
  339. set_height(self.FSM.owner)
  340. if self.v_to_target[0] > 2:
  341. align_to_target(self.FSM.owner, .65)
  342. else:
  343. self.FSM.owner.obj.alignAxisToVect(-self.FSM.owner.target.obj.worldOrientation[0], 0, .05)
  344. if self.FSM.stateLife == 440 or goal_met:
  345. self.cleanup()
  346. #self.FSM.owner.obj.suspendPhysics()
  347. def Exit(self):
  348. pass
  349. #====================================
  350. class NavigateToTarget(State):
  351. def __init__(self,FSM):
  352. super(NavigateToTarget, self).__init__(FSM)
  353. def Enter(self):
  354. self.FSM.stateLife = 1
  355. self.FSM.owner.obj.actuators['cs'].volume = .5
  356. #self.FSM.owner.obj.actuators['cs'].startSound()
  357. super(NavigateToTarget, self).Enter()
  358. def Execute(self):
  359. self.FSM.stateLife += 1
  360. self.forward_ray = forward_ray(self.FSM.owner)
  361. update_point(self.FSM.owner)
  362. align_to_point(self.FSM.owner, 1)
  363. align_to_road(self.FSM.owner)
  364. set_height(self.FSM.owner)
  365. delta_to_vect(self.FSM.owner)
  366. gas = True
  367. #print(self.FSM.owner.manager.parent.name)
  368. #self.FSM.owner.manager.parent['sndmgr'].queue_sound(['engine_idle', self.FSM.owner.obj, self.FSM.owner.manager.parent, 1, 1])
  369. #self.FSM.owner.manager.parent['sndmgr'].queue_sound(['engine_idle', self.FSM.owner.manager.parent, self.FSM.owner.obj, .5, 1])
  370. #print(self.FSM.owner.obj.actuators['cs'])
  371. if self.forward_ray[0]:
  372. dist = self.FSM.owner.obj.getDistanceTo(self.forward_ray[1])
  373. if dist < 10:
  374. gas = False
  375. self.FSM.owner.obj.linearVelocity.x *= .98
  376. #print(dist, 'forward ray', self.forward_ray[0])
  377. if gas:
  378. apply_gas(self.FSM.owner, 1)
  379. pitch = self.FSM.owner.obj.linearVelocity.x
  380. pitch = pitch / self.FSM.owner.manager.default_speed
  381. pitch2 = abs(round(pitch * 1.5, 2))
  382. #if pitch2 > .75:
  383. #self.FSM.owner.obj.actuators['cs'].pitch = 1.25
  384. #else:
  385. #self.FSM.owner.obj.actuators['cs'].pitch = .75
  386. self.FSM.owner.obj.actuators['cs'].pitch = pitch2
  387. #print(pitch2, 'pitch')
  388. #emergency exit
  389. if self.FSM.stateLife > 30 * 90:
  390. get_parking_type(self.FSM.owner)
  391. #self.FSM.ToTransition('toEnterParallelPark')
  392. def Exit(self):
  393. self.FSM.owner.obj.actuators['cs'].volume = .0
  394. pass
  395. #====================================
  396. class Activate(State):
  397. def __init__(self,FSM):
  398. super(Activate, self).__init__(FSM)
  399. def Enter(self):
  400. self.FSM.stateLife = 1
  401. super(Activate, self).Enter()
  402. def find_target(self):
  403. pass
  404. def drive_to_point(self):
  405. pass
  406. def Execute(self):
  407. self.FSM.stateLife += 1
  408. def Exit(self):
  409. pass
  410. #====================================
  411. class RequestPath(State):
  412. def __init__(self,FSM):
  413. super(RequestPath, self).__init__(FSM)
  414. def Enter(self):
  415. self.FSM.stateLife = 1
  416. self.FSM.owner.target, self.FSM.owner.path = find_new_parking(self.FSM.owner)
  417. self.FSM.owner.path_index = 0
  418. super(RequestPath, self).Enter()
  419. def Execute(self):
  420. self.FSM.stateLife += 1
  421. def Exit(self):
  422. pass