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 = []