def find_qualitative_path_extended(self, velocity, zones, man_obj_id):
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
scenario.current_man_id = man_obj_id
scenario.apply_velocity_and_run(*velocity)
traj = scenario.find_man_traj()
b2contacts = scenario.find_contacts_with_mobile_objs()
initial_zone = -1
pre_zone = initial_zone
path = [initial_zone]
for traj_pt in traj:
occupied_zone = -1
man_position = Point(traj_pt)
for i in xrange(len(zones)):
if zones[i].contains(man_position):
occupied_zone = i
break
# if out of scope, still wait to see if it will come back, quite slow
if occupied_zone == -1 or occupied_zone == pre_zone:
continue
path.append(occupied_zone)
pre_zone = occupied_zone
return path, b2contacts
def find_qualitative_path_ptlike(self, action, zones, initial_zone):
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
scenario.apply_impulse_and_run(action)
traj = scenario.find_man_traj()
b2contacts = scenario.find_contacts_with_mobile_objs()
#print contacts
pre_zone = initial_zone
path = [initial_zone]
for traj_pt in traj:
occupied_zone = -1
man_position = Point(traj_pt)
for i in xrange(len(zones)):
if zones[i].contains(man_position):
occupied_zone = i
break
# if out of scope, still wait to see if it will come back, quite slow
if occupied_zone == -1 or occupied_zone == pre_zone:
continue
'''
if occupied_zone == -1:
# reach end zone
path.append(-1)
break
elif occupied_zone == pre_zone:
continue
'''
path.append(occupied_zone)
pre_zone = occupied_zone
return path, b2contacts
def find_qualitative_path_rich(self, action, zones, initial_zone):
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
scenario.apply_impulse_and_run(action)
traj = scenario.find_man_traj()
num_zones = len(zones)
initial_time, x,y, touching = traj[0]
# ball must touch the surface
last_touching_time = initial_time # beginning of the last touching
last_state_time = initial_time
last_touching_state = True # False: not touched, True, touched
pre_zone = initial_zone
last_motion_type = 0
quali_path = ''
for traj_pt in traj:
time, x, y, touching = traj_pt
man_position = Point(x,y)
for i in xrange(num_zones):
if zones[i].contains(man_position):
occupied_zone = i
break
if occupied_zone == pre_zone:
if last_touching_state == touching:
continue
else:
# previously touch, currently not
if last_touching_state:
time_duration = time - last_touching_time
if time_duration > 30: #SLIDING
motion_type = 4
else:
motion_type = 8 # BOUCING
else:
motion_type = 2 # FLYING
last_touching_time = time
last_motion_type = last_motion_type | motion_type
last_motion_type = motion_type
last_touching_state = touching
else:
if time - last_state_time < 10:
continue
if last_motion_type == 0:
if last_touching_state:
time_duration = time - last_touching_time
if time_duration > 30:
last_motion_type = 4
else:
last_motion_type = 8
else:
last_motion_type = 2
quali_path = quali_path + str(pre_zone) + "*" +str(last_motion_type) + ","
last_touching_state = touching
pre_zone = occupied_zone
last_state_time = time
last_motion_type = 0
if touching:
last_touching_time = time
return quali_path
