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