def get_num_voting_booths(geography_list): voting_booths = [] count = 0 # create polygon for geography in geography_list: if type(geography.geometry) is MultiPolygon: exteriors = [Polygon(polygon.exterior) for polygon in geography.geometry] exterior_polygon = MultiPolygon(exteriors) else: exterior_polygon = Polygon(geography.geometry.exterior) # List of each of the columns in the public school csv file school_col_list = ["X", "Y", "OBJECTID", "NCESSCH", "NAME", "OPSTFIPS", "LSTREE", "LCITY", "LSTATE", "LZIP", "LZIP4", "STFIP15", "CNTY15", "NMCNTY15", "LOCALE15", "LAT1516", "LON1516", "CBSA15", "NMCBSA15", "CBSATYPE15", "CSA15", "NMCSA15", "NECTA15", "NMNECTA15", "CD15", "SLDL15", "SLDU15"] # read each line in the csv file # each column of the col_list will be filled with corresponding data school_file = pd.read_csv("../Polling Locations CSV files/Public_School_Locations_201516.csv", usecols=school_col_list) # List of each of the columns in the public library csv file lib_col_list = ["Location Number", "Location Name", "Location Type", "Address", "City", "State", "Zip Code", "Phone Number", "County", "Latitude", "Longitude", "Accuracy"] # read each line in the csv file # each column of the col_list will be filled with corresponding data lib_file = pd.read_csv("../Polling Locations CSV files/public_libraries.csv", usecols=lib_col_list) # initialize list voting_booths.append(0) # Create shapely point based on public school coordinates and check to see if point is contained in polygon # increment the number of voting booths by the number of points that lie in the polygon for i in range(len(school_file)): point = Point(((school_file["X"]).iloc[i]), ((school_file["Y"]).iloc[i])) if exterior_polygon.contains(point): voting_booths[count] = voting_booths[count] + 1 for i in range(len(lib_file)): point = Point(((lib_file["Longitude"]).iloc[i]), ((lib_file["Latitude"]).iloc[i])) if exterior_polygon.contains(point): voting_booths[count] = voting_booths[count] + 1 count = count + 1 return voting_booths
def postJuntarPuntos(caminos, centros, separateTol=25): canchList = [] for cam in caminos: canchList.append(cam) MPcanch = MultiPolygon(canchList) trueCentros = [] while centros: auxX = [centros[0].x] auxY = [centros[0].y] minX = centros[0] maxX = centros[0] minY = centros[0] maxY = centros[0] centros.pop(0) selectedPoints = [] for j in range(len(centros)): if minX.distance(centros[j]) < separateTol or maxX.distance( centros[j]) < separateTol or minY.distance( centros[j]) < separateTol or minY.distance( centros[j]) < separateTol: lin1 = LineString([centros[j], minX]) lin2 = LineString([centros[j], maxX]) lin3 = LineString([centros[j], minY]) lin4 = LineString([centros[j], maxY]) if not MPcanch.contains(lin1) or not MPcanch.contains( lin2) or not MPcanch.contains( lin3) or not MPcanch.contains(lin4): break if minX.x > centros[j].x: minX = centros[j] if maxX.x < centros[j].x: maxX = centros[j] if minY.y > centros[j].y: minY = centros[j] if maxY.y < centros[j].y: maxY = centros[j] auxX.append(centros[j].x) auxY.append(centros[j].y) selectedPoints.append(j) meanX = stats.mean(list(auxX)) meanY = stats.mean(list(auxY)) trueCentros.append(Point(meanX, meanY)) popCount = 0 for j in selectedPoints: centros.pop(j - popCount) popCount += 1 return trueCentros
def group_contains_NE(NE, group_loc_names, GN_data, OSM_data, PR_shp_lookup, verbose=False): """ Check if group municipalities contain the NE mentioned. :param NE: NE to check :param group_loc_names: group municipality names :param GN_data: GeoNames data :param OSM_data: OSM data :param PR_shp_lookup: shape files for all municipalities :returns contains_NE:: if group contains NE """ # get combined shape for group locations group_shp = MultiPolygon([PR_shp_lookup[x] for x in group_loc_names]) # disambiguate NE to valid location using GN/OSM data NE_coords = [[-300, -300]] # by default, shape cannot contain coordinate if (NE in GN_data.keys()): NE_coords = GN_data[NE] elif (NE in OSM_data.keys()): NE_coords = OSM_data[NE] # containment: if any of the coordinates are contained in group if (verbose): logging.debug('NE=%s, group shape %s' % (NE, group_shp)) logging.debug('NE=%s, NE_coords=%s' % (NE, str(NE_coords))) contains_NE = any( [group_shp.contains(Point(NE_coord)) for NE_coord in NE_coords]) return contains_NE
def glyph_to_mp(g: fontforge.glyph) -> MultiPolygon: """ Converts fontforge glyphs to shapely polygons. Parameters ---------- g: fontforge glyph Glyph to be converted Returns ------- mp: shapely MultiPolygon MultyPolygon representing the whole glyph """ fg = g.layers[1] polys = MultiPolygon([Polygon([(p.x, p.y) for p in c]) for c in fg]) mp = MultiPolygon([polys[0]]) if len(polys) > 1: for h in polys[1:]: if mp.contains(h): mp = mp.difference(h) else: mp = MultiPolygon([*mp, h]) if type(mp) == Polygon: mp = MultiPolygon([mp]) return (mp)
def sampleRandomLatLong(wardIndex): if presampledflag: i = random.randint(0, len(presampledpoints[wardIndex]) - 1) (lat, lon) = presampledpoints[wardIndex].loc[i] return (round(lat, 4), round(lon, 4)) else: (lon1, lat1, lon2, lat2) = geoDF['wardBounds'][wardIndex] wardPoly = MultiPolygon(geoDF['geometry'][wardIndex]) while True: lat = round(random.uniform(lat1, lat2), 4) lon = round(random.uniform(lon1, lon2), 4) point = Point( lon, lat) #IMPORTANT: Point takes in order of longitude, latitude if wardPoly.contains(point): return (lat, lon)
class OSM_Boundary(object): def __init__(self, relation_element, way_elements, node_elements): self.polygons = [] self.ways = {} self.relation = relation_element self.open = True reporting = Reporting() self.id = relation_element.attrib["id"] self.version = relation_element.attrib["version"] self.changeset = relation_element.attrib["changeset"] self.tags = {} nodes = {} for element in node_elements: node = OSM_Node(element) nodes[node.id] = node ways = {} for element in way_elements: way = OSM_Way(element) way.add_nodes(nodes) ways[way.id] = way for sub in relation_element: if sub.tag == 'tag': key = sub.attrib["k"] value = sub.attrib["v"] self.tags[key] = value elif sub.tag == 'member' and sub.attrib["type"] == 'way': role = sub.attrib.get("role", "") if role: raise NotImplementedError, "Role %r for relation way members not supported" % (role,) way_id = sub.attrib["ref"] way = ways.get(way_id) if way: if not way.complete: raise ValueError, "Incomplete way: %r" % (way,) self.ways[way_id] = way else: raise ValueError, "Way not found: %r" % (way_id,) self.name = self.tags.get("name") if not self.ways: raise ValueError, "No ways" self.open = False ways_left = self.ways.values() while ways_left: segment_start = ways_left.pop(0) polygon = [] for node in segment_start.nodes: polygon.append((node.lat, node.lon)) last_end = segment_start.end_node while ways_left: if last_end is segment_start.start_node: # cycle ended break next = None for way in ways_left: if way.start_node is last_end: last_end = way.end_node for node in way.nodes[1:]: polygon.append((node.lat, node.lon)) next = way break elif way.end_node is last_end: last_end = way.start_node rnodes = list(way.nodes[1:]) rnodes.reverse() for node in rnodes: polygon.append((node.lat, node.lon)) next = way break if next: ways_left.remove(next) else: # open segment ends self.open = True break self.polygons.append(polygon) if HAVE_SHAPELY: reporting.output_msg("info", "Using Shapely for 'point in polygon' checks") self.multi_polygon = MultiPolygon([(p, ()) for p in self.polygons]) self. _contains_impl = self._contains_shapely_impl else: reporting.output_msg("info", "Using Python function for the 'point in polygon' checks") self. _contains_impl = self._contains_python_impl def __repr__(self): if self.open: open_s = "open" else: open_s = "closed" return "<OSM_Boundary #%s %r %s %i ways %i polygons>" % (self.id, self.name, open_s, len(self.ways), len(self.polygons)) def _contains_python_impl(self, location): lat = location.lat lon = location.lon for pol in self.polygons: contains = False plen = len(pol) i = 0 j = plen - 1 while i < plen: if ( ((pol[i][0] > lat) != (pol[j][0] > lat)) and (lon < ((pol[j][1] - pol[i][1]) * (lat - pol[i][0]) / (pol[j][0] - pol[i][0]) + pol[i][1])) ): contains = not contains j = i i += 1 if contains: return True return False def _contains_shapely_impl(self, location): point = Point(location.lat, location.lon) return self.multi_polygon.contains(point) def __contains__(self, location): if self.open: return True return self._contains_impl(location)
randompt = (random.uniform(0, 100), random.uniform(0, 100), random.uniform(0.0, 2 * math.pi)) #Finds nearest point to the random point x_y = nearest(pts, randompt) # euler = DiffDrive_Euler.Euler(x_y,ur,ul) runge_kutta = DiffDrive_RungeKutta.RungeKutta(x_y, ul, ur) #this function call returns three possible points straight, left, right # possible_euler_pts = euler.euler_method() possible_pts = runge_kutta.runge_kutta() # print "straight , left , Right ",possible_euler_pts # finds the nearest of the three points to the random point newpt = nearest(possible_pts, randompt) #Collision check whether the newpoint generated lies inside any obstacle or not if polygons.contains(Point((newpt[0], newpt[1]))): continue #Check for new point not crossing the boundary of environment if newpt[0] > 100 or newpt[0] < 0 or newpt[1] < 0 or newpt[1] > 100: continue #generate a line from the nearest point "x-y" to the "newpt" and check whether the line is crossing any obstacle if not add line to the environment line = [(x_y[0], x_y[1]), (newpt[0], newpt[1])] if polygons.crosses(LineString(line)): continue lines.append(line) # color.append((0, 1, 1, 1)) # Add the line generated as an edge to the graph from vertex at point "x_y" to "newpt" graph.addEdge(index1, x_y, index2, newpt) index1 += 2 index2 += 2
if not utm_zone_number: utm_zone_number = pos[2] graph.add_node(item.id, lat=item.lat, lon=item.lon, pos=pos[:2]) #nodes += 1 items += 1 print('{0} items processed'.format(items), end='\r') print('{0} items processed'.format(items)) if shape_file: n = graph.number_of_nodes() i = 0 print('Apply shapefile') for node in graph.nodes(): p = Point(graph.node[node]['lon'], graph.node[node]['lat']) if not shape_file.contains(p): graph.remove_node(node) i += 1 print('{0}/{1} nodes processed'.format(i, n), end='\r') print('{0}/{1} nodes processed'.format(i, n)) print('Search for orphaned nodes') orphaned = set() n = graph.number_of_nodes() i = 0 for node in graph.nodes_iter(): if graph.degree(node) == 0: orphaned.add(node) i += 1 print('{0}/{1} nodes processed'.format(i, n), end='\r')
class Environment: def __init__(self, input_file, factor=1): self.plot_obstacles_polygon = [] self.obs_list = [] self.obs_polygon = MultiPolygon() # Shapely object to store all polygons self.initial_state, self.goal_state = [], [] self.resolution = 0 # Dimension of the plane. self.read_env_from_file(input_file) def read_env_from_file(self, input_file): # Read json input try: print(input_file) with open(input_file, mode='r', encoding='utf-8') as a_file: environment = json.loads(a_file.read()) except FileNotFoundError as fl: print("File not found for JSON ", fl) exit(1) except ValueError: print("Invalid JSON") exit(1) except Exception: print("Unable to process input file") exit(1) try: # Making sure the required entities are defined in the input json file. environment['resolution'] and environment['obstacles'] environment['initial_state'] and environment['goal_state'] except KeyError: print("Invalid Environment definition") exit(1) self.initial_state, self.goal_state = environment['initial_state'], environment['goal_state'] self.resolution = environment['resolution'] temp_polygon_list = [] for obs in environment['obstacles']: if not obs.get('shape') and obs.get('property') and obs['property'].get('vertices'): print("Shape element not present for the obstacles") continue if obs['shape'] == 'polygon': # print("Polygon with vertices %s" %(np.array(obs['property']['vertices'])/100)) polygon = mPolygon(np.array(obs['property']['vertices'])) temp_polygon_list.append(Polygon(obs['property']['vertices'])) self.plot_obstacles_polygon.append(polygon) self.obs_list.append(obs['property']['vertices']) else: print("Undefined shape") break self.obs_polygon = MultiPolygon(temp_polygon_list) def is_point_inside(self, xy): """ :param xy: tuple with x coordinate as first element and y coordinate as second element :return: True if the point is inside the obstacles and False if it isn't """ return Point(xy[0], xy[1]).within(self.obs_polygon) def is_line_inside(self, xy_start, xy_end): # xy_start is tuple of (x, y) coordinate of one end of the line. # xy_end is tuple of (x, y) coordinate of the other end of line. line = LineString([xy_start, xy_end]) return self.obs_polygon.contains(line) or self.obs_polygon.touches(line) or self.obs_polygon.crosses(line) def draw_env(self, path, key_xy, k_value): # Method to draw an arrow in the environment # path is a list of state objects. index 0 maps from-state and index 1 maps to-state. fig, ax = plt.subplots() x_path, y_path = [], [] for ls in path: x_path.append(key_xy(ls)[0]) y_path.append(key_xy(ls)[1]) colors = 100*np.random.rand(len(self.plot_obstacles_polygon)) p = PatchCollection(self.plot_obstacles_polygon, cmap=matplotlib.cm.jet, alpha=0.4) p.set_array(np.array(colors)) ax.add_collection(p) plt.colorbar(p) plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs', self.goal_state[0], self.goal_state[1], 'g^') plt.axis([0, self.resolution, 0, self.resolution]) plt.arrow(x_path[0], y_path[0], x_path[1]-x_path[0], y_path[1]-y_path[0], fc="k", ec="k", head_width=1.55, head_length=1.1) plt.title("figure" + str(k_value)+".png") fig.savefig("figure" + str(k_value)+".png", format='png', dpi=fig.dpi) def animate_path(self, path, key_xy): fig, ax = plt.subplots() colors = 100*np.random.rand(len(self.plot_obstacles_polygon)) p = PatchCollection(self.plot_obstacles_polygon, cmap=matplotlib.cm.jet, alpha=0.4) p.set_array(np.array(colors)) ax.add_collection(p) plt.colorbar(p) plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs', self.goal_state[0], self.goal_state[1], 'g^') plt.axis([0, self.resolution, 0, self.resolution]) x_0, y_0 = key_xy(path[0])[0], key_xy(path[0])[1] x_1, y_1 = key_xy(path[0 + 1])[0], key_xy(path[0 + 1])[1] dx, dy = x_1 - x_0, y_0 - y_1 qv = ax.quiver(x_0, y_0, dx, dy, angles='xy',scale_units='xy',scale=1) def animate(i): x_init, y_init =key_xy(path[i])[0], key_xy(path[i])[1] x_f, y_f = key_xy(path[i + 1])[0], key_xy(path[i + 1])[1] dx, dy = x_f - x_init, y_f - y_init qv.set_UVC(np.array(dx), np.array(dy)) qv.set_offsets((x_init, y_init)) return qv anim = animation.FuncAnimation(fig, animate, frames=range(0, len(path)-1), interval=500) plt.show() def get_apprx_visible_vertices(self, xy_robot): # To get visible vertices from robot point # xy_robot should be a tuple of (x, y) coordinate if self.is_point_inside(xy_robot): print("Invalid robot position") return None pool = copy.deepcopy(self.obs_list) pool.append([self.goal_state]) visible_vertices, visible_lines = [], [] for obj in pool: for vertex in obj: vertex = tuple(vertex) if vertex == xy_robot: continue crosses, line = self.visibility_line(xy_robot, vertex) if not crosses: visible_lines.append(line) visible_vertices.extend([x.xy[0][1], x.xy[1][1]] for x in visible_lines) return visible_vertices def get_actual_visible_vertices(self, xy_robot): if self.is_point_inside(xy_robot): print("Invalid robot position") return None pool = copy.deepcopy(self.obs_list) pool.append([self.goal_state]) visible_vertices, line_robot_vertices = [], {} def line_slope(xy1, xy2): return (xy2[1] - xy1[1])/(xy2[0] - xy1[0]) if (xy2[0] - xy1[0]) != 0 else sys.maxsize for obj in pool: for vertex in obj: crosses, line = self.visibility_line(xy_robot, vertex) if not crosses: if line_slope(xy_robot, vertex) in line_robot_vertices: if line.length < line_robot_vertices[line_slope(xy_robot, vertex)].length: line_robot_vertices[line_slope(xy_robot, vertex)] = line else: line_robot_vertices[line_slope(xy_robot, vertex)] = line visible_vertices.extend([x.xy[0][1], x.xy[1][1]] for x in line_robot_vertices.values()) return visible_vertices def visibility_line(self, xy_start, xy_end): # Helper Method to check if the line is intersected by any obstacles. line = LineString([xy_start, xy_end]) return self.obs_polygon.crosses(line) or self.obs_polygon.contains(line), line def __str__(self): return "Obstacle list: %s\nInitial State: %s\nGoal State: %s\nResolution: %d\n" \ % ([cord.xy for cord in self.plot_obstacles_polygon], self.initial_state, self.goal_state, self.resolution)
#Randomly generates a point with (x,y,theta) random values randompt = (random.uniform(0,100),random.uniform(0,100),random.uniform(0.0,2*math.pi)) #Finds nearest point to the random point x_y = nearest(pts,randompt) # euler = DiffDrive_Euler.Euler(x_y,ur,ul) runge_kutta = DiffDrive_RungeKutta.RungeKutta(x_y,ul,ur) #this function call returns three possible points straight, left, right # possible_euler_pts = euler.euler_method() possible_pts = runge_kutta.runge_kutta() # print "straight , left , Right ",possible_euler_pts # finds the nearest of the three points to the random point newpt = nearest(possible_pts,randompt) #Collision check whether the newpoint generated lies inside any obstacle or not if polygons.contains(Point((newpt[0],newpt[1]))): continue #Check for new point not crossing the boundary of environment if newpt[0]>100 or newpt[0]<0 or newpt[1]<0 or newpt[1]>100: continue #generate a line from the nearest point "x-y" to the "newpt" and check whether the line is crossing any obstacle if not add line to the environment line = [(x_y[0],x_y[1]),(newpt[0],newpt[1])] if polygons.crosses(LineString(line)): continue lines.append(line) # color.append((0, 1, 1, 1)) # Add the line generated as an edge to the graph from vertex at point "x_y" to "newpt" graph.addEdge(index1,x_y,index2,newpt) index1 += 2 index2 += 2
print 'Mask_C_FFC' tic = time.time() df['Mask_C_FFC'] = [haversine(FFC.Long.map(float)*np.pi/180.0,FFC.Lat.map(float)*np.pi/180.0,float(df.Long[i])*np.pi/180.0,float(df.Lat[i])*np.pi/180.0).min()<150 for i in range(len(df.Lat))] toc = time.time() print 'Mask_C_FFC', toc-tic print 'Mask_SPA_Out' tic = time.time() df['Mask_SPA_Out'] = [haversine((-168.9)*np.pi/180.0,-55.0*np.pi/180.0,df.Long[i]*np.pi/180.0,df.Lat[i]*np.pi/180.0)> 1.238*970.0 for i in range(len(df.Lat))] toc = time.time() print 'Mask_SPA_Out', toc-tic print 'Mask_Highland' tic = time.time() df['Mask_Highlands'] = [not Maria.contains(Point(df.Long[i],df.Lat[i])) for i in range(len(df.Lat))] toc = time.time() print 'Mask_Highland', toc-tic print 'index' tic = time.time() df['Index'] = range(len(df)) toc = time.time() print 'index', toc-tic data['feat_df'] = {k: np.array(v.values())[:,np.newaxis] for k,v in df.to_dict().iteritems()} with open(Output+'LOLA'+str(pix)+'_GRAIL_Dataset_2', 'wb') as fi: pickle.dump(data, fi, pickle.HIGHEST_PROTOCOL)
class Environment: def __init__(self, input_file, factor=1): self.plot_obstacles_polygon = [] self.obs_list = [] self.obs_polygon = MultiPolygon( ) # Shapely object to store all polygons self.initial_state, self.goal_state = [], [] self.resolution = 0 # Dimension of the plane. self.read_env_from_file(input_file) def read_env_from_file(self, input_file): # Read json input try: print(input_file) with open(input_file, mode='r', encoding='utf-8') as a_file: environment = json.loads(a_file.read()) except FileNotFoundError as fl: print("File not found for JSON ", fl) exit(1) except ValueError: print("Invalid JSON") exit(1) except Exception: print("Unable to process input file") exit(1) try: # Making sure the required entities are defined in the input json file. environment['resolution'] and environment['obstacles'] environment['initial_state'] and environment['goal_state'] except KeyError: print("Invalid Environment definition") exit(1) self.initial_state, self.goal_state = environment[ 'initial_state'], environment['goal_state'] self.resolution = environment['resolution'] temp_polygon_list = [] for obs in environment['obstacles']: if not obs.get('shape') and obs.get( 'property') and obs['property'].get('vertices'): print("Shape element not present for the obstacles") continue if obs['shape'] == 'polygon': # print("Polygon with vertices %s" %(np.array(obs['property']['vertices'])/100)) polygon = mPolygon(np.array(obs['property']['vertices'])) temp_polygon_list.append(Polygon(obs['property']['vertices'])) self.plot_obstacles_polygon.append(polygon) self.obs_list.append(obs['property']['vertices']) else: print("Undefined shape") break self.obs_polygon = MultiPolygon(temp_polygon_list) def is_point_inside(self, xy): """ :param xy: tuple with x coordinate as first element and y coordinate as second element :return: True if the point is inside the obstacles and False if it isn't """ return Point(xy[0], xy[1]).within(self.obs_polygon) def is_line_inside(self, xy_start, xy_end): # xy_start is tuple of (x, y) coordinate of one end of the line. # xy_end is tuple of (x, y) coordinate of the other end of line. line = LineString([xy_start, xy_end]) return self.obs_polygon.contains(line) or self.obs_polygon.touches( line) or self.obs_polygon.crosses(line) def draw_env(self, path, key_xy, k_value): # Method to draw an arrow in the environment # path is a list of state objects. index 0 maps from-state and index 1 maps to-state. fig, ax = plt.subplots() x_path, y_path = [], [] for ls in path: x_path.append(key_xy(ls)[0]) y_path.append(key_xy(ls)[1]) colors = 100 * np.random.rand(len(self.plot_obstacles_polygon)) p = PatchCollection(self.plot_obstacles_polygon, cmap=matplotlib.cm.jet, alpha=0.4) p.set_array(np.array(colors)) ax.add_collection(p) plt.colorbar(p) plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs', self.goal_state[0], self.goal_state[1], 'g^') plt.axis([0, self.resolution, 0, self.resolution]) plt.arrow(x_path[0], y_path[0], x_path[1] - x_path[0], y_path[1] - y_path[0], fc="k", ec="k", head_width=1.55, head_length=1.1) plt.title("figure" + str(k_value) + ".png") fig.savefig("figure" + str(k_value) + ".png", format='png', dpi=fig.dpi) def animate_path(self, path, key_xy): fig, ax = plt.subplots() colors = 100 * np.random.rand(len(self.plot_obstacles_polygon)) p = PatchCollection(self.plot_obstacles_polygon, cmap=matplotlib.cm.jet, alpha=0.4) p.set_array(np.array(colors)) ax.add_collection(p) plt.colorbar(p) plt.plot([self.initial_state[0]], [self.initial_state[1]], 'bs', self.goal_state[0], self.goal_state[1], 'g^') plt.axis([0, self.resolution, 0, self.resolution]) x_0, y_0 = key_xy(path[0])[0], key_xy(path[0])[1] x_1, y_1 = key_xy(path[0 + 1])[0], key_xy(path[0 + 1])[1] dx, dy = x_1 - x_0, y_0 - y_1 qv = ax.quiver(x_0, y_0, dx, dy, angles='xy', scale_units='xy', scale=1) def animate(i): x_init, y_init = key_xy(path[i])[0], key_xy(path[i])[1] x_f, y_f = key_xy(path[i + 1])[0], key_xy(path[i + 1])[1] dx, dy = x_f - x_init, y_f - y_init qv.set_UVC(np.array(dx), np.array(dy)) qv.set_offsets((x_init, y_init)) return qv anim = animation.FuncAnimation(fig, animate, frames=range(0, len(path) - 1), interval=500) plt.show() def get_apprx_visible_vertices(self, xy_robot): # To get visible vertices from robot point # xy_robot should be a tuple of (x, y) coordinate if self.is_point_inside(xy_robot): print("Invalid robot position") return None pool = copy.deepcopy(self.obs_list) pool.append([self.goal_state]) visible_vertices, visible_lines = [], [] for obj in pool: for vertex in obj: vertex = tuple(vertex) if vertex == xy_robot: continue crosses, line = self.visibility_line(xy_robot, vertex) if not crosses: visible_lines.append(line) visible_vertices.extend([x.xy[0][1], x.xy[1][1]] for x in visible_lines) return visible_vertices def get_actual_visible_vertices(self, xy_robot): if self.is_point_inside(xy_robot): print("Invalid robot position") return None pool = copy.deepcopy(self.obs_list) pool.append([self.goal_state]) visible_vertices, line_robot_vertices = [], {} def line_slope(xy1, xy2): return (xy2[1] - xy1[1]) / (xy2[0] - xy1[0]) if ( xy2[0] - xy1[0]) != 0 else sys.maxsize for obj in pool: for vertex in obj: crosses, line = self.visibility_line(xy_robot, vertex) if not crosses: if line_slope(xy_robot, vertex) in line_robot_vertices: if line.length < line_robot_vertices[line_slope( xy_robot, vertex)].length: line_robot_vertices[line_slope(xy_robot, vertex)] = line else: line_robot_vertices[line_slope(xy_robot, vertex)] = line visible_vertices.extend([x.xy[0][1], x.xy[1][1]] for x in line_robot_vertices.values()) return visible_vertices def visibility_line(self, xy_start, xy_end): # Helper Method to check if the line is intersected by any obstacles. line = LineString([xy_start, xy_end]) return self.obs_polygon.crosses(line) or self.obs_polygon.contains( line), line def __str__(self): return "Obstacle list: %s\nInitial State: %s\nGoal State: %s\nResolution: %d\n" \ % ([cord.xy for cord in self.plot_obstacles_polygon], self.initial_state, self.goal_state, self.resolution)