|
@@ -62,7 +62,7 @@ def find_new_parking(self):
|
62
|
62
|
min_dist = 100
|
63
|
63
|
dist = self.obj.getDistanceTo(x.obj)
|
64
|
64
|
if dist < min_dist:
|
65
|
|
- potentials.remove(x)
|
|
65
|
+ #potentials.remove(x)
|
66
|
66
|
print('----removing potential')
|
67
|
67
|
if len(potentials) > 0:
|
68
|
68
|
|
|
@@ -84,9 +84,11 @@ def get_lane_point(self):
|
84
|
84
|
nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees
|
85
|
85
|
self.last_lane_point = self.lane_point
|
86
|
86
|
self.lane_point = self.point + self.manager.lane_position * nv
|
|
87
|
+ else:
|
|
88
|
+ print('skipping lane point')
|
87
|
89
|
|
88
|
90
|
def update_point(self):
|
89
|
|
-
|
|
91
|
+ print(self.path_index, 'path index', len(self.path))
|
90
|
92
|
if self.path_index >= (len(self.path) ):
|
91
|
93
|
self.FSM.FSM.ToTransition('toEnterParallelPark')
|
92
|
94
|
else:
|
|
@@ -111,7 +113,12 @@ def delta_to_vect(self):
|
111
|
113
|
delta_mult = -.1
|
112
|
114
|
mult = 1.0
|
113
|
115
|
deltamove = delta[2] * delta_mult
|
114
|
|
- f = deltamove * 5000
|
|
116
|
+
|
|
117
|
+ #check if in front
|
|
118
|
+ local = self.obj.worldOrientation.inverted() * (self.lane_point - self.obj.worldPosition)
|
|
119
|
+ f = deltamove * 50
|
|
120
|
+ if local < 0:
|
|
121
|
+ f *= -1
|
115
|
122
|
self.obj.applyForce([0, f, 0], True)
|
116
|
123
|
|
117
|
124
|
|
|
@@ -193,16 +200,19 @@ class EnterParallelPark(State):
|
193
|
200
|
def Enter(self):
|
194
|
201
|
self.FSM.stateLife = 1
|
195
|
202
|
print('entering parallel park')
|
196
|
|
- self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition
|
197
|
|
- self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation
|
|
203
|
+ self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition.copy()
|
|
204
|
+ self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation.copy()
|
198
|
205
|
self.FSM.owner.obj.applyMovement([0, -6, 0], True)
|
199
|
206
|
self.FSM.owner.target.status = 'in_use'
|
200
|
207
|
self.FSM.owner.obj.worldPosition.z += .9
|
201
|
208
|
self.FSM.owner.active = False
|
202
|
209
|
self.FSM.owner.start_empty = self.FSM.owner.target
|
203
|
210
|
self.FSM.owner.last_point = self.FSM.owner.target.obj.worldPosition.copy()
|
204
|
|
- self.FSM.owner.last__lane_point = self.FSM.owner.obj.worldPosition
|
|
211
|
+ self.FSM.owner.last_lane_point = self.FSM.owner.obj.worldPosition.copy()
|
205
|
212
|
self.FSM.owner.point = self.FSM.owner.target.obj.worldPosition.copy()
|
|
213
|
+ self.FSM.owner.path_index = 0
|
|
214
|
+ self.FSM.owner.path = None
|
|
215
|
+ print('clearning last path')
|
206
|
216
|
|
207
|
217
|
clear_markers(self)
|
208
|
218
|
|
|
@@ -236,11 +246,11 @@ class NavigateToTarget(State):
|
236
|
246
|
align_to_point(self.FSM.owner)
|
237
|
247
|
align_to_road(self.FSM.owner)
|
238
|
248
|
set_height(self.FSM.owner)
|
239
|
|
- delta_to_vect(self.FSM.owner)
|
|
249
|
+ #delta_to_vect(self.FSM.owner)
|
240
|
250
|
apply_gas(self.FSM.owner)
|
241
|
251
|
|
242
|
252
|
#emergency exit
|
243
|
|
- if self.FSM.stateLife > 30 * 60:
|
|
253
|
+ if self.FSM.stateLife > 30 * 100:
|
244
|
254
|
self.FSM.ToTransition('toEnterParallelPark')
|
245
|
255
|
|
246
|
256
|
def Exit(self):
|
|
@@ -260,93 +270,94 @@ class Activate(State):
|
260
|
270
|
pass
|
261
|
271
|
|
262
|
272
|
def drive_to_point(self):
|
263
|
|
- if self.FSM.path:
|
264
|
|
- ground_ray = get_ground_ray(self)
|
265
|
|
- if ground_ray[0]:
|
266
|
|
- set_height(self, ground_ray)
|
267
|
|
- align_to_road(self, ground_ray)
|
268
|
|
-
|
269
|
|
- v = self.FSM.owner.getVectTo(self.lane_point)
|
270
|
|
- speed_force = 800.0
|
271
|
|
- max_speed = 5.0
|
272
|
|
- behind = False
|
273
|
|
- local = self.FSM.owner.worldOrientation.inverted() * (self.lane_point - self.FSM.owner.worldPosition)
|
274
|
|
- v2 = v[1].copy()
|
275
|
|
- v2.z = 0
|
276
|
|
- delta = self.last_lane_point - self.lane_point
|
277
|
|
- delta = delta.cross(v[1])
|
278
|
|
- delta_mult = -.1
|
279
|
|
- mult = 1.0
|
280
|
|
- backup_time = 20
|
281
|
|
-
|
282
|
|
- #change max speed
|
283
|
|
- if self.FSM.stateLife % 180 == 0:
|
284
|
|
- print('change speed force')
|
285
|
|
- self.speed_force = random.choice([max_speed * 1.3, max_speed * 1.6, max_speed * .8, max_speed * .6, max_speed])
|
286
|
|
-
|
287
|
|
- if local.x > 0 and self.backup == 0:
|
288
|
|
-
|
289
|
|
- if self.FSM.owner.linearVelocity.x < self.max_speed:
|
290
|
|
- self.FSM.owner.applyForce([speed_force, 0, 0], True)
|
291
|
|
-
|
292
|
|
- deltamove = delta[2] * delta_mult
|
293
|
|
- #self.FSM.owner.applyMovement([0, deltamove, 0], True)
|
294
|
|
-
|
295
|
|
- f = deltamove * 5000
|
296
|
|
- self.FSM.owner.applyForce([0, f, 0], True)
|
297
|
|
- v = self.FSM.owner.getVectTo(self.lane_point)
|
298
|
|
- v2 = v[1].copy()
|
299
|
|
- v2.z = 0
|
300
|
|
- self.FSM.owner.alignAxisToVect(v2, 0, .05)
|
301
|
|
-
|
302
|
|
-
|
303
|
|
- else:
|
304
|
|
- if local.x < 0:
|
305
|
|
- self.backup = backup_time
|
306
|
|
-
|
307
|
|
- if self.backup > 0:
|
308
|
|
- print('backing up')
|
309
|
|
- v = self.FSM.owner.getVectTo(self.FSM.path[0])
|
310
|
|
- v2 = v[1].copy()
|
311
|
|
- v2.z = 0
|
312
|
|
- self.FSM.owner.alignAxisToVect(v2, 0, .02)
|
313
|
|
- if self.FSM.owner.linearVelocity.x > -max_speed / 2:
|
314
|
|
- self.FSM.owner.applyForce([-speed_force * .8, 0, 0], True)
|
315
|
|
- self.backup -= 1
|
316
|
|
-
|
317
|
|
- dist = self.FSM.owner.getDistanceTo(self.lane_point)
|
318
|
|
-
|
319
|
|
- if dist < 2.5 and (len(self.FSM.path) > 0):
|
320
|
|
- #print(self.FSM.path,'this is the path')
|
321
|
|
- #print('navmesh point removed')
|
322
|
|
- self.last_point = self.point
|
323
|
|
- self.last_lane_point = self.lane_point
|
324
|
|
- self.FSM.path.remove(self.FSM.path[0])
|
325
|
|
-
|
326
|
|
- if len(self.FSM.path) > 0:
|
327
|
|
- self.point = self.FSM.path[0]
|
328
|
|
- v = Vector([self.last_point.x - self.point.x, self.last_point.y - self.point.y, 0])
|
329
|
|
- tv = v.normalized()
|
330
|
|
- nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees
|
331
|
|
- self.lane_point = self.point + self.lane_position * nv
|
332
|
|
-
|
333
|
|
- else:
|
334
|
|
- self.point = None
|
335
|
|
-
|
336
|
|
- if self.FSM.path == []:
|
337
|
|
- self.FSM.curTarget = None
|
338
|
|
-
|
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:
|
|
273
|
+ pass
|
|
274
|
+ # if self.FSM.path:
|
|
275
|
+ # ground_ray = get_ground_ray(self)
|
|
276
|
+ # if ground_ray[0]:
|
|
277
|
+ # set_height(self, ground_ray)
|
|
278
|
+ # align_to_road(self, ground_ray)
|
|
279
|
+
|
|
280
|
+ # v = self.FSM.owner.getVectTo(self.lane_point)
|
|
281
|
+ # speed_force = 800.0
|
|
282
|
+ # max_speed = 5.0
|
|
283
|
+ # behind = False
|
|
284
|
+ # local = self.FSM.owner.worldOrientation.inverted() * (self.lane_point - self.FSM.owner.worldPosition)
|
|
285
|
+ # v2 = v[1].copy()
|
|
286
|
+ # v2.z = 0
|
|
287
|
+ # delta = self.last_lane_point - self.lane_point
|
|
288
|
+ # delta = delta.cross(v[1])
|
|
289
|
+ # delta_mult = -.1
|
|
290
|
+ # mult = 1.0
|
|
291
|
+ # backup_time = 20
|
|
292
|
+
|
|
293
|
+ # #change max speed
|
|
294
|
+ # if self.FSM.stateLife % 180 == 0:
|
|
295
|
+ # print('change speed force')
|
|
296
|
+ # self.speed_force = random.choice([max_speed * 1.3, max_speed * 1.6, max_speed * .8, max_speed * .6, max_speed])
|
|
297
|
+
|
|
298
|
+ # if local.x > 0 and self.backup == 0:
|
|
299
|
+
|
|
300
|
+ # if self.FSM.owner.linearVelocity.x < self.max_speed:
|
|
301
|
+ # self.FSM.owner.applyForce([speed_force, 0, 0], True)
|
|
302
|
+
|
|
303
|
+ # deltamove = delta[2] * delta_mult
|
|
304
|
+ # #self.FSM.owner.applyMovement([0, deltamove, 0], True)
|
|
305
|
+
|
|
306
|
+ # f = deltamove * 5000
|
|
307
|
+ # self.FSM.owner.applyForce([0, f, 0], True)
|
|
308
|
+ # v = self.FSM.owner.getVectTo(self.lane_point)
|
|
309
|
+ # v2 = v[1].copy()
|
|
310
|
+ # v2.z = 0
|
|
311
|
+ # self.FSM.owner.alignAxisToVect(v2, 0, .05)
|
|
312
|
+
|
|
313
|
+
|
|
314
|
+ # else:
|
|
315
|
+ # if local.x < 0:
|
|
316
|
+ # self.backup = backup_time
|
|
317
|
+
|
|
318
|
+ # if self.backup > 0:
|
|
319
|
+ # print('backing up')
|
|
320
|
+ # v = self.FSM.owner.getVectTo(self.FSM.path[0])
|
|
321
|
+ # v2 = v[1].copy()
|
|
322
|
+ # v2.z = 0
|
|
323
|
+ # self.FSM.owner.alignAxisToVect(v2, 0, .02)
|
|
324
|
+ # if self.FSM.owner.linearVelocity.x > -max_speed / 2:
|
|
325
|
+ # self.FSM.owner.applyForce([-speed_force * .8, 0, 0], True)
|
|
326
|
+ # self.backup -= 1
|
|
327
|
+
|
|
328
|
+ # dist = self.FSM.owner.getDistanceTo(self.lane_point)
|
|
329
|
+
|
|
330
|
+ # if dist < 2.5 and (len(self.FSM.path) > 0):
|
|
331
|
+ # #print(self.FSM.path,'this is the path')
|
|
332
|
+ # #print('navmesh point removed')
|
|
333
|
+ # self.last_point = self.point
|
|
334
|
+ # self.last_lane_point = self.lane_point
|
|
335
|
+ # self.FSM.path.remove(self.FSM.path[0])
|
|
336
|
+
|
|
337
|
+ # if len(self.FSM.path) > 0:
|
|
338
|
+ # self.point = self.FSM.path[0]
|
|
339
|
+ # v = Vector([self.last_point.x - self.point.x, self.last_point.y - self.point.y, 0])
|
|
340
|
+ # tv = v.normalized()
|
|
341
|
+ # nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees
|
|
342
|
+ # self.lane_point = self.point + self.lane_position * nv
|
|
343
|
+
|
|
344
|
+ # else:
|
|
345
|
+ # self.point = None
|
|
346
|
+
|
|
347
|
+ # if self.FSM.path == []:
|
|
348
|
+ # self.FSM.curTarget = None
|
|
349
|
+
|
|
350
|
+ # #progress
|
|
351
|
+ # self.pos_his.append(self.FSM.owner.worldPosition.copy())
|
|
352
|
+ # pos_his_len = len(self.pos_his)
|
|
353
|
+ # if pos_his_len > 200:
|
343
|
354
|
|
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]
|
|
355
|
+ # sum_ = abs(self.FSM.owner.worldPosition.x - self.pos_his[0][0]) + abs(self.FSM.owner.worldPosition.y - self.pos_his[0][1])
|
|
356
|
+ # #print(sum_, 'sum')
|
|
357
|
+ # if sum_ < .05:
|
|
358
|
+ # self.backup = backup_time
|
|
359
|
+ # print('progress stopped')
|
|
360
|
+ # del self.pos_his[0]
|
350
|
361
|
|
351
|
362
|
|
352
|
363
|
def Execute(self):
|