from matplotlib.path import Path def is_inside_outline(room, x, y): """Check whether given point is inside room outline From sara_virtual_scan_extractor.Annotator, written by Kousuke Ariga.""" # Edges look something like: # [[[x1, y1, z1], [x2, y2, z2]], # [[x3, y3, z3], [x2, y2, z2]], # ..., # [[xk, yk, zk], [xn, yn, zn]]] # # Therefore, an edge has a form: # [[x1, y1, z1], [x2, y2, z2]] edges = copy.deepcopy(room['outline']) for edge in edges: edge[0] = edge[0][:2] edge[1] = edge[1][:2] # Now edges look like: # [[x1, y1], [x2, y2]] # Sort the edges so that it will be in order of a path. The direction # of the path (i.e. clockwise or not) doesn't matter. Put one edge in # edges in the front of sorted_edges. This edge is the starting point # of path search. sorted_edges = [edges[0]] edges.remove(edges[0]) # Until all edge in the edges move to sorted_edges while edges: # Suppose the last edge in the sorted_edges look like: # [[[xi, yi], [xj, yj]] # ..., # [[xk, yk], [xl, yl]]] # # For each edge, if the edge is, for edge in edges: # [[xl, yl], [xm, ym]]] # Append the edge to the end of the sored edges if edge[0] == sorted_edges[-1][1]: sorted_edges.append(edge) edges.remove(edge) # [[xm, ym], [xl, yl]]] # Swap the edge and append to the end of the sored edges elif edge[1] == sorted_edges[-1][1]: swapped_edge = [edge[1], edge[0]] sorted_edges.append(swapped_edge) edges.remove(edge) # [[xm, ym], [xi, yi]]] # Insert the edge in the front of the sored edges elif edge[1] == sorted_edges[0][0]: sorted_edges.insert(0, edge) edges.remove(edge) # [[xi, yi], [xm, ym]]] # Swap the edge and insert in the front of the sored edges elif edge[0] == sorted_edges[0][0]: swapped_edge = [edge[1], edge[0]] sorted_edges.insert(0, swapped_edge) edges.remove(edge) # sorted_edges now looks like: # [[[x1, y1], [x2, y2]], # [[x2, y2], [x3, y3]], # ..., # [[xn-1, yn-1], [xn, yn]]] outline = [] for edge in sorted_edges: outline.append(edge[0]) # outline looks like: # [[x1, y1], # [x2, y2], # ..., # [xn-1, yn-1]] # # Append [xn, yn] to outline outline.append(sorted_edges[-1][1]) p = Path(np.array(outline)) return p.contains_point((x, y))