123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505 |
- import utils
- import bge
- import random
- from mathutils import Vector
- import mathutils
- #import math
-
- #====================================
-
- 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 single_marker(pos, col):
- pm = bge.logic.getCurrentScene().addObject('path_marker', None, 0)
- pm.worldPosition = pos
- pm.color = col
-
- def clear_markers(self):
- 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 = 45
- dist = self.obj.getDistanceTo(x.obj)
- if dist < min_dist:
- potentials.remove(x)
- if len(potentials) > 0:
- new_parking = random.choice(potentials)
- path2 = self.manager.navmesh2.queue_path(self.start_empty.obj.worldPosition, new_parking.obj.worldPosition, self)
- #print('astar request')
- #if path2 is not None:
-
- return new_parking, path2
- else:
- get_parking_type(self)
- #self.FSM.FSM.ToTransition('toEnterParallelPark')
-
- def get_lane_point(self):
- self.point = self.path[self.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
- else:
- pass
-
- def update_point(self):
- if self.path_index >= (len(self.path) ):
- get_parking_type(self)
- #self.FSM.FSM.ToTransition('toEnterParallelPark')
- else:
- dist = self.obj.getDistanceTo(self.lane_point)
- 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, mult):
- v = self.obj.getVectTo(self.lane_point)[1]
- v.z = 0
- self.obj.alignAxisToVect(v, 0, .05 * mult)
-
- def align_to_target(self, mult):
- v = self.obj.getVectTo(self.FSM.FSM.curState.park_loc)[1]
- v.z = 0
- self.obj.alignAxisToVect(v, 0, .05 * mult)
-
-
- def delta_to_vect(self):
- v = self.obj.getVectTo(self.lane_point)[1]
- delta = self.last_lane_point - self.lane_point
- delta = delta.cross(v)
- delta_mult = -.1
- mult = 1.0
- deltamove = delta[2] * delta_mult
-
- #check if in front
- local = self.obj.worldOrientation.inverted() * (self.lane_point - self.obj.worldPosition)
- f = deltamove * 500
- if local < 0:
- f *= -1
- self.obj.applyForce([0, f, 0], True)
-
-
- def apply_gas(self, mult):
- if self.obj.linearVelocity.x < self.speed_targ * mult:
- self.obj.applyForce([self.speed_inc * mult, 0, 0], True)
-
- def starting_mod(self):
- path2 = self.path
- #path2.append(self.target.obj.worldPosition) #add endpoint
- #path2.append(self.target.obj.worldPosition)
-
- def check_front(self): #
- obj_vec = self.obj.getAxisVect([1,0,0])
- targ_vec = self.obj.getVectTo(self.path[self.path_index] +1)[1].normalized()
- dot_ = obj_vec.dot(targ_vec)
-
- if dot_ > 0:
- print('in front', self)
- else:
- print('behind', self)
-
- if dot_ > 0:
- return True
- else:
- return False
-
- def get_parking_type(self):
- if self.target.type == 'perp':
- self.FSM.FSM.ToTransition('toEnterPerpPark')
- else:
- self.FSM.FSM.ToTransition('toEnterParallelPark')
-
- def get_parking_point(self):
- 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
-
- local = Vector([7,0,0])
- return Vector(self.target.obj.worldPosition + (self.target.obj.worldOrientation * local))
-
-
- #return Vector(self.target.obj.worldPosition) - 7 * tv
-
- def move_to_point(self):
- #print(self.v_to_target[0])
- if self.v_to_target[0] < .95:
- #print('reached')
- return True
- else:
- vectTo = Vector(self.v_to_target[1])
- vectTo.magnitude = 4.01
- self.FSM.owner.obj.linearVelocity = vectTo
- return False
-
- def forward_ray(self):
-
- Axis = 0
- Distance = 30
- End = self.obj.worldPosition + (self.obj.worldOrientation.col[Axis]*Distance)
- Start = self.obj.worldPosition.copy()
- return self.obj.rayCast(End, Start, 10,'traffic', 1, 0, 0)
-
- #dist = v1[0]
- #print(dist)
-
- #====================================
-
- 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')
-
- 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.path_index = 1
-
- try:
- self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index]
- get_lane_point(self.FSM.owner)
- except:
- print('no path')
- self.FSM.owner.target.status = 'targetted'
- self.FSM.owner.start_empty.status = 'available'
- starting_mod(self.FSM.owner)
- super(ExitParallelPark, self).Enter()
-
- def Execute(self):
- self.FSM.stateLife += 1
- set_height(self.FSM.owner)
- if self.FSM.owner.path != []:
- update_point(self.FSM.owner)
- #v = self.FSM.owner.obj.getVectTo(self.FSM.owner.path[0])
- #v = self.FSM.owner.obj.getVectTo(self.FSM.owner.lane_point)
- #print(v[0])
- #self.FSM.owner.obj.alignAxisToVect(v[1], 0, .02)
- #self.FSM.owner.obj.alignAxisToVect([0,0,1], 2, 1)
- #if self.FSM.stateLife > 160 or self.FSM.owner.obj.getDistanceTo(self.FSM.owner.lane_point) < 2:
- #if self.FSM.owner.obj.getDistanceTo(self.FSM.owner.path[0]) < 3.5:# or (self.FSM.stateLife > 30 and check_front(self.FSM.owner)):
- if self.FSM.owner.path_index > 1:
- #print('exiting park')
- #self.FSM.owner.path_index = 2
- self.FSM.ToTransition('toNavigateToTarget')
-
-
- align_to_point(self.FSM.owner, .65)
- align_to_road(self.FSM.owner)
- #set_height(self.FSM.owner)
- delta_to_vect(self.FSM.owner)
- apply_gas(self.FSM.owner, .5)
-
- #if self.FSM.stateLife < 30:
- #self.FSM.owner.obj.applyRotation([0,0,.0075], True)
-
- def Exit(self):
- pass
-
- #====================================
-
-
- class ExitPerpPark(State):
- def __init__(self,FSM):
- super(ExitPerpPark, 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.path_index = 1
-
- try:
- self.FSM.owner.point = self.FSM.owner.path[self.FSM.owner.path_index]
- get_lane_point(self.FSM.owner)
- except:
- print('no path')
- self.FSM.owner.target.status = 'targetted'
- self.FSM.owner.start_empty.status = 'available'
- starting_mod(self.FSM.owner)
- super(ExitPerpPark, self).Enter()
-
- def Execute(self):
- #print('exiting perp park', self.FSM.owner.start_empty.type)
- self.FSM.stateLife += 1
- set_height(self.FSM.owner)
- if self.FSM.owner.path != []:
- update_point(self.FSM.owner)
- if self.FSM.owner.path_index > 1 or self.FSM.stateLife > 300:
- self.FSM.ToTransition('toNavigateToTarget')
-
-
- #align_to_point(self.FSM.owner, .65)
- align_to_road(self.FSM.owner)
- #set_height(self.FSM.owner)
- #delta_to_vect(self.FSM.owner)
- #apply_gas(self.FSM.owner, .5)
-
- if self.FSM.stateLife < 120:
- self.FSM.owner.obj.applyForce([-50,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
-
- clear_markers(self)
- super(EnterParallelPark, self).Enter()
-
- def cleanup(self):
- self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition.copy()
- self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation.copy()
- 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.lane_point = self.FSM.owner.target.obj.worldPosition.copy()
- self.FSM.owner.last_lane_point = self.FSM.owner.target.obj.worldPosition.copy()
- self.FSM.owner.point = self.FSM.owner.target.obj.worldPosition.copy()
- self.FSM.owner.path_index = 0
- self.FSM.owner.path = None
-
- self.FSM.owner.manager.cars_active.remove(self.FSM.owner)
- self.FSM.owner.obj.suspendDynamics()
-
- def Execute(self):
- self.FSM.stateLife += 1
- if self.FSM.stateLife == 240:
- self.cleanup()
- #self.FSM.owner.obj.suspendPhysics()
- def Exit(self):
- pass
-
- class EnterPerpPark(State):
- def __init__(self,FSM):
- super(EnterPerpPark, self).__init__(FSM)
-
- def Enter(self):
- self.FSM.stateLife = 1
- self.park_loc = get_parking_point(self.FSM.owner)
- single_marker(self.park_loc, [1,0,0,.3])
-
- clear_markers(self)
- super(EnterPerpPark, self).Enter()
-
- def cleanup(self):
- self.FSM.owner.obj.worldPosition = self.FSM.owner.target.obj.worldPosition.copy()
- self.FSM.owner.obj.worldOrientation = self.FSM.owner.target.obj.worldOrientation.copy()
- self.FSM.owner.obj.applyMovement([7, 0, 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.lane_point = self.FSM.owner.target.obj.worldPosition.copy()
- self.FSM.owner.last_lane_point = self.FSM.owner.target.obj.worldPosition.copy()
- self.FSM.owner.point = self.FSM.owner.target.obj.worldPosition.copy()
- self.FSM.owner.path_index = 0
- self.FSM.owner.path = None
-
- self.FSM.owner.manager.cars_active.remove(self.FSM.owner)
- self.FSM.owner.obj.suspendDynamics()
-
- def Execute(self):
- self.FSM.stateLife += 1
-
- self.v_to_target = self.FSM.owner.obj.getVectTo(self.park_loc)
-
- goal_met = move_to_point(self)
- set_height(self.FSM.owner)
- if self.v_to_target[0] > 2:
-
- align_to_target(self.FSM.owner, .65)
-
- else:
- self.FSM.owner.obj.alignAxisToVect(-self.FSM.owner.target.obj.worldOrientation[0], 0, .05)
-
- if self.FSM.stateLife == 440 or goal_met:
- self.cleanup()
- #self.FSM.owner.obj.suspendPhysics()
-
- 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
- self.forward_ray = forward_ray(self.FSM.owner)
- update_point(self.FSM.owner)
-
- align_to_point(self.FSM.owner, 1)
- align_to_road(self.FSM.owner)
- set_height(self.FSM.owner)
- delta_to_vect(self.FSM.owner)
- gas = True
- if self.forward_ray[0]:
-
- dist = self.FSM.owner.obj.getDistanceTo(self.forward_ray[1])
- if dist < 10:
- gas = False
- self.FSM.owner.obj.linearVelocity.x *= .98
- print(dist, 'forward ray', self.forward_ray[0])
-
- if gas:
- apply_gas(self.FSM.owner, 1)
-
- #emergency exit
- if self.FSM.stateLife > 30 * 90:
- get_parking_type(self.FSM.owner)
- #self.FSM.ToTransition('toEnterParallelPark')
-
- def Exit(self):
- pass
-
- #====================================
-
- class Activate(State):
- def __init__(self,FSM):
- super(Activate, self).__init__(FSM)
-
- def Enter(self):
- self.FSM.stateLife = 1
- super(Activate, self).Enter()
-
- def find_target(self):
- pass
-
- def drive_to_point(self):
- pass
-
- def Execute(self):
- self.FSM.stateLife += 1
-
- def Exit(self):
- pass
-
- #====================================
-
- class RequestPath(State):
- def __init__(self,FSM):
- super(RequestPath, self).__init__(FSM)
-
- def Enter(self):
- self.FSM.stateLife = 1
- self.FSM.owner.target, self.FSM.owner.path = find_new_parking(self.FSM.owner)
- self.FSM.owner.path_index = 0
- super(RequestPath, self).Enter()
-
- def Execute(self):
- self.FSM.stateLife += 1
-
- def Exit(self):
- pass
|