def calculate_region_ellipse(x, y, centre, size): # Return boolean array of same shape as x and y. xy = np.stack((x.ravel(), y.ravel()), axis=1) # Shape (npoints, 2) ellipse = Ellipse(centre, width=size[0], height=size[1]) region = ellipse.contains_points(xy) # Shape (2*npoints) region.shape = x.shape # Shape (npoints, 2) return region
####### ELIPSE OBSTACLE #################### elipse = Ellipse((150, 100), 80, 40, 0, facecolor='None', edgecolor='green') ###### PATCHING THEM TOGETHER ############### pathpatch = PathPatch(path, facecolor='None', edgecolor='green') pathpatch2 = PathPatch(circle, facecolor='None', edgecolor='green') pathpatch3 = Ellipse((150, 100), 80, 40, 0, facecolor='None', edgecolor='green') ####### CHECKING TO SEE IF ROBOT IS IN OBSTACLE ################ inside_polygons = (path.contains_points(points, radius=1e-9) ) #true if it is inside the polygon,otherwise false inside_elipse = (elipse.contains_points(points, radius=1e-9)) inside_circle = (circle.contains_points(points, radius=1e-9)) inside_obstacle = (any(inside_polygons == True)) or (any( inside_circle == True)) or (any(inside_elipse == True)) if inside_obstacle: print("inside the obstacle you are, you little thing") ####################### REDUCING SEACH NODES BY SUBTRACTING OBSTACLES ################ inside_polygons2 = np.where((path.contains_points(map, radius=1e-9)), 20000, 0) inside_elipse2 = np.where((elipse.contains_points(map, radius=1e-9)), 20000, 0) inside_circle2 = np.where((circle.contains_points(map, radius=1e-9)), 20000, 0) mask = (inside_polygons2 + inside_elipse2 + inside_circle2) reduced = np.array(list(zip(x_m.flatten(), y_m.flatten(), mask.flatten()))) reduced2 = np.vstack((x_m.flatten(), y_m.flatten(), mask)).T newmap = reduced[np.all( reduced < 20000, axis=1), :] # if any value in reduced map is True remove it