import utils import bge import random from mathutils import Vector #==================================== def mark_path(path, y): iter_ = 0 for x in path: pm = bge.logic.getCurrentScene().addObject('path_marker', y.obj, 0) pm.worldPosition = path[iter_] iter_ += 1 if iter_ == 1: pm.color = [0,1,0,.4] if iter_ == (len(path) ): pm.color = [1,0,0,.4] if iter_ == (len(path) +1): pm.color = [1,0,1,.4] y.path_display.append(pm) def clear_markers(self): # for x in bge.logic.getCurrentScene().objects: # if 'path_marker' in x.name: # x.endObject() for x in self.FSM.owner.path_display: try: x.endObject() except: pass def get_ground_ray(self): Axis = 2 Distance = -10 end = self.obj.worldPosition + (self.obj.worldOrientation.col[Axis]*Distance) start = self.obj.worldPosition.copy() ground_ray = self.obj.rayCast(end, start, 6,'', 1, 0) return ground_ray def set_height(self): ground_ray = get_ground_ray(self) target_height = 0.9 hitpoint = ground_ray[1] try: dist = self.obj.getDistanceTo(hitpoint) if dist < target_height: self.obj.worldPosition.z += target_height - dist self.obj.linearVelocity.z = 0 self.obj.linearVelocity.y *= .1 except: pass def align_to_road(self): ground_ray = get_ground_ray(self) try: self.obj.alignAxisToVect(ground_ray[2], 2, .15) except: pass def find_new_parking(self): potentials = [] for x in self.manager.parking_spots: if x.status == 'available': potentials.append(x) for x in potentials: min_dist = 100 dist = self.obj.getDistanceTo(x.obj) if dist < min_dist: potentials.remove(x) print('----removing potential') if len(potentials) > 0: new_parking = random.choice(potentials) path = self.manager.navmesh.findPath(self.start_empty.obj.worldPosition, new_parking.obj.worldPosition) print('parking added at', new_parking.obj.worldPosition) mark_path(path, self) return new_parking, path else: print('cant find parking for', self) self.FSM.FSM.ToTransition('toEnterParallelPark') def get_lane_point(self): self.point = self.path[self.path_index] #print(self.path_index, 'path index') if self.point != self.last_lane_point: v = Vector([self.last_lane_point.x - self.point.x, self.last_lane_point.y - self.point.y, 0]) tv = v.normalized() nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees self.last_lane_point = self.lane_point self.lane_point = self.point + self.manager.lane_position * nv def update_point(self): if self.path_index >= (len(self.path) ): self.FSM.FSM.ToTransition('toEnterParallelPark') else: dist = self.obj.getDistanceTo(self.lane_point) #print(dist, self.path_index) self.point = self.path[self.path_index] if dist < 2.5: get_lane_point(self) if self.path_index > (len(self.path)): pass else: self.path_index += 1 def align_to_point(self): v = self.obj.getVectTo(self.lane_point)[1] v.z = 0 self.obj.alignAxisToVect(v, 0, .1) def delta_to_vect(self): v = self.obj.getVectTo(self.lane_point)[1] #vt = Vector([self.last_lane_point.x - self.lane_point.x, self.last_lane_point.y - self.lane_point.y, 0]) delta = self.last_lane_point - self.lane_point delta = delta.cross(v) delta_mult = -.1 mult = 1.0 deltamove = delta[2] * delta_mult #self.obj.applyMovement([0, deltamove, 0], True) f = deltamove * 5000 self.obj.applyForce([0, f, 0], True) def apply_gas(self): if self.obj.linearVelocity.x < self.speed_targ: self.obj.applyForce([self.speed_inc, 0, 0], True) #==================================== State = type("State", (object,), {}) #==================================== class State(object): def __init__(self, FSM): self.FSM = FSM self.timer = 0 self.startTime = 0 def Enter(self): self.timer = 0 self.startTime = 0 def Execute(self): print('Executing') def Exit(self): print('Exiting') #==================================== class Example(State): def __init__(self,FSM): super(Example, self).__init__(FSM) def Enter(self): self.FSM.stateLife = 1 self.FSM.owner.resumePhysics() self.FSM.owner.resumeDynamics() print('physics resumed') super(Example, self).Enter() def Execute(self): self.FSM.stateLife += 1 print('doing example') #o = self.FSM.owner #g = o.me['game'] #self.FSM.ToTransition('toActivate') #print(self.FSM.owner) #self.FSM.owner.applyRotation([0,0,.01], True) #self.FSM.owner.worldPosition.z = 0 #self.FSM.owner.worldPosition.z += 0.1 #self.FSM.owner.worldPosition.x += 1.1 #print(self.FSM.owner.children) def Exit(self): pass class ExitParallelPark(State): def __init__(self,FSM): super(ExitParallelPark, self).__init__(FSM) def Enter(self): self.FSM.stateLife = 1 self.FSM.owner.obj.restorePhysics() self.FSM.owner.obj.restoreDynamics() self.FSM.owner.obj.linearVelocity = [0,0,0] self.FSM.owner.target, self.FSM.owner.path = find_new_parking(self.FSM.owner) self.FSM.owner.path_index = 0 self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index] self.FSM.owner.target.status = 'targetted' self.FSM.owner.start_empty.status = 'available' #print('target is', self.FSM.owner.target) print('physics resumed') super(ExitParallelPark, self).Enter() def Execute(self): self.FSM.stateLife += 1 #print('doing ExitParallelPark') v = self.FSM.owner.obj.getVectTo(self.FSM.owner.path[0]) #v.z = 0 self.FSM.owner.obj.alignAxisToVect(v[1], 0, .01) self.FSM.owner.obj.alignAxisToVect([0,0,1], 2, 1) if self.FSM.stateLife > 220: self.FSM.ToTransition('toNavigateToTarget') #self.FSM.owner.obj.applyForce([6, 0, 0], True) def Exit(self): pass #==================================== class EnterParallelPark(State): def __init__(self,FSM): super(EnterParallelPark, self).__init__(FSM) def Enter(self): self.FSM.stateLife = 1 print('entering parallel park') self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation self.FSM.owner.obj.applyMovement([0, -6, 0], True) self.FSM.owner.target.status = 'in_use' self.FSM.owner.obj.worldPosition.z += .9 self.FSM.owner.active = False self.FSM.owner.start_empty = self.FSM.owner.target self.FSM.owner.last_point = self.FSM.owner.target.obj.worldPosition.copy() self.FSM.owner.last__lane_point = self.FSM.owner.obj.worldPosition self.FSM.owner.point = self.FSM.owner.target.obj.worldPosition.copy() clear_markers(self) self.FSM.owner.obj.suspendDynamics() self.FSM.owner.obj.suspendPhysics() super(EnterParallelPark, self).Enter() def Execute(self): self.FSM.stateLife += 1 if self.FSM.stateLife == 2: self.FSM.owner.manager.cars_active.remove(self.FSM.owner) #self.FSM.ToTransition('toActivate') def Exit(self): pass #==================================== class NavigateToTarget(State): def __init__(self,FSM): super(NavigateToTarget, self).__init__(FSM) def Enter(self): self.FSM.stateLife = 1 super(NavigateToTarget, self).Enter() def Execute(self): self.FSM.stateLife += 1 update_point(self.FSM.owner) align_to_point(self.FSM.owner) align_to_road(self.FSM.owner) set_height(self.FSM.owner) delta_to_vect(self.FSM.owner) apply_gas(self.FSM.owner) #print('target', self.FSM.owner.target.obj.worldPosition) #emergency exit if self.FSM.stateLife > 30 * 60: self.FSM.ToTransition('toEnterParallelPark') #print('doing NavigateToTarget') #self.FSM.owner.obj.applyForce([6, 0, 0], True) def Exit(self): pass #==================================== class Activate(State): def __init__(self,FSM): super(Activate, self).__init__(FSM) def Enter(self): self.FSM.stateLife = 1 # self.FSM.owner.restorePhysics() # self.FSM.owner.restoreDynamics() # self.FSM.owner.worldPosition.z = 2 # self.FSM.curTarget = None # self.FSM.last_target = None # #self.FSM.path = None # self.point = None # self.last_point = self.FSM.owner.worldPosition.copy() # self.last_pos = self.FSM.owner.worldPosition.copy() # self.backup = 0 # self.lane_point = None # self.last_lane_point = self.last_pos # self.lane_position = 1.75 # self.pos_his = [] # self.max_speed = 5.0 # self.FSM.path = None # print('physics resumed') super(Activate, self).Enter() def find_target(self): pass # if self.FSM.curTarget == None: # choices = self.FSM.owner['manager'].targets # if self.FSM.last_target in choices: # choices.remove(self.FSM.last_target) # if choices: # self.FSM.curTarget = random.choice(choices) # self.FSM.path = self.FSM.owner['manager'].navmesh.findPath(self.FSM.owner.worldPosition, self.FSM.curTarget.worldPosition) # self.FSM.path.remove(self.FSM.path[0]) # self.point = self.FSM.path[0] # vt = Vector([self.last_point.x - self.point.x, self.last_point.y - self.point.y, 0]) # tv = vt.normalized() # #rotate 90 degrees # nv = Vector([-tv.y, tv.x, 0]) # distance = 3.5 # self.lane_point = self.point + self.lane_position * nv # else: # print('no fucking choices') # return False # else: # return True def drive_to_point(self): if self.FSM.path: ground_ray = get_ground_ray(self) if ground_ray[0]: set_height(self, ground_ray) align_to_road(self, ground_ray) v = self.FSM.owner.getVectTo(self.lane_point) speed_force = 800.0 max_speed = 5.0 behind = False local = self.FSM.owner.worldOrientation.inverted() * (self.lane_point - self.FSM.owner.worldPosition) v2 = v[1].copy() v2.z = 0 delta = self.last_lane_point - self.lane_point delta = delta.cross(v[1]) delta_mult = -.1 mult = 1.0 backup_time = 20 #change max speed if self.FSM.stateLife % 180 == 0: print('change speed force') self.speed_force = random.choice([max_speed * 1.3, max_speed * 1.6, max_speed * .8, max_speed * .6, max_speed]) if local.x > 0 and self.backup == 0: if self.FSM.owner.linearVelocity.x < self.max_speed: self.FSM.owner.applyForce([speed_force, 0, 0], True) deltamove = delta[2] * delta_mult #self.FSM.owner.applyMovement([0, deltamove, 0], True) f = deltamove * 5000 self.FSM.owner.applyForce([0, f, 0], True) v = self.FSM.owner.getVectTo(self.lane_point) v2 = v[1].copy() v2.z = 0 self.FSM.owner.alignAxisToVect(v2, 0, .05) else: if local.x < 0: self.backup = backup_time if self.backup > 0: print('backing up') v = self.FSM.owner.getVectTo(self.FSM.path[0]) v2 = v[1].copy() v2.z = 0 self.FSM.owner.alignAxisToVect(v2, 0, .02) if self.FSM.owner.linearVelocity.x > -max_speed / 2: self.FSM.owner.applyForce([-speed_force * .8, 0, 0], True) self.backup -= 1 dist = self.FSM.owner.getDistanceTo(self.lane_point) if dist < 2.5 and (len(self.FSM.path) > 0): #print(self.FSM.path,'this is the path') #print('navmesh point removed') self.last_point = self.point self.last_lane_point = self.lane_point self.FSM.path.remove(self.FSM.path[0]) if len(self.FSM.path) > 0: self.point = self.FSM.path[0] v = Vector([self.last_point.x - self.point.x, self.last_point.y - self.point.y, 0]) tv = v.normalized() nv = Vector([-tv.y, tv.x, 0]) #rotate 90 degrees self.lane_point = self.point + self.lane_position * nv else: self.point = None if self.FSM.path == []: self.FSM.curTarget = None #progress self.pos_his.append(self.FSM.owner.worldPosition.copy()) pos_his_len = len(self.pos_his) if pos_his_len > 200: #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]) sum_ = abs(self.FSM.owner.worldPosition.x - self.pos_his[0][0]) + abs(self.FSM.owner.worldPosition.y - self.pos_his[0][1]) #print(sum_, 'sum') if sum_ < .05: self.backup = backup_time print('progress stopped') del self.pos_his[0] def Execute(self): self.FSM.stateLife += 1 #if self.find_target(): #self.drive_to_point() #self.FSM.owner.linearVelocity.y = 0 #self.last_pos = self.FSM.owner.worldPosition.copy() #self.FSM.owner['manager'].target_loc.worldPosition = self.lane_point def Exit(self): pass #====================================