import networkx as nx
from triangle_utils import triangle_center
from triangle_relation import printRels
from scenario_reader import loadScenario
#scenario_file = './scenarios/s3.json'
file_name = 's2'
scenario_width, scenario_height, immobile_objs, mobile_objs, manipulatable_obj, target_obj = loadScenario('./scenarios/' + file_name + '.json')
scenario = SG(scenario_width, scenario_height, immobile_objs, mobile_objs, manipulatable_obj, target_obj, showRender=True)
## get objects
game_objects = scenario.getGameObjects()
tri = []
graph, edges_index, edges_dirs, edges_by_tri, vertices_of_edges, edges_surface_dic, tri_by_surface, surface_rel_dic, tri_neighbor_by_edge, edges_by_object_id, objects_id_by_edge, zones = triangulate_advanced(game_objects, scenario_width, scenario_height, tri)
plot.plot(plt.axes(),**(tri[0]))
################### Arrange graph according to the position of triangles #########
#triangles_by_vertices = tri["vertices"][tri["triangles"]]
pos = {}
for index, triangle in enumerate(zones):
pos[index] = (triangle.centroid.x, triangle.centroid.y)
#triangle_center(*triangle)
edge_text_labels = {}
edge_lists = []
def __init__(self, scenario_file):
# create scenario
self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
game_objects = scenario.getGameObjects()
self.b2_objects = scenario.b2_objects
## draw triangulation
# tri = [] may cause segmentation fault
tri = None
###
self.graph, self.edges_index, self.edges_dir, self.edges_by_tri, self.vertices_of_edges, self.is_edge_surface, self.tri_by_surface, self.surface_rel_dic, self.tri_neighbor_by_edge, self.edges_by_object_id, self.objects_id_by_edge, self.edges_length, self.zones = triangulate_advanced(game_objects, self.width, self.height, tri)
#triangulate_advanced(game_objects, self.width, self.height, tri)
#triangulate_advanced(game_objects, self.width, self.height, tri)
#dt(self.graph, tri, self.zones)
self.initial_zone = -1
initial_obj = scenario.b2_objects[self.manipulatable_obj['id']]
self.object_size = 16
initial_position = Point(initial_obj.position)
for i in xrange(len(self.zones)):
if self.zones[i].contains(initial_position):
self.initial_zone = i
break
self.target_zones = []
target_edges = self.edges_by_object_id[self.target_obj['id']]
for edge in target_edges:
#print "edge: ", edge
if edge in self.edges_index:
self.target_zones.append(self.tri_by_surface[edge])
self.initial_edge = None
min_d = 99999
for edge in self.edges_by_tri[self.initial_zone]:
flag, distance = self.is_on_surface(initial_obj, edge)
if flag:
self.initial_edge = edge
break
else:
if min_d > distance:
min_d = distance
self.initial_edge = edge
#self.target_zone = 8
self.initial_obj_id = self.manipulatable_obj['id']
self.target_obj_id = self.target_obj['id']
initial_motion = "FLYING"
self.initial_directions = []
self.initial_states = []
#path = simple_paths.next()
self.initial_position = self.b2_objects[self.manipulatable_obj['id']].position
for target_zone in self.target_zones:
simple_paths = nx.all_simple_paths(self.graph, source=self.initial_zone, target=target_zone)
for path in simple_paths:
initial_target_edge = self.graph.get_edge_data(path[0], path[1])['rel']
#initial_direction = self.edges_dir[(self.edges_index[initial_edge], self.edges_index[initial_target_edge])]
########################## DEBUG added #######################
initial_direction = self.compute_initial_direction_exact_position(self.initial_position, self.vertices_of_edges[initial_target_edge])
if initial_direction[0] < 0:
initial_direction = (0, initial_direction[1])
############################ ###################################################
if initial_direction not in self.initial_directions:
self.initial_directions.append(initial_direction)
### state = (current zone id, current edge index set, direction:[(),()], motion_type, collision happened, direction before updated, current obj id)
initial_state = (path[0], self.initial_edge, initial_direction, initial_motion, False , None, self.initial_obj_id)
self.initial_states.append(initial_state)
### check if can slide:
if self.is_edge_surface[self.initial_edge] == 1:
connected_surfaces = self.surface_rel_dic[self.initial_edge]
for surface_info in connected_surfaces:
connected_surface, on_surface_direction, exiting_direction, intersection_angle = surface_info
if on_surface_direction[0] <= initial_direction[1] and on_surface_direction[0] >=initial_direction[0]:
initial_state = (path[0], self.initial_edge, on_surface_direction, SLIDING, False, None, self.initial_obj_id)
#print "add initial state ", initial_state
#self.initial_states.append(initial_state)
'''
if self.is_edge_surface[self.initial_edge] == 1:
connected_surfaces = self.surface_rel_dic[self.initial_edge]
for surface_info in connected_surfaces:
connected_surface, on_surface_direction, exiting_direction, intersection_angle = surface_info
if self.tri_by_surface[connected_surface] == path[1]:
initial_state = (path[0], self.initial_edge, on_surface_direction, SLIDING, False, None, self.initial_obj_id )
# add event
self.initial_states.append(initial_state)
'''
##### Can remove later
#print self.edges_by_object_id[self.target_obj_id]
self.root_paths = {}
self.all_paths = []
def __init__(self, scenario_file):
# create scenario
self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj = loadScenario(scenario_file)
scenario = Scenario_Generator(self.width, self.height, self.immobile_objs, self.mobile_objs, self.manipulatable_obj, self.target_obj, showRender=False)
game_objects = scenario.getGameObjects()
self.graph, self.edges_index, self.edges_dir, self.edges_by_tri, self.vertices_of_edges, self.is_edge_surface, self.tri_by_surface, self.surface_rel_dic, self.tri_neighbor_by_edge, self.edges_by_object_id, self.objects_id_by_edge, self.zones = triangulate_advanced(game_objects, self.width, self.height)
self.initial_zone = -1
initial_obj = scenario.b2_objects[self.manipulatable_obj['id']]
initial_position = Point(initial_obj.position)
for i in xrange(len(self.zones)):
if self.zones[i].contains(initial_position):
self.initial_zone = i
break
self.target_zones = []
target_edges = self.edges_by_object_id[self.target_obj['id']]
for edge in target_edges:
#print "edge: ", edge
if edge in self.edges_index:
self.target_zones.append(self.tri_by_surface[edge])
initial_edge = None
for edge in self.edges_by_tri[self.initial_zone]:
if self.is_on_surface(initial_obj, edge):
initial_edge = edge
break
#self.target_zone = 8
self.initial_obj_id = self.manipulatable_obj['id']
self.target_obj_id = self.target_obj['id']
initial_motion = "FLYING"
self.initial_directions = []
self.initial_states = []
#path = simple_paths.next()
for target_zone in self.target_zones:
simple_paths = nx.all_simple_paths(self.graph, source=self.initial_zone, target=target_zone)
for path in simple_paths:
initial_target_edge = self.graph.get_edge_data(path[0], path[1])['rel']
initial_direction = self.edges_dir[(self.edges_index[initial_edge], self.edges_index[initial_target_edge])]
if initial_direction not in self.initial_directions:
self.initial_directions.append(initial_direction)
initial_state = (path[0], initial_edge, initial_direction, initial_motion, None, self.initial_obj_id)
self.initial_states.append(initial_state)
#print target_edge
self.root_paths = {}
self.all_paths = []
