|
@@ -20,6 +20,10 @@ def mark_path(path, y):
|
20
|
20
|
if iter_ == (len(path) +1):
|
21
|
21
|
pm.color = [1,0,1,.4]
|
22
|
22
|
y.path_display.append(pm)
|
|
23
|
+def single_marker(pos, col):
|
|
24
|
+ pm = bge.logic.getCurrentScene().addObject('path_marker', None, 0)
|
|
25
|
+ pm.worldPosition = pos
|
|
26
|
+ pm.color = col
|
23
|
27
|
|
24
|
28
|
def clear_markers(self):
|
25
|
29
|
for x in self.FSM.owner.path_display:
|
|
@@ -73,7 +77,8 @@ def find_new_parking(self):
|
73
|
77
|
|
74
|
78
|
return new_parking, path2
|
75
|
79
|
else:
|
76
|
|
- self.FSM.FSM.ToTransition('toEnterParallelPark')
|
|
80
|
+ get_parking_type(self)
|
|
81
|
+ #self.FSM.FSM.ToTransition('toEnterParallelPark')
|
77
|
82
|
|
78
|
83
|
def get_lane_point(self):
|
79
|
84
|
self.point = self.path[self.path_index]
|
|
@@ -88,7 +93,8 @@ def get_lane_point(self):
|
88
|
93
|
|
89
|
94
|
def update_point(self):
|
90
|
95
|
if self.path_index >= (len(self.path) ):
|
91
|
|
- self.FSM.FSM.ToTransition('toEnterParallelPark')
|
|
96
|
+ get_parking_type(self)
|
|
97
|
+ #self.FSM.FSM.ToTransition('toEnterParallelPark')
|
92
|
98
|
else:
|
93
|
99
|
dist = self.obj.getDistanceTo(self.lane_point)
|
94
|
100
|
self.point = self.path[self.path_index]
|
|
@@ -104,6 +110,12 @@ def align_to_point(self, mult):
|
104
|
110
|
v.z = 0
|
105
|
111
|
self.obj.alignAxisToVect(v, 0, .05 * mult)
|
106
|
112
|
|
|
113
|
+def align_to_target(self, mult):
|
|
114
|
+ v = self.obj.getVectTo(self.FSM.FSM.curState.park_loc)[1]
|
|
115
|
+ v.z = 0
|
|
116
|
+ self.obj.alignAxisToVect(v, 0, .05 * mult)
|
|
117
|
+
|
|
118
|
+
|
107
|
119
|
def delta_to_vect(self):
|
108
|
120
|
v = self.obj.getVectTo(self.lane_point)[1]
|
109
|
121
|
delta = self.last_lane_point - self.lane_point
|
|
@@ -125,37 +137,15 @@ def apply_gas(self, mult):
|
125
|
137
|
self.obj.applyForce([self.speed_inc * mult, 0, 0], True)
|
126
|
138
|
|
127
|
139
|
def starting_mod(self):
|
128
|
|
- #print('doing path mod')
|
129
|
|
- #self.path[0] = self.start_empty.obj.worldPosition
|
130
|
|
- #del self.path[0]
|
131
|
140
|
path2 = self.path
|
132
|
|
- path2.append(self.target.obj.worldPosition) #add endpoint
|
133
|
|
- path2.append(self.target.obj.worldPosition)
|
134
|
|
-
|
135
|
|
- #check if vector between 1 and start point and vector between 2 and sp are the same dir
|
136
|
|
- # v1 = Vector(self.start_empty.obj.worldPosition).normalized() - Vector(self.path[0]).normalized()
|
137
|
|
- # v2 = Vector(self.start_empty.obj.worldPosition).normalized() - Vector(self.path[1]).normalized()
|
138
|
|
- # lens = []
|
139
|
|
- # lens.append(len(self.path))
|
140
|
|
- # if v1.dot(v2) < 0:
|
141
|
|
- # del self.path[0]
|
142
|
|
- # lens.append(len(self.path))
|
143
|
|
- # del self.path[0]
|
144
|
|
- # lens.append(len(self.path))
|
145
|
|
- # del self.path[0]
|
146
|
|
- # lens.append(len(self.path))
|
147
|
|
- # print(lens, 'path length', v1.dot(v2))
|
|
141
|
+ #path2.append(self.target.obj.worldPosition) #add endpoint
|
|
142
|
+ #path2.append(self.target.obj.worldPosition)
|
148
|
143
|
|
149
|
144
|
def check_front(self): #
|
150
|
|
- #obj_vec = self.obj.worldOrientation[0].normalized()
|
151
|
145
|
obj_vec = self.obj.getAxisVect([1,0,0])
|
152
|
146
|
targ_vec = self.obj.getVectTo(self.path[self.path_index] +1)[1].normalized()
|
153
|
147
|
dot_ = obj_vec.dot(targ_vec)
|
154
|
148
|
|
155
|
|
- # while dot_ < 0:
|
156
|
|
- # del self.path[0]
|
157
|
|
- # targ_vec = self.obj.getVectTo(self.path[0])[1].normalized()
|
158
|
|
- # dot_ = obj_vec.dot(targ_vec)
|
159
|
149
|
if dot_ > 0:
|
160
|
150
|
print('in front', self)
|
161
|
151
|
else:
|
|
@@ -164,17 +154,47 @@ def check_front(self): #
|
164
|
154
|
if dot_ > 0:
|
165
|
155
|
return True
|
166
|
156
|
else:
|
|
157
|
+ return False
|
|
158
|
+
|
|
159
|
+def get_parking_type(self):
|
|
160
|
+ if self.target.type == 'perp':
|
|
161
|
+ self.FSM.FSM.ToTransition('toEnterPerpPark')
|
|
162
|
+ else:
|
|
163
|
+ self.FSM.FSM.ToTransition('toEnterParallelPark')
|
|
164
|
+
|
|
165
|
+def get_parking_point(self):
|
|
166
|
+ v = Vector([self.last_lane_point.x - self.point.x, self.last_lane_point.y - self.point.y, 0])
|
|
167
|
+ tv = v.normalized()
|
|
168
|
+ #nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees
|
|
169
|
+ #self.last_lane_point = self.lane_point
|
|
170
|
+
|
|
171
|
+ local = Vector([7,0,0])
|
|
172
|
+ return Vector(self.target.obj.worldPosition + (self.target.obj.worldOrientation * local))
|
|
173
|
+
|
|
174
|
+
|
|
175
|
+ #return Vector(self.target.obj.worldPosition) - 7 * tv
|
|
176
|
+
|
|
177
|
+def move_to_point(self):
|
|
178
|
+ #print(self.v_to_target[0])
|
|
179
|
+ if self.v_to_target[0] < .95:
|
|
180
|
+ #print('reached')
|
|
181
|
+ return True
|
|
182
|
+ else:
|
|
183
|
+ vectTo = Vector(self.v_to_target[1])
|
|
184
|
+ vectTo.magnitude = 4.01
|
|
185
|
+ self.FSM.owner.obj.linearVelocity = vectTo
|
167
|
186
|
return False
|
168
|
|
- #get starting pos
|
169
|
|
- # v = Vector([path2[0].x - path2[1].x, path2[0].y - path2[1].y, 0])
|
170
|
|
- # tv = v.normalized()
|
171
|
|
- # nv = Vector([-tv.y, tv.x, 0])
|
172
|
|
- # print('og', path2[0])
|
173
|
|
- # path2[0] = path2[0] + -.2 * nv
|
174
|
|
- # print('mod1', path2[0])
|
175
|
|
- # #path2[0] *= nv
|
176
|
|
- # print('mod2', path2[0])
|
177
|
|
- # self.path = path2
|
|
187
|
+
|
|
188
|
+def forward_ray(self):
|
|
189
|
+
|
|
190
|
+ Axis = 0
|
|
191
|
+ Distance = 30
|
|
192
|
+ End = self.obj.worldPosition + (self.obj.worldOrientation.col[Axis]*Distance)
|
|
193
|
+ Start = self.obj.worldPosition.copy()
|
|
194
|
+ return self.obj.rayCast(End, Start, 10,'traffic', 1, 0, 0)
|
|
195
|
+
|
|
196
|
+#dist = v1[0]
|
|
197
|
+#print(dist)
|
178
|
198
|
|
179
|
199
|
#====================================
|
180
|
200
|
|
|
@@ -222,9 +242,10 @@ class ExitParallelPark(State):
|
222
|
242
|
self.FSM.owner.obj.restoreDynamics()
|
223
|
243
|
self.FSM.owner.obj.linearVelocity = [0,0,0]
|
224
|
244
|
self.FSM.owner.path_index = 1
|
225
|
|
- get_lane_point(self.FSM.owner)
|
|
245
|
+
|
226
|
246
|
try:
|
227
|
247
|
self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index]
|
|
248
|
+ get_lane_point(self.FSM.owner)
|
228
|
249
|
except:
|
229
|
250
|
print('no path')
|
230
|
251
|
self.FSM.owner.target.status = 'targetted'
|
|
@@ -264,12 +285,69 @@ class ExitParallelPark(State):
|
264
|
285
|
|
265
|
286
|
#====================================
|
266
|
287
|
|
|
288
|
+
|
|
289
|
+class ExitPerpPark(State):
|
|
290
|
+ def __init__(self,FSM):
|
|
291
|
+ super(ExitPerpPark, self).__init__(FSM)
|
|
292
|
+
|
|
293
|
+ def Enter(self):
|
|
294
|
+ self.FSM.stateLife = 1
|
|
295
|
+ self.FSM.owner.obj.restorePhysics()
|
|
296
|
+ self.FSM.owner.obj.restoreDynamics()
|
|
297
|
+ self.FSM.owner.obj.linearVelocity = [0,0,0]
|
|
298
|
+ self.FSM.owner.path_index = 1
|
|
299
|
+
|
|
300
|
+ try:
|
|
301
|
+ self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index]
|
|
302
|
+ get_lane_point(self.FSM.owner)
|
|
303
|
+ except:
|
|
304
|
+ print('no path')
|
|
305
|
+ self.FSM.owner.target.status = 'targetted'
|
|
306
|
+ self.FSM.owner.start_empty.status = 'available'
|
|
307
|
+ starting_mod(self.FSM.owner)
|
|
308
|
+ super(ExitPerpPark, self).Enter()
|
|
309
|
+
|
|
310
|
+ def Execute(self):
|
|
311
|
+ #print('exiting perp park', self.FSM.owner.start_empty.type)
|
|
312
|
+ self.FSM.stateLife += 1
|
|
313
|
+ set_height(self.FSM.owner)
|
|
314
|
+ if self.FSM.owner.path != []:
|
|
315
|
+ update_point(self.FSM.owner)
|
|
316
|
+ if self.FSM.owner.path_index > 1 or self.FSM.stateLife > 300:
|
|
317
|
+ self.FSM.ToTransition('toNavigateToTarget')
|
|
318
|
+
|
|
319
|
+
|
|
320
|
+ #align_to_point(self.FSM.owner, .65)
|
|
321
|
+ align_to_road(self.FSM.owner)
|
|
322
|
+ #set_height(self.FSM.owner)
|
|
323
|
+ #delta_to_vect(self.FSM.owner)
|
|
324
|
+ #apply_gas(self.FSM.owner, .5)
|
|
325
|
+
|
|
326
|
+ if self.FSM.stateLife < 120:
|
|
327
|
+ self.FSM.owner.obj.applyForce([-50,0,0], True)
|
|
328
|
+
|
|
329
|
+ def Exit(self):
|
|
330
|
+ pass
|
|
331
|
+
|
|
332
|
+#====================================
|
|
333
|
+
|
|
334
|
+
|
|
335
|
+
|
|
336
|
+
|
|
337
|
+
|
|
338
|
+
|
|
339
|
+
|
267
|
340
|
class EnterParallelPark(State):
|
268
|
341
|
def __init__(self,FSM):
|
269
|
342
|
super(EnterParallelPark, self).__init__(FSM)
|
270
|
343
|
|
271
|
344
|
def Enter(self):
|
272
|
345
|
self.FSM.stateLife = 1
|
|
346
|
+
|
|
347
|
+ clear_markers(self)
|
|
348
|
+ super(EnterParallelPark, self).Enter()
|
|
349
|
+
|
|
350
|
+ def cleanup(self):
|
273
|
351
|
self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition.copy()
|
274
|
352
|
self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation.copy()
|
275
|
353
|
self.FSM.owner.obj.applyMovement([0, -6, 0], True)
|
|
@@ -283,18 +361,69 @@ class EnterParallelPark(State):
|
283
|
361
|
self.FSM.owner.point = self.FSM.owner.target.obj.worldPosition.copy()
|
284
|
362
|
self.FSM.owner.path_index = 0
|
285
|
363
|
self.FSM.owner.path = None
|
286
|
|
- clear_markers(self)
|
287
|
|
- super(EnterParallelPark, self).Enter()
|
|
364
|
+
|
|
365
|
+ self.FSM.owner.manager.cars_active.remove(self.FSM.owner)
|
|
366
|
+ self.FSM.owner.obj.suspendDynamics()
|
288
|
367
|
|
289
|
368
|
def Execute(self):
|
290
|
369
|
self.FSM.stateLife += 1
|
291
|
|
- if self.FSM.stateLife == 2:
|
292
|
|
- self.FSM.owner.manager.cars_active.remove(self.FSM.owner)
|
293
|
|
- self.FSM.owner.obj.suspendDynamics()
|
|
370
|
+ if self.FSM.stateLife == 240:
|
|
371
|
+ self.cleanup()
|
294
|
372
|
#self.FSM.owner.obj.suspendPhysics()
|
295
|
373
|
def Exit(self):
|
296
|
374
|
pass
|
297
|
375
|
|
|
376
|
+class EnterPerpPark(State):
|
|
377
|
+ def __init__(self,FSM):
|
|
378
|
+ super(EnterPerpPark, self).__init__(FSM)
|
|
379
|
+
|
|
380
|
+ def Enter(self):
|
|
381
|
+ self.FSM.stateLife = 1
|
|
382
|
+ self.park_loc = get_parking_point(self.FSM.owner)
|
|
383
|
+ single_marker(self.park_loc, [1,0,0,.3])
|
|
384
|
+
|
|
385
|
+ clear_markers(self)
|
|
386
|
+ super(EnterPerpPark, self).Enter()
|
|
387
|
+
|
|
388
|
+ def cleanup(self):
|
|
389
|
+ self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition.copy()
|
|
390
|
+ self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation.copy()
|
|
391
|
+ self.FSM.owner.obj.applyMovement([7, 0, 0], True)
|
|
392
|
+ self.FSM.owner.target.status = 'in_use'
|
|
393
|
+ self.FSM.owner.obj.worldPosition.z += .9
|
|
394
|
+ self.FSM.owner.active = False
|
|
395
|
+ self.FSM.owner.start_empty = self.FSM.owner.target
|
|
396
|
+ self.FSM.owner.last_point = self.FSM.owner.target.obj.worldPosition.copy()
|
|
397
|
+ self.FSM.owner.lane_point = self.FSM.owner.target.obj.worldPosition.copy()
|
|
398
|
+ self.FSM.owner.last_lane_point = self.FSM.owner.target.obj.worldPosition.copy()
|
|
399
|
+ self.FSM.owner.point = self.FSM.owner.target.obj.worldPosition.copy()
|
|
400
|
+ self.FSM.owner.path_index = 0
|
|
401
|
+ self.FSM.owner.path = None
|
|
402
|
+
|
|
403
|
+ self.FSM.owner.manager.cars_active.remove(self.FSM.owner)
|
|
404
|
+ self.FSM.owner.obj.suspendDynamics()
|
|
405
|
+
|
|
406
|
+ def Execute(self):
|
|
407
|
+ self.FSM.stateLife += 1
|
|
408
|
+
|
|
409
|
+ self.v_to_target = self.FSM.owner.obj.getVectTo(self.park_loc)
|
|
410
|
+
|
|
411
|
+ goal_met = move_to_point(self)
|
|
412
|
+ set_height(self.FSM.owner)
|
|
413
|
+ if self.v_to_target[0] > 2:
|
|
414
|
+
|
|
415
|
+ align_to_target(self.FSM.owner, .65)
|
|
416
|
+
|
|
417
|
+ else:
|
|
418
|
+ self.FSM.owner.obj.alignAxisToVect(-self.FSM.owner.target.obj.worldOrientation[0], 0, .05)
|
|
419
|
+
|
|
420
|
+ if self.FSM.stateLife == 440 or goal_met:
|
|
421
|
+ self.cleanup()
|
|
422
|
+ #self.FSM.owner.obj.suspendPhysics()
|
|
423
|
+
|
|
424
|
+ def Exit(self):
|
|
425
|
+ pass
|
|
426
|
+
|
298
|
427
|
#====================================
|
299
|
428
|
|
300
|
429
|
class NavigateToTarget(State):
|
|
@@ -308,17 +437,29 @@ class NavigateToTarget(State):
|
308
|
437
|
|
309
|
438
|
def Execute(self):
|
310
|
439
|
self.FSM.stateLife += 1
|
|
440
|
+ self.forward_ray = forward_ray(self.FSM.owner)
|
311
|
441
|
update_point(self.FSM.owner)
|
312
|
442
|
|
313
|
443
|
align_to_point(self.FSM.owner, 1)
|
314
|
444
|
align_to_road(self.FSM.owner)
|
315
|
445
|
set_height(self.FSM.owner)
|
316
|
446
|
delta_to_vect(self.FSM.owner)
|
317
|
|
- apply_gas(self.FSM.owner, 1)
|
|
447
|
+ gas = True
|
|
448
|
+ if self.forward_ray[0]:
|
|
449
|
+
|
|
450
|
+ dist = self.FSM.owner.obj.getDistanceTo(self.forward_ray[1])
|
|
451
|
+ if dist < 10:
|
|
452
|
+ gas = False
|
|
453
|
+ self.FSM.owner.obj.linearVelocity.x *= .98
|
|
454
|
+ print(dist, 'forward ray', self.forward_ray[0])
|
|
455
|
+
|
|
456
|
+ if gas:
|
|
457
|
+ apply_gas(self.FSM.owner, 1)
|
318
|
458
|
|
319
|
459
|
#emergency exit
|
320
|
460
|
if self.FSM.stateLife > 30 * 90:
|
321
|
|
- self.FSM.ToTransition('toEnterParallelPark')
|
|
461
|
+ get_parking_type(self.FSM.owner)
|
|
462
|
+ #self.FSM.ToTransition('toEnterParallelPark')
|
322
|
463
|
|
323
|
464
|
def Exit(self):
|
324
|
465
|
pass
|