def test_point_in_polygon(self): # on polygon self.assertEqual(pyclipper.PointInPolygon((180, 200), PATH_SUBJ_1), -1) # in polygon self.assertEqual(pyclipper.PointInPolygon((200, 180), PATH_SUBJ_1), 1) # outside of polygon self.assertEqual(pyclipper.PointInPolygon((500, 500), PATH_SUBJ_1), 0)
def get_outside_point(start_point, vect, polygon): point = start_point reverse_point = start_point soluce = None # soluce in english walkthrough while soluce is None: point = [point[0]+vect[0], point[1]+vect[1]] if not pyclipper.PointInPolygon(point, polygon): return point reverse_point = [reverse_point[0] - vect[0], reverse_point[1] - vect[1]] if not pyclipper.PointInPolygon(reverse_point, polygon): return reverse_point
def resolveTree(clipperPaths, processHoles, side): ''' resolves the tree of the paths, from outside to inside, returns list of [path_to_cut_inside, <[hole_path_to_avoid1], [hole_path_to_avoid2], ...>] ''' pairGraph = [] for ip1 in range(0, len(clipperPaths)): p1 = clipperPaths[ip1] for ip2 in range(ip1 + 1, len(clipperPaths)): p2 = clipperPaths[ip2] if pyclipper.PointInPolygon(p2[0], p1) != 0: pairGraph.append([ip1, ip2]) #ip1 parent,ip2 child elif pyclipper.PointInPolygon(p1[0], p2) != 0: pairGraph.append([ip2, ip1]) #ip2 parent,ip1 child #calc nesting levels nesting = [0] * len(clipperPaths) for ip1 in range(0, len(clipperPaths)): #for each path find the nesting level for pp in pairGraph: if pp[1] == ip1: nesting[ip1] = nesting[ip1] + 1 print "Nesting info: ", nesting #make feature paths first path is outer others are inner (direct holes) features = [] for ip in range(0, len(clipperPaths)): if side == "Outside" and not processHoles: if nesting[ ip] == 0: #add all paths with odd nesting as outside paths, odd will be added as holes features.append([ip]) else: if nesting[ ip] % 2 == 0: #add all paths with odd nesting as outside paths, odd will be added as holes features.append([ip]) if processHoles or side == "Outside": for pair in pairGraph: ip1 = pair[0] ip2 = pair[1] for feat in features: if feat[0] == ip1 and nesting[ip2] % 2 == 1: feat.append(ip2) #print(features) tree = [] for feat in features: paths = [] for ip in feat: paths.append(clipperPaths[ip]) tree.append(paths) return tree
def point_in_polygon(point, polygon): stc = pyclipper.scale_to_clipper in_polygon = pyclipper.PointInPolygon(stc(point), stc(polygon)) if in_polygon == 1: return True else: return False
def point_inside_loop(point, loop): """tests to see if a point is inside (1), on(-1), or outside (0) of a loop""" scaled_loop = pyclipper.scale_to_clipper(loop, SCALING_FACTOR) scaled_point = [int(point[0] * SCALING_FACTOR), int(point[1] * SCALING_FACTOR)] is_point_inside = pyclipper.PointInPolygon(scaled_point, scaled_loop) return is_point_inside
def paths_contain(pt, paths): cnt = 0 pt = pyclipper.scale_to_clipper([pt], SCALING_FACTOR)[0] for path in paths: path = pyclipper.scale_to_clipper(path, SCALING_FACTOR) if pyclipper.PointInPolygon(pt, path): cnt = 1 - cnt return cnt % 2 != 0
def isOutsideCutRegion(toolPos, cut_region_tp_polytree, include_boundary=False): #check if toolPos is outside cut area - i.e. used to check if we reached end of this continuous pass for node in cut_region_tp_polytree.Childs: pip = pyclipper.PointInPolygon(toolPos, node.Contour) if pip == -1 and include_boundary: return True if pip == 1: #if inside the root path, check if not inside any of its holes for hole in node.Childs: pip = pyclipper.PointInPolygon(toolPos, hole.Contour) if pip == -1 and include_boundary: return True if pip == 1: return True # must not be inside all other (holes) return False return True
def findInterior(polygon, polys): for point in polygon.path: for exterior_polygon in polys: if exterior_polygon == polygon: continue if pyclipper.PointInPolygon(point, exterior_polygon.path) == 1: print("Polygon %d is inside polygon %d" % (polygon.id, exterior_polygon.id)) polygon.interior = True return
def terminals(geom, cell, datafield): terms = dict() for key, polygons in datafield.get_terminals().items(): for points in polygons: for lbl in cell.labels: if pyclipper.PointInPolygon(lbl.position, points) == 1: print(' .terms detected: ' + lbl.text) terms[lbl.text] = Terminal([points]) for name, term in terms.items(): term.set_slope() term.metal_connection(datafield, name) term.metal_edge(datafield) term.extrude(geom, datafield) utils.write_cell((99, 0), 'Terminal Edges', terms)
def rate_transferred(room): #obj in room paths = pyclipper.scale_to_clipper(room["contour"]) objpos = pyclipper.scale_to_clipper( [[o["translate"][0], o["translate"][2]] for o in room["objList"]]) tolerance = ( (np.array(paths).max(axis=1) - np.array(paths).min(axis=1)).min() * 0.01).astype("int") pco = pyclipper.PyclipperOffset() pco.AddPaths(paths, pyclipper.JT_SQUARE, pyclipper.ET_CLOSEDPOLYGON) offsetpath = pco.Execute(tolerance) flags = [ any([pyclipper.PointInPolygon(pos, path) for path in offsetpath]) for pos in objpos ] score = np.log(np.std(np.array(objpos), axis=0) + 0.000001).sum() + np.mean(flags) * 10 return score
def test_point_in_polygon(self): with self.assertWarns(DeprecationWarning): self.assertEqual(pyclipper.PointInPolygon((180, 200), PATH_SUBJ_1), -1)
def encloses_endpoints(self, points): if pyclipper.PointInPolygon(self.endpoints[0], points) != 0: return True elif pyclipper.PointInPolygon(self.endpoints[1], points) != 0: return True
def poly1_in_poly2(poly1, poly2): point = poly1[0] if pyclipper.PointInPolygon(point, poly2): return True else: return False
def encloses(coord, points): """ """ sc = constants.CLIPPER_SCALE coord = st(coord.to_list(), sc) points = st(points, sc) return pyclipper.PointInPolygon(coord, points) != 0
def isPointInPolygon(point, path): import pyclipper return True if (pyclipper.PointInPolygon(point, path) == 1) else False
def point_inside(points, position): assert position is not None, 'No label position found.' if pyclipper.PointInPolygon(position, points) != 0: return True return False
def get_path(bound, holes, path): pc = pyclipper.Pyclipper() pc.AddPath(path, pyclipper.PT_SUBJECT, False) pc.AddPath(bound, pyclipper.PT_CLIP, True) result = pyclipper.PolyTreeToPaths(pc.Execute2(pyclipper.CT_DIFFERENCE,\ pyclipper.PFT_EVENODD,\ pyclipper.PFT_EVENODD)) result_size = len(result) if result_size == 1: # cross once the bound middle_point = get_middle_point([result[0][0], result[0][1]]) vetctor = get_vector([result[0][0], result[0][1]]) point = get_inside_point(middle_point, vetctor, bound) return get_path(bound, holes, [path[0], point]) + \ get_path(bound, holes, [point, path[1]])[1:] elif result_size > 1: # cross multiple times the bound middle_point = None solution = [path[0]] previous_middle_point = path[0] for i in range(0, result_size-1): middle_point = get_middle_point([result[i][1], result[i+1][0]]) vetctor = get_vector([result[i][1], result[i+1][0]]) for hole in holes: if pyclipper.PointInPolygon(middle_point, hole): middle_point = get_outside_point(middle_point, vetctor, hole) break solution += get_path(bound, holes, [previous_middle_point, middle_point])[1:] previous_middle_point = middle_point solution += get_path(bound, holes, [previous_middle_point, path[1]])[1:] return solution pc.Clear() pc.AddPath(path, pyclipper.PT_SUBJECT, False) for hole in holes: pc.AddPath(hole, pyclipper.PT_CLIP, True) result = pyclipper.PolyTreeToPaths(pc.Execute2(pyclipper.CT_DIFFERENCE, pyclipper.PFT_EVENODD, pyclipper.PFT_EVENODD)) result_size = len(result) if result_size == 2: # cross only one hole once for hole in holes: # seek the crossing hole pc.Clear() pc.AddPath(path, pyclipper.PT_SUBJECT, False) pc.AddPath(hole, pyclipper.PT_CLIP, True) result = pyclipper.PolyTreeToPaths( pc.Execute2(pyclipper.CT_DIFFERENCE, pyclipper.PFT_EVENODD, pyclipper.PFT_EVENODD)) result_size = len(result) if result_size == 2: middle_point = get_middle_point([result[0][1], result[1][0]]) vetctor = get_vector([result[0][1], result[1][0]]) point = get_outside_point(middle_point, vetctor, hole) return get_path(bound, holes, [path[0], point]) + \ get_path(bound, holes, [point, path[1]])[1:] elif result_size > 2: # cross multipleholes or one hole multiple times middle_point = None solution = [path[0]] previous_middle_point = path[0] for i in range(1, result_size - 1): middle_point = get_middle_point([result[i][0], result[i][1]]) solution += get_path(bound, holes, [previous_middle_point, middle_point])[1:] previous_middle_point = middle_point solution += get_path(bound, holes, [previous_middle_point, path[1]])[1:] return solution return path
def constructSSD(asas, traf, priocode="RS1"): """ Calculates the FRV and ARV of the SSD """ N = 0 # Parameters N_angle = 180 # [-] Number of points on circle (discretization) vmin = asas.vmin # [m/s] Defined in asas.py vmax = asas.vmax # [m/s] Defined in asas.py hsep = asas.R # [m] Horizontal separation (5 NM) margin = asas.mar # [-] Safety margin for evasion hsepm = hsep * margin # [m] Horizontal separation with safety margin alpham = 0.4999 * np.pi # [rad] Maximum half-angle for VO betalos = np.pi / 4 # [rad] Minimum divertion angle for LOS (45 deg seems optimal) adsbmax = 65. * nm # [m] Maximum ADS-B range beta = np.pi / 4 + betalos / 2 if priocode == "RS7" or priocode == "RS8": adsbmax /= 2 # Relevant info from traf gsnorth = traf.gsnorth gseast = traf.gseast lat = traf.lat lon = traf.lon ntraf = traf.ntraf hdg = traf.hdg gs_ap = traf.ap.tas hdg_ap = traf.ap.trk apnorth = np.cos(hdg_ap / 180 * np.pi) * gs_ap apeast = np.sin(hdg_ap / 180 * np.pi) * gs_ap # Local variables, will be put into asas later FRV_loc = [None] * traf.ntraf ARV_loc = [None] * traf.ntraf # For calculation purposes ARV_calc_loc = [None] * traf.ntraf FRV_area_loc = np.zeros(traf.ntraf, dtype=np.float32) ARV_area_loc = np.zeros(traf.ntraf, dtype=np.float32) # # Use velocity limits for the ring-shaped part of the SSD # Discretize the circles using points on circle angles = np.arange(0, 2 * np.pi, 2 * np.pi / N_angle) # Put points of unit-circle in a (180x2)-array (CW) xyc = np.transpose( np.reshape(np.concatenate((np.sin(angles), np.cos(angles))), (2, N_angle))) # Map them into the format pyclipper wants. Outercircle CCW, innercircle CW circle_tup = (tuple(map(tuple, np.flipud(xyc * vmax))), tuple(map(tuple, xyc * vmin))) circle_lst = [ list(map(list, np.flipud(xyc * vmax))), list(map(list, xyc * vmin)) ] # If no traffic if ntraf == 0: return # If only one aircraft elif ntraf == 1: # Map them into the format ARV wants. Outercircle CCW, innercircle CW ARV_loc[0] = circle_lst FRV_loc[0] = [] ARV_calc_loc[0] = ARV_loc[0] # Calculate areas and store in asas FRV_area_loc[0] = 0 ARV_area_loc[0] = np.pi * (vmax**2 - vmin**2) return # Function qdrdist_matrix needs 4 vectors as input (lat1,lon1,lat2,lon2) # To be efficient, calculate all qdr and dist in one function call # Example with ntraf = 5: ind1 = [0,0,0,0,1,1,1,2,2,3] # ind2 = [1,2,3,4,2,3,4,3,4,4] # This way the qdrdist is only calculated once between every aircraft # To get all combinations, use this function to get the indices ind1, ind2 = qdrdist_matrix_indices(ntraf) # Get absolute bearing [deg] and distance [nm] # Not sure abs/rel, but qdr is defined from [-180,180] deg, w.r.t. North [qdr, dist] = geo.qdrdist_matrix(lat[ind1], lon[ind1], lat[ind2], lon[ind2]) # Put result of function from matrix to ndarray qdr = np.reshape(np.array(qdr), np.shape(ind1)) dist = np.reshape(np.array(dist), np.shape(ind1)) # SI-units from [deg] to [rad] qdr = np.deg2rad(qdr) # Get distance from [nm] to [m] dist = dist * nm # In LoS the VO can't be defined, act as if dist is on edge dist[dist < hsepm] = hsepm # Calculate vertices of Velocity Obstacle (CCW) # These are still in relative velocity space, see derivation in appendix # Half-angle of the Velocity obstacle [rad] # Include safety margin alpha = np.arcsin(hsepm / dist) # Limit half-angle alpha to 89.982 deg. Ensures that VO can be constructed alpha[alpha > alpham] = alpham # Relevant sin/cos/tan sinqdr = np.sin(qdr) cosqdr = np.cos(qdr) tanalpha = np.tan(alpha) cosqdrtanalpha = cosqdr * tanalpha sinqdrtanalpha = sinqdr * tanalpha # Relevant x1,y1,x2,y2 (x0 and y0 are zero in relative velocity space) x1 = (sinqdr + cosqdrtanalpha) * 2 * vmax x2 = (sinqdr - cosqdrtanalpha) * 2 * vmax y1 = (cosqdr - sinqdrtanalpha) * 2 * vmax y2 = (cosqdr + sinqdrtanalpha) * 2 * vmax # Consider every aircraft for i in range(ntraf): # Calculate SSD only for aircraft in conflict (See formulas appendix) if asas.inconf[i]: # SSD for aircraft i # Get indices that belong to aircraft i ind = np.where(np.logical_or(ind1 == i, ind2 == i))[0] # Check whether there are any aircraft in the vicinity if len(ind) == 0: # No aircraft in the vicinity # Map them into the format ARV wants. Outercircle CCW, innercircle CW ARV_loc[i] = circle_lst FRV_loc[i] = [] ARV_calc_loc[i] = ARV_loc[i] # Calculate areas and store in asas FRV_area_loc[i] = 0 ARV_area_loc[i] = np.pi * (vmax**2 - vmin**2) else: # The i's of the other aircraft i_other = np.delete(np.arange(0, ntraf), i) # Aircraft that are within ADS-B range ac_adsb = np.where(dist[ind] < adsbmax)[0] # Now account for ADS-B range in indices of other aircraft (i_other) ind = ind[ac_adsb] i_other = i_other[ac_adsb] if not priocode == "RS7" and not priocode == "RS8": # Put it in class-object (not for RS7 and RS8) asas.inrange[i] = i_other else: asas.inrange2[i] = i_other # VO from 2 to 1 is mirror of 1 to 2. Only 1 to 2 can be constructed in # this manner, so need a correction vector that will mirror the VO fix = np.ones(np.shape(i_other)) fix[i_other < i] = -1 # Relative bearing [deg] from [-180,180] # (less required conversions than rad in RotA) fix_ang = np.zeros(np.shape(i_other)) fix_ang[i_other < i] = 180. # Get vertices in an x- and y-array of size (ntraf-1)*3x1 x = np.concatenate( (gseast[i_other], x1[ind] * fix + gseast[i_other], x2[ind] * fix + gseast[i_other])) y = np.concatenate( (gsnorth[i_other], y1[ind] * fix + gsnorth[i_other], y2[ind] * fix + gsnorth[i_other])) # Reshape [(ntraf-1)x3] and put arrays in one array [(ntraf-1)x3x2] x = np.transpose(x.reshape(3, np.shape(i_other)[0])) y = np.transpose(y.reshape(3, np.shape(i_other)[0])) xy = np.dstack((x, y)) # Make a clipper object pc = pyclipper.Pyclipper() # Add circles (ring-shape) to clipper as subject pc.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) # Extra stuff needed for RotA if priocode == "RS6": # Make another clipper object for RotA pc_rota = pyclipper.Pyclipper() pc_rota.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) # Bearing calculations from own view and other view brg_own = np.mod( (np.rad2deg(qdr[ind]) + fix_ang - hdg[i]) + 540., 360.) - 180. brg_other = np.mod( (np.rad2deg(qdr[ind]) + 180. - fix_ang - hdg[i_other]) + 540., 360.) - 180. # Add each other other aircraft to clipper as clip for j in range(np.shape(i_other)[0]): ## Debug prints ## print(traf.id[i] + " - " + traf.id[i_other[j]]) ## print(dist[ind[j]]) # Scale VO when not in LOS if dist[ind[j]] > hsepm: # Normally VO shall be added of this other a/c VO = pyclipper.scale_to_clipper( tuple(map(tuple, xy[j, :, :]))) else: # Pair is in LOS, instead of triangular VO, use darttip # Check if bearing should be mirrored if i_other[j] < i: qdr_los = qdr[ind[j]] + np.pi else: qdr_los = qdr[ind[j]] # Length of inner-leg of darttip leg = 1.1 * vmax / np.cos(beta) * np.array( [1, 1, 1, 0]) # Angles of darttip angles_los = np.array([ qdr_los + 2 * beta, qdr_los, qdr_los - 2 * beta, 0. ]) # Calculate coordinates (CCW) x_los = leg * np.sin(angles_los) y_los = leg * np.cos(angles_los) # Put in array of correct format xy_los = np.vstack((x_los, y_los)).T # Scale darttip VO = pyclipper.scale_to_clipper( tuple(map(tuple, xy_los))) # Add scaled VO to clipper pc.AddPath(VO, pyclipper.PT_CLIP, True) # For RotA it is possible to ignore if priocode == "RS6": if brg_own[j] >= -20. and brg_own[j] <= 110.: # Head-on or converging from right pc_rota.AddPath(VO, pyclipper.PT_CLIP, True) elif brg_other[j] <= -110. or brg_other[j] >= 110.: # In overtaking position pc_rota.AddPath(VO, pyclipper.PT_CLIP, True) # Detect conflicts for smaller layer in RS7 and RS8 if priocode == "RS7" or priocode == "RS8": if pyclipper.PointInPolygon( pyclipper.scale_to_clipper( (gseast[i], gsnorth[i])), VO): asas.inconf2[i] = True if priocode == "RS5": if pyclipper.PointInPolygon( pyclipper.scale_to_clipper( (apeast[i], apnorth[i])), VO): asas.ap_free[i] = False # Execute clipper command FRV = pyclipper.scale_from_clipper( pc.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) ARV = pc.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO) if not priocode == "RS1" and not priocode == "RS5" and not priocode == "RS7" and not priocode == "RS8": # Make another clipper object for extra intersections pc2 = pyclipper.Pyclipper() # When using RotA clip with pc_rota if priocode == "RS6": # Calculate ARV for RotA ARV_rota = pc_rota.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO) if len(ARV_rota) > 0: pc2.AddPaths(ARV_rota, pyclipper.PT_CLIP, True) else: # Put the ARV in there, make sure it's not empty if len(ARV) > 0: pc2.AddPaths(ARV, pyclipper.PT_CLIP, True) # Scale back ARV = pyclipper.scale_from_clipper(ARV) # Check if ARV or FRV is empty if len(ARV) == 0: # No aircraft in the vicinity # Map them into the format ARV wants. Outercircle CCW, innercircle CW ARV_loc[i] = [] FRV_loc[i] = circle_lst ARV_calc_loc[i] = [] # Calculate areas and store in asas FRV_area_loc[i] = np.pi * (vmax**2 - vmin**2) ARV_area_loc[i] = 0 elif len(FRV) == 0: # Should not happen with one a/c or no other a/c in the vicinity. # These are handled earlier. Happens when RotA has removed all # Map them into the format ARV wants. Outercircle CCW, innercircle CW ARV_loc[i] = circle_lst FRV_loc[i] = [] ARV_calc_loc[i] = circle_lst # Calculate areas and store in asas FRV_area_loc[i] = 0 ARV_area_loc[i] = np.pi * (vmax**2 - vmin**2) else: # Check multi exteriors, if this layer is not a list, it means it has no exteriors # In that case, make it a list, such that its format is consistent with further code if not type(FRV[0][0]) == list: FRV = [FRV] if not type(ARV[0][0]) == list: ARV = [ARV] # Store in asas FRV_loc[i] = FRV ARV_loc[i] = ARV # Calculate areas and store in asas FRV_area_loc[i] = area(FRV) ARV_area_loc[i] = area(ARV) # For resolution purposes sometimes extra intersections are wanted if priocode == "RS2" or priocode == "RS9" or priocode == "RS6" or priocode == "RS3" or priocode == "RS4": # Make a box that covers right or left of SSD own_hdg = hdg[i] * np.pi / 180 # Efficient calculation of box, see notes if priocode == "RS2" or priocode == "RS6": # CW or right-turning sin_table = np.array( [[1, 0], [-1, 0], [-1, -1], [1, -1]], dtype=np.float64) cos_table = np.array( [[0, 1], [0, -1], [1, -1], [1, 1]], dtype=np.float64) elif priocode == "RS9": # CCW or left-turning sin_table = np.array( [[1, 0], [1, 1], [-1, 1], [-1, 0]], dtype=np.float64) cos_table = np.array( [[0, 1], [-1, 1], [-1, -1], [0, -1]], dtype=np.float64) # Overlay a part of the full SSD if priocode == "RS2" or priocode == "RS9" or priocode == "RS6": # Normalized coordinates of box xyp = np.sin(own_hdg) * sin_table + np.cos( own_hdg) * cos_table # Scale with vmax (and some factor) and put in tuple part = pyclipper.scale_to_clipper( tuple(map(tuple, 1.1 * vmax * xyp))) pc2.AddPath(part, pyclipper.PT_SUBJECT, True) elif priocode == "RS3": # Small ring xyp = (tuple( map(tuple, np.flipud(xyc * min(vmax, gs_ap[i] + 0.1)))), tuple( map(tuple, xyc * max(vmin, gs_ap[i] - 0.1)))) part = pyclipper.scale_to_clipper(xyp) pc2.AddPaths(part, pyclipper.PT_SUBJECT, True) elif priocode == "RS4": hdg_sel = hdg[i] * np.pi / 180 xyp = np.array([[ np.sin(hdg_sel - 0.0087), np.cos(hdg_sel - 0.0087) ], [0, 0], [ np.sin(hdg_sel + 0.0087), np.cos(hdg_sel + 0.0087) ]], dtype=np.float64) part = pyclipper.scale_to_clipper( tuple(map(tuple, 1.1 * vmax * xyp))) pc2.AddPath(part, pyclipper.PT_SUBJECT, True) # Execute clipper command ARV_calc = pyclipper.scale_from_clipper( pc2.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) N += 1 # If no smaller ARV is found, take the full ARV if len(ARV_calc) == 0: ARV_calc = ARV # Check multi exteriors, if this layer is not a list, it means it has no exteriors # In that case, make it a list, such that its format is consistent with further code if not type(ARV_calc[0][0]) == list: ARV_calc = [ARV_calc] # Shortest way out prio, so use full SSD (ARV_calc = ARV) else: ARV_calc = ARV # Update calculatable ARV for resolutions ARV_calc_loc[i] = ARV_calc # If sequential approach, the local should go elsewhere if not priocode == "RS7" and not priocode == "RS8": asas.FRV = FRV_loc asas.ARV = ARV_loc asas.ARV_calc = ARV_calc_loc asas.FRV_area = FRV_area_loc asas.ARV_area = ARV_area_loc else: asas.ARV_calc2 = ARV_calc_loc return
def is_nested_polygons(hole, poly): for point in hole.points: if pyclipper.PointInPolygon(point, poly.points) != 1: return False return True
def constructSSD1(asas, traf): """ Calculates the FRV and ARV of the SSD """ #N = 0 # Parameters N_angle = 180 # [-] Number of points on circle (discretization) vmin = asas.vmin # [m/s] Defined in asas.py vmax = asas.vmax # [m/s] Defined in asas.py hsep = asas.R # [m] Horizontal separation (5 NM) margin = asas.mar # [-] Safety margin for evasion hsepm = hsep * margin # [m] Horizontal separation with safety margin alpham = 0.4999 * np.pi # [rad] Maximum half-angle for VO betalos = np.pi / 4 # [rad] Minimum divertion angle for LOS (45 deg seems optimal) adsbmax = 200. * nm # [m] Maximum ADS-B range PRIOR: 65NM, 250NM beta = np.pi / 4 + betalos / 2 # Relevant info from traf gsnorth = traf.gsnorth gseast = traf.gseast lat = traf.lat lon = traf.lon ntraf = traf.ntraf # Local variables, will be put into asas later FRV_loc = [None] * traf.ntraf FRV_loc_5 = [None] * traf.ntraf #NEWWWW FRV_loc_3 = [None] * traf.ntraf #NEWWWW FRV_loc_min = [None] * traf.ntraf #NEWWWW FRV_loc_tla = [None] * traf.ntraf #NEWWWW FRV_loc_dlos = [None] * traf.ntraf #NEWWWW ARV_loc = [None] * traf.ntraf ARV_loc_3 = [None] * traf.ntraf #NEWWWW ARV_loc_5 = [None] * traf.ntraf #NEWWWW ARV_loc_min = [None] * traf.ntraf #NEWWWW ARV_loc_tla = [None] * traf.ntraf #NEWWWW # For calculation purposes ARV_calc_loc = [None] * traf.ntraf ARV_calc_locmin = [None] * traf.ntraf ARV_calc_loc_glb = [None] * traf.ntraf ARV_calc_loc_dlos = [None] * traf.ntraf ARV_calc_loc_dcpa = [None] * traf.ntraf FRV_area_loc = np.zeros(traf.ntraf, dtype=np.float32) FRV_area_loc_5 = np.zeros(traf.ntraf, dtype=np.float32) FRV_area_loc_3 = np.zeros(traf.ntraf, dtype=np.float32) FRV_area_loc_min = np.zeros(traf.ntraf, dtype=np.float32) FRV_area_loc_tla = np.zeros(traf.ntraf, dtype=np.float32) FRV_area_loc_dlos = np.zeros(traf.ntraf, dtype=np.float32) ARV_area_loc = np.zeros(traf.ntraf, dtype=np.float32) ARV_area_loc_5 = np.zeros(traf.ntraf, dtype=np.float32) ARV_area_loc_3 = np.zeros(traf.ntraf, dtype=np.float32) ARV_area_loc_min = np.zeros(traf.ntraf, dtype=np.float32) ARV_area_loc_tla = np.zeros(traf.ntraf, dtype=np.float32) ARV_area_loc_dlos = np.zeros(traf.ntraf, dtype=np.float32) ARV_area_loc_dcpa = np.zeros(traf.ntraf, dtype=np.float32) # # Use velocity limits for the ring-shaped part of the SSD # Discretize the circles using points on circle angles = np.arange(0, 2 * np.pi, 2 * np.pi / N_angle) # Put points of unit-circle in a (180x2)-array (CW) xyc = np.transpose( np.reshape(np.concatenate((np.sin(angles), np.cos(angles))), (2, N_angle))) # Map them into the format pyclipper wants. Outercircle CCW, innercircle CW circle_tup = (tuple(map(tuple, np.flipud(xyc * vmax))), tuple(map(tuple, xyc * vmin))) circle_lst = [ list(map(list, np.flipud(xyc * vmax))), list(map(list, xyc * vmin)) ] #A default priocide must be defined for this CR method, otherwise it won't work with the predefined one if asas.priocode not in asas.strategy_dict: asas.priocode = "SRS1" # If no traffic if ntraf == 0: return # If only one aircraft elif ntraf == 1: # Map them into the format ARV wants. Outercircle CCW, innercircle CW asas.ARV[0] = circle_lst asas.ARV_3[0] = circle_lst asas.ARV_5[0] = circle_lst asas.ARV_min[0] = circle_lst asas.ARV_tla[0] = circle_lst asas.FRV[0] = [] asas.FRV_5[0] = [] asas.FRV_3[0] = [] asas.FRV_min[0] = [] asas.FRV_tla[0] = [] asas.FRV_dlos[0] = [] asas.ARV_calc[0] = circle_lst asas.ARV_calc_min[0] = circle_lst asas.ARV_calc_glb[0] = circle_lst asas.ARV_calc_dlos[0] = circle_lst asas.ARV_calc_dcpa[0] = circle_lst # Calculate areas and store in asas asas.FRV_area[0] = 0 asas.FRV_area_3[0] = 0 asas.FRV_area_5[0] = 0 asas.FRV_area_min[0] = 0 asas.FRV_area_tla[0] = 0 asas.FRV_area_dlos[0] = 0 asas.ARV_area[0] = np.pi * (vmax**2 - vmin**2) asas.ARV_area_5[0] = np.pi * (vmax**2 - vmin**2) asas.ARV_area_3[0] = np.pi * (vmax**2 - vmin**2) asas.ARV_area_min[0] = np.pi * (vmax**2 - vmin**2) asas.ARV_area_tla[0] = np.pi * (vmax**2 - vmin**2) asas.ARV_area_dlos[0] = np.pi * (vmax**2 - vmin**2) return # Function qdrdist_matrix needs 4 vectors as input (lat1,lon1,lat2,lon2) # To be efficient, calculate all qdr and dist in one function call # Example with ntraf = 5: ind1 = [0,0,0,0,1,1,1,2,2,3] # ind2 = [1,2,3,4,2,3,4,3,4,4] # This way the qdrdist is only calculated once between every aircraft # To get all combinations, use this function to get the indices ind1, ind2 = qdrdist_matrix_indices(ntraf) # Get absolute bearing [deg] and distance [nm] # Not sure abs/rel, but qdr is defined from [-180,180] deg, w.r.t. North [qdr, dist] = geo.qdrdist_matrix(lat[ind1], lon[ind1], lat[ind2], lon[ind2]) # Put result of function from matrix to ndarray qdr = np.reshape(np.array(qdr), np.shape(ind1)) dist = np.reshape(np.array(dist), np.shape(ind1)) # SI-units from [deg] to [rad] qdr = np.deg2rad(qdr) # Get distance from [nm] to [m] dist = dist * nm # In LoS the VO can't be defined, act as if dist is on edge dist[dist < hsepm] = hsepm # Calculate vertices of Velocity Obstacle (CCW) # These are still in relative velocity space, see derivation in appendix # Half-angle of the Velocity obstacle [rad] # Include safety margin alpha = np.arcsin(hsepm / dist) # Limit half-angle alpha to 89.982 deg. Ensures that VO can be constructed alpha[alpha > alpham] = alpham #Take the cosinus of alpha to calculate the maximum length of the VO's legs cosalpha = np.cos(alpha) # Consider every aircraft for i in range(ntraf): # Calculate SSD only for aircraft in conflict (See formulas appendix) if asas.inconf[i] == True: # SSD for aircraft i # Get indices that belong to aircraft i ind = np.where(np.logical_or(ind1 == i, ind2 == i))[0] # The i's of the other aircraft i_other = np.delete(np.arange(0, ntraf), i) # Aircraft that are within ADS-B range ac_adsb = np.where(dist[ind] < adsbmax)[0] # Now account for ADS-B range in indices of other aircraft (i_other) ind = ind[ac_adsb] i_other = i_other[ac_adsb] asas.inrange[i] = i_other # VO from 2 to 1 is mirror of 1 to 2. Only 1 to 2 can be constructed in # this manner, so need a correction vector that will mirror the VO fix = np.ones(np.shape(i_other)) fix[i_other < i] = -1 drel_x, drel_y = fix * dist[ind] * np.sin( qdr[ind]), fix * dist[ind] * np.cos(qdr[ind]) drel = np.dstack((drel_x, drel_y)) cosalpha_i = cosalpha[ind] # Make a clipper object pc = pyclipper.Pyclipper() pc_5 = pyclipper.Pyclipper() #NEWWW pc_3 = pyclipper.Pyclipper() #NEWWW pc_min = pyclipper.Pyclipper() #NEWWW pc_tla = pyclipper.Pyclipper() #NEWWW pc_calc = pyclipper.Pyclipper() #NEWWW pc_calc_min = pyclipper.Pyclipper() #NEWWW pc_calc_global = pyclipper.Pyclipper() #NEWWW pc_calc_dlos = pyclipper.Pyclipper() #NEWWW pc_calc_dcpa = pyclipper.Pyclipper() #NEWWW # Add circles (ring-shape) to clipper as subject pc.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) pc_5.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) pc_3.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) pc_min.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) pc_tla.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) pc_calc.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) pc_calc_min.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) pc_calc_global.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) pc_calc_dlos.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) pc_calc_dcpa.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) #To analyze current conflicts only it is more pratical #to take indexes of the returning variables of the CD method: asas.confpairs, asas.dist, etc.. #Conflict pairs and intruders within tla ind_current = [ index for index, item in enumerate(asas.confpairs) if item[0] == traf.id[i] ] #original indexs conf_pairs = [ list(item) for item in asas.confpairs if item[0] == traf.id[i] ] inconf_with = np.array([item[1] for item in conf_pairs]) #Minimum time to LoS and intruders at minimum time to LoS +1 min threshold min_tlos = np.around(min(asas.tLOS[ind_current]), decimals=0) ind_mintlos = [ index for index, item in enumerate(asas.tLOS[ind_current]) if item <= min_tlos + 60 ] #non original indexes ids_min = inconf_with[ind_mintlos] #Minimum distance to LoS and intruders at that distance dlos = asas.tLOS[ind_current] * asas.vrel[ind_current] min_dlos = min(dlos) ind_mindlos = [ index for index, item in enumerate(dlos) if item <= min_dlos + 60 ] #the threshold is only to make sure computational errors don't mess this ids_dlos = inconf_with[ind_mindlos] #Minimum distance at CPA min_dcpa2 = np.around(min(asas.dcpa2[ind_current]), decimals=0) ind_mindcpa = [ index for index, item in enumerate(asas.dcpa2[ind_current]) if item <= min_dcpa2 + 60 ] #threshold for safety only ids_dcpa = inconf_with[ind_mindcpa] """ #Debug prints print("Confpairs",conf_pairs) print("In Conflict with",inconf_with) print("minimum time to los",min_tlos) print("mintlos indexes", ind_mintlos) print("ids min", ids_min) print("ids dlos", ids_dlos) print("ids dcpa", ids_dcpa) """ # Add each other other aircraft to clipper as clip for j in range(np.shape(i_other)[0]): ## Debug prints ##print(traf.id[i] + " - " + traf.id[i_other[j]]) ## print(dist[ind[j]]) # Scale VO when not in LOS if dist[ind[j]] > hsepm: #Make an auxiliary pyclipper object for resolution purposes pc_aux = pyclipper.Pyclipper() #NEWWW #Add the area defined by the current velocity with a threshold of 5m/s pc_aux.AddPaths(pyclipper.scale_to_clipper(circle_tup), pyclipper.PT_SUBJECT, True) dist_mod = dist[ind[ j]] #the value (not array) of the distance is needed for future computations #direction of the VO's bisector nd = drel[0, j, :] / dist_mod R_pz = asas.R * asas.mar #R_pz = asas.R*1.1 R = np.array( [[np.sqrt(1 - (R_pz / dist_mod)**2), R_pz / dist_mod], [-R_pz / dist_mod, np.sqrt(1 - (R_pz / dist_mod)**2)]]) n_t1 = np.matmul(nd, R) #Direction of leg2 n_t2 = np.matmul(nd, np.transpose(R)) #Direction of leg1 #VO points v_other = [gseast[i_other[j]], gsnorth[i_other[j]]] legs_length = 10 * vmax / cosalpha_i[j] VO_points = np.array([ v_other, np.add(n_t2 * legs_length, v_other), np.add(n_t1 * legs_length, v_other) ]) #take only the farthest 2 vertices of the VO and make a tupple vertexes = tuple(map(tuple, VO_points[1:, :])) # Normally VO shall be added of this other a/c VO = pyclipper.scale_to_clipper( tuple(map(tuple, VO_points))) """ #Define cuts First cut: @5mins to LoS Second cut: @3mins to LoS Third cut : @min time to LoS Fourth cut : @Look-ahead time """ #========== #First cut #========== tau = 5 * 60 #5min to LoS VO_5, _ = roundoff(tau, R_pz, dist_mod, VO_points[0, :], nd, n_t1, n_t2, vertexes, xyc) pc_5.AddPath(pyclipper.scale_to_clipper(VO_5), pyclipper.PT_CLIP, True) #========== #Second cut #========== tau = 3 * 60 #3min for LoS VO_3, _ = roundoff(tau, R_pz, dist_mod, VO_points[0, :], nd, n_t1, n_t2, vertexes, xyc) pc_3.AddPath(pyclipper.scale_to_clipper(VO_3), pyclipper.PT_CLIP, True) #========== #Third cut #========== if np.around(min_tlos, decimals=0) != 0: tau = min_tlos + 60 #closest conflict with a threshold one minute v = [gseast[i], gsnorth[i]] VO_min, leg_points = roundoff(tau, R_pz, dist_mod, VO_points[0, :], nd, n_t1, n_t2, vertexes, xyc) if pyclipper.PointInPolygon( pyclipper.scale_to_clipper( (gseast[i], gsnorth[i])), pyclipper.scale_to_clipper(VO_min)): v = [gseast[i], gsnorth[i]] leg_points.insert(1, v) pc_min.AddPath(pyclipper.scale_to_clipper(VO_min), pyclipper.PT_CLIP, True) #Current conflicts at mintlos if traf.id[i_other[j]] in ids_min: pc_calc_min.AddPath(VO, pyclipper.PT_CLIP, True) #========== #Fourth cut- at tLA (for resolution purposes only) #========== #For global resolution @tla considering all VOs if asas.dtlookahead > 0: #Make cut at tla with a threshold of 1 min tau = asas.dtlookahead + 60 v = [gseast[i], gsnorth[i]] VO_tla, leg_points = roundoff(tau, R_pz, dist_mod, VO_points[0, :], nd, n_t1, n_t2, vertexes, xyc) if pyclipper.PointInPolygon( pyclipper.scale_to_clipper( (gseast[i], gsnorth[i])), pyclipper.scale_to_clipper(VO_tla)): v = [gseast[i], gsnorth[i]] leg_points.insert(1, v) pc_calc_global.AddPath( pyclipper.scale_to_clipper(leg_points), pyclipper.PT_CLIP, True) pc_tla.AddPath(pyclipper.scale_to_clipper(VO_tla), pyclipper.PT_CLIP, True) if traf.id[i_other[j]] in inconf_with: pc_calc.AddPath(VO, pyclipper.PT_CLIP, True) #====================================================== #Selection of conflicts based on distance to LoS #====================================================== if traf.id[i_other[j]] in ids_dlos: #previous:with VO, with leg_points pc_calc_dlos.AddPath(VO, pyclipper.PT_CLIP, True) #====================================================== #Selection of conflicts based on distance at CPA #====================================================== if traf.id[i_other[j]] in ids_dcpa: pc_calc_dcpa.AddPath(VO, pyclipper.PT_CLIP, True) else: # Pair is in LOS asas.los[i] = True #Instead of triangular VO, use darttip # Check if bearing should be mirrored if i_other[j] < i: qdr_los = qdr[ind[j]] + np.pi else: qdr_los = qdr[ind[j]] # Length of inner-leg of darttip leg = 1.1 * vmax / np.cos(beta) * np.array([1, 1, 1, 0]) # Angles of darttip angles_los = np.array( [qdr_los + 2 * beta, qdr_los, qdr_los - 2 * beta, 0.]) # Calculate coordinates (CCW) x_los = leg * np.sin(angles_los) y_los = leg * np.cos(angles_los) # Put in array of correct format xy_los = np.vstack((x_los, y_los)).T # Scale darttip VO = pyclipper.scale_to_clipper(tuple(map(tuple, xy_los))) #Store into the calculation object #pc_calc.AddPath(VO, pyclipper.PT_CLIP, True) # Add scaled VO to clipper pc.AddPath(VO, pyclipper.PT_CLIP, True) #(end of the for cycle) # Execute clipper command FRV = pyclipper.scale_from_clipper( pc.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) ARV = pc.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO) # Scale back ARV = pyclipper.scale_from_clipper(ARV) #5mins layer (cuts @5mins) FRV_5 = pyclipper.scale_from_clipper( pc_5.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) ARV_5 = pyclipper.scale_from_clipper( pc_5.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) #3mins layer (cuts @3mins) FRV_3 = pyclipper.scale_from_clipper( pc_3.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) ARV_3 = pyclipper.scale_from_clipper( pc_3.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) #mintlos layer (cuts @tlos) FRV_min = pyclipper.scale_from_clipper( pc_min.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) ARV_min = pyclipper.scale_from_clipper( pc_min.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) #Special cuts for calculation purposes ARV_calc_min = pyclipper.scale_from_clipper( pc_calc_min.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) #cuts @tla FRV_tla = pyclipper.scale_from_clipper( pc_tla.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) ARV_tla = pyclipper.scale_from_clipper( pc_tla.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) #cuts @tla for computation purposes ARV_tla_glb = pyclipper.scale_from_clipper( pc_calc_global.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) #Current conflicts within tla (take the full layer of VO) ARV_calc_tla = pyclipper.scale_from_clipper( pc_calc.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) #dlos layer (takes current conflicts at minimum dlos) FRV_dlos = pyclipper.scale_from_clipper( pc_calc_dlos.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) ARV_dlos = pyclipper.scale_from_clipper( pc_calc_dlos.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) #dcpa layer ARV_dcpa = pyclipper.scale_from_clipper( pc_calc_dcpa.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_NONZERO, pyclipper.PFT_NONZERO)) # Check multi exteriors, if this layer is not a list, it means it has no exteriors # In that case, make it a list, such that its format is consistent with further code if len(ARV) == 0: ARV_loc[i] = [] FRV_loc[i] = circle_lst # Calculate areas and store in asas FRV_area_loc[i] = np.pi * (vmax**2 - vmin**2) ARV_area_loc[i] = 0 if len(ARV_min) == 0: ARV_loc_min[i] = [] ARV_area_loc_min[i] = 0 ARV_calc_locmin[i] = [] else: if not type(ARV_min[0][0]) == list: ARV_min = [ARV_min] ARV_loc_min[i] = ARV_min ARV_area_loc_min[i] = area(ARV_min) if len(ARV_tla) == 0: ARV_loc_tla[i] = [] ARV_area_loc_tla[i] = 0 else: if not type(ARV_tla[0][0]) == list: ARV_tla = [ARV_tla] ARV_loc_tla[i] = ARV_tla ARV_area_loc_tla[i] = area(ARV_tla) if len(ARV_dlos) == 0: ARV_calc_loc_dlos[i] = [] ARV_area_loc_dlos[i] = 0 else: if not type(ARV_dlos[0][0]) == list: ARV_dlos = [ARV_dlos] ARV_calc_loc_dlos[i] = ARV_dlos ARV_area_loc_dlos[i] = area(ARV_dlos) if len(ARV_dcpa) == 0: ARV_calc_loc_dcpa[i] = [] ARV_area_loc_dcpa[i] = 0 else: if not type(ARV_dcpa[0][0]) == list: ARV_dcpa = [ARV_dcpa] ARV_calc_loc_dcpa[i] = ARV_dcpa ARV_area_loc_dcpa[i] = area(ARV_dcpa) if len(ARV_calc_tla) == 0: ARV_calc_loc[i] = [] else: if not type(ARV_calc_tla[0][0]) == list: ARV_calc_tla = [ARV_calc_tla] ARV_calc_loc[i] = ARV_calc_tla if len(ARV_tla_glb) == 0: ARV_calc_loc_glb[i] = [] else: if not type(ARV_tla_glb[0][0]) == list: ARV_tla_glb = [ARV_tla_glb] ARV_calc_loc_glb[i] = ARV_tla_glb if len(ARV_calc_min) == 0: ARV_calc_locmin[i] = [] else: if not type(ARV_calc_min[0][0]) == list: ARV_calc_min = [ARV_calc_min] ARV_calc_locmin[i] = ARV_calc_min else: #Then: ARV_calc_loc[i] = ARV_calc_tla ARV_calc_loc_glb[i] = ARV_tla_glb ARV_calc_locmin[i] = ARV_calc_min #Then/Except: if FRV_5: #Calcular tambem ARVs if not type(ARV_5[0][0]) == list: ARV_5 = [ARV_5] ARV_loc_5[i] = ARV_5 ARV_area_loc_5[i] = area(ARV_5) if not type(FRV_5[0][0]) == list: FRV_5 = [FRV_5] FRV_loc_5[i] = FRV_5 FRV_area_loc_5[i] = area(FRV_5) else: FRV_loc_5[i] = [] FRV_area_loc_5[i] = 0 ARV_loc_5[i] = circle_lst ARV_area_loc_5[i] = np.pi * (vmax**2 - vmin**2) if FRV_3: if not type(ARV_3[0][0]) == list: ARV_3 = [ARV_3] ARV_loc_3[i] = ARV_3 ARV_area_loc_3[i] = area(ARV_3) if not type(FRV_3[0][0]) == list: FRV_3 = [FRV_3] FRV_loc_3[i] = FRV_3 FRV_area_loc_3[i] = area(FRV_3) else: FRV_loc_3[i] = [] FRV_area_loc_3[i] = 0 ARV_loc_3[i] = circle_lst ARV_area_loc_3[i] = np.pi * (vmax**2 - vmin**2) if FRV_min: if not type(ARV_min[0][0]) == list: ARV_min = [ARV_min] ARV_loc_min[i] = ARV_min ARV_area_loc_min[i] = area(ARV_min) if not type(FRV_min[0][0]) == list: FRV_min = [FRV_min] FRV_area_loc_min[i] = area(FRV_min) FRV_loc_min[i] = FRV_min else: FRV_loc_min[i] = [] FRV_area_loc_min[i] = 0 ARV_loc_min[i] = circle_lst ARV_area_loc_min[i] = np.pi * (vmax**2 - vmin**2) ARV_calc_locmin[i] = circle_lst if FRV_tla: if not type(ARV_tla[0][0]) == list: ARV_tla = [ARV_tla] ARV_loc_tla[i] = ARV_tla ARV_area_loc_tla[i] = area(ARV_tla) if not type(FRV_tla[0][0]) == list: FRV_tla = [FRV_tla] FRV_area_loc_tla[i] = area(FRV_tla) FRV_loc_tla[i] = FRV_tla else: FRV_loc_tla[i] = [] FRV_area_loc_tla[i] = 0 ARV_loc_tla[i] = circle_lst ARV_area_loc_tla[i] = np.pi * (vmax**2 - vmin**2) ARV_calc_loc[i] = circle_lst ARV_calc_loc_glb[i] = circle_lst if FRV_dlos: if not type(ARV_dlos[0][0]) == list: ARV_dlos = [ARV_dlos] ARV_calc_loc_dlos[i] = ARV_dlos ARV_area_loc_dlos[i] = area(ARV_dlos) if not type(FRV_dlos[0][0]) == list: FRV_dlos = [FRV_dlos] FRV_area_loc_dlos[i] = area(FRV_dlos) FRV_loc_dlos[i] = FRV_dlos else: FRV_loc_dlos[i] = [] FRV_area_loc_dlos[i] = 0 ARV_calc_loc_dlos[i] = circle_lst ARV_area_loc_dlos[i] = np.pi * (vmax**2 - vmin**2) if not type(FRV[0][0]) == list: FRV = [FRV] if not type(ARV[0][0]) == list: ARV = [ARV] if not type(ARV_dcpa[0][0]) == list: ARV_dcpa = [ARV_dcpa] # Store in asas FRV_loc[i] = FRV ARV_loc[i] = ARV ARV_calc_loc_dcpa[i] = ARV_dcpa # Calculate areas and store in asas FRV_area_loc[i] = area(FRV) ARV_area_loc[i] = area(ARV) ARV_area_loc_dcpa[i] = area(ARV_dcpa) #Storing the results into asas asas.FRV = FRV_loc asas.FRV_5 = FRV_loc_5 asas.FRV_3 = FRV_loc_3 asas.FRV_min = FRV_loc_min asas.FRV_tla = FRV_loc_tla asas.FRV_dlos = FRV_loc_dlos asas.ARV = ARV_loc asas.ARV_3 = ARV_loc_3 asas.ARV_5 = ARV_loc_5 asas.ARV_min = ARV_loc_min asas.ARV_tla = ARV_loc_tla asas.ARV_calc = ARV_calc_loc asas.ARV_calc_min = ARV_calc_locmin asas.ARV_calc_glb = ARV_calc_loc_glb asas.ARV_calc_dlos = ARV_calc_loc_dlos asas.ARV_calc_dcpa = ARV_calc_loc_dcpa asas.FRV_area = FRV_area_loc asas.FRV_area_5 = FRV_area_loc_5 asas.FRV_area_3 = FRV_area_loc_3 asas.FRV_area_min = FRV_area_loc_min asas.FRV_area_tla = FRV_area_loc_tla asas.FRV_area_dlos = FRV_area_loc_dlos asas.ARV_area = ARV_area_loc asas.ARV_area_5 = ARV_area_loc_5 asas.ARV_area_3 = ARV_area_loc_3 asas.ARV_area_min = ARV_area_loc_min asas.ARV_area_tla = ARV_area_loc_tla asas.ARV_area_dlos = ARV_area_loc_dlos asas.ARV_area_dcpa = ARV_area_loc_dcpa #The layers list asas.layers = [ None, asas.ARV, asas.ARV_tla, asas.ARV_calc, asas.ARV_min, asas.ARV_calc_min, asas.ARV_calc_dlos, asas.ARV_calc_dcpa ] #asas.layers = [None, asas.ARV, asas.ARV_tla, asas.ARV_min, asas.ARV_calc_min, asas.ARV_calc_dlos, asas.ARV_calc_dcpa] return
def point_inside(self, polygon): return pyclipper.PointInPolygon(self.position, polygon) != 0
def point_inside(self, polygon): return pyclipper.PointInPolygon(self.midpoint, polygon) != 0
def Execute(op, obj, feat_num, feat, p_scale_factor): global toolGeometry global optimalCutAreaPerDist global iteration_limit_count global total_point_count global total_iteration_count global topZ global toolRadiusScaled global output_point_count global cache_hit_count global cache_pot_count global cache global cp global of global scale_factor scale_factor = p_scale_factor if RELOAD_MODULES: reload(Interpolation) reload(EngagementPoint) reload(GeomUtils) toolDiaScaled = op.tool.Diameter * scale_factor toolRadiusScaled = toolDiaScaled / 2 perf_start_time = time.time() perf_total_len = 0.0 print "toolRadiusScaled: ", toolRadiusScaled helixDiameter = min( op.tool.Diameter, 1000.0 if obj.HelixDiameterLimit.Value == 0.0 else obj.HelixDiameterLimit.Value) print "Helix diameter: ", helixDiameter helixDiameterScaled = helixDiameter * scale_factor stepOver = 0.01 * obj.StepOver # percentage to factor stepOverDistanceScaled = toolDiaScaled * stepOver finishPassOffset = 1.0 * obj.Tolerance / 2 cache_hit_count = 0 cache_pot_count = 0 cache = {} output_point_count = 0 topZ = op.stock.Shape.BoundBox.ZMax #initialization of = pyclipper.PyclipperOffset() cp = pyclipper.Pyclipper() toleranceScaled = int(obj.Tolerance * scale_factor) stepScaled = 10 toolGeometry = genToolGeometry(toolRadiusScaled + 1) # intitalization = calculate slot cutting area per step i.e. 100% step over sceneInit(toolRadiusScaled, topZ, scale_factor) distScaled = toolRadiusScaled / 2 slotStep = translatePath(toolGeometry, [0, distScaled]) cp.Clear() cp.AddPath(toolGeometry, pyclipper.PT_SUBJECT, True) cp.AddPath(slotStep, pyclipper.PT_CLIP, True) crossing = cp.Execute(pyclipper.CT_DIFFERENCE, pyclipper.PFT_EVENODD, pyclipper.PFT_EVENODD) referenceCutArea = pyclipper.Area(crossing[0]) fullSlotAreaPerDist = referenceCutArea / distScaled bound_extend = int(toolDiaScaled) #optimalCutAreaPerDistance (in scaled units) optimalCutAreaPerDist = stepOver * fullSlotAreaPerDist print "Optimal APD: ", optimalCutAreaPerDist, " Clipper scale factor: ", scale_factor, " Tolerance: ", obj.Tolerance, " Tolerance scaled: ", toleranceScaled try: # find cut region toolpath polygons of.Clear() of.AddPaths(feat, pyclipper.JT_ROUND, pyclipper.ET_CLOSEDPOLYGON) cut_region_tp = of.Execute(-int(finishPassOffset * scale_factor) - toolRadiusScaled) cut_region_tp_polytree = of.Execute2( -int(finishPassOffset * scale_factor) - toolRadiusScaled) cut_region_tp = pyclipper.CleanPolygons(cut_region_tp) sceneDrawPaths("BOUNDARY", cut_region_tp, scale_factor, (1, 0, 0), True) cleared, startPoint = findStartPoint(op, feat_num, cut_region_tp, cut_region_tp_polytree, helixDiameterScaled / 2, toolRadiusScaled, scale_factor) if cleared == None: return [], [0, 0] cleared = pyclipper.CleanPolygons(cleared) toolPos = startPoint of.Clear() of.AddPath([startPoint, startPoint], pyclipper.JT_ROUND, pyclipper.ET_OPENROUND) helixTp = of.Execute(helixDiameterScaled / 2) sceneDrawPaths("TOOLPATH", helixTp, scale_factor, (0, 0, 1), True) EngagementPoint.Initialize(cut_region_tp, toolRadiusScaled, stepScaled, scale_factor) total_point_count = 1 total_iteration_count = 0 iteration_limit_count = 0 toolPaths = [] no_cut_count = 0 over_cut_count = 0 toolPos = [startPoint[0], startPoint[1] - helixDiameterScaled / 2] toolDir = [1.0, 0.0] firstEngagePoint = True last_tool_gui_update = 0 pas = 0 while True: pas = pas + 1 #print "pass:"******"finding next pass engange point... pas:"******"Pass: %d\n"%pas) if toolPos == None: Console.PrintMessage("next engage point not found\n") break #nothing more to cut passToolPath = [] toClearPath = [] cumulativeCuttingArea = 0 no_cut_count = 0 reachedBoundaryPoint = None isOutside = False gyro = [toolDir] * 5 toolDir = GeomUtils.normalize(GeomUtils.sumv(gyro)) if SHOW_MARKERS: sceneClearPaths("ENGAGE") sceneDrawToolDir("ENGAGE", toolPos, toolDir, scale_factor, (0, 1, 0)) engagePoint = toolPos #iteration through the points on path/pass i = 0 deflectionAngle = math.pi #init del deflectionAngleHistory[:] distToBoundary = 0 while True: #points i = i + 1 sceneUpdateGui() if obj.StopProcessing: break #Console.PrintMessage("findNextPoint: %d =================================================================\n"%i) total_point_count = total_point_count + 1 toolDir = GeomUtils.normalize(GeomUtils.sumv(gyro)) #distance to the boundary line #if distToBoundary<toolRadiusScaled: clp, distToBoundary = GeomUtils.getClosestPointOnPaths( cut_region_tp, toolPos) distToEngagePoint = GeomUtils.magnitude( GeomUtils.sub2v(toolPos, engagePoint)) relDistToBoundary = 2.0 * distToBoundary / toolRadiusScaled minCutAreaPerDist = optimalCutAreaPerDist / 3 + 1 #if we are away from end boundary line, try making the optimal cut if relDistToBoundary > 1 or distToEngagePoint < toolRadiusScaled: targetArea = optimalCutAreaPerDist else: #decrease the cut area if we are close to boundary, adds a little bit of smoothing to the end of cut targetArea = relDistToBoundary * \ (optimalCutAreaPerDist-minCutAreaPerDist) + minCutAreaPerDist if distToBoundary < toolRadiusScaled or distToEngagePoint < toolRadiusScaled: stepScaled = toleranceScaled * 2 elif math.fabs(deflectionAngle) > 0.0001: stepScaled = 4.0 / deflectionAngle else: stepScaled = toleranceScaled * 4 if stepScaled < toleranceScaled * 2: stepScaled = toleranceScaled * 2 if stepScaled > toolRadiusScaled / 2: stepScaled = toolRadiusScaled / 2 # # Find next point with optimal cut # bound_box = [ [toolPos[0] - bound_extend, toolPos[1] - bound_extend], [toolPos[0] + bound_extend, toolPos[1] - bound_extend], [toolPos[0] + bound_extend, toolPos[1] + bound_extend], [toolPos[0] - bound_extend, toolPos[1] + bound_extend] ] cp.Clear() cp.AddPath(bound_box, pyclipper.PT_SUBJECT, True) cp.AddPaths(cleared, pyclipper.PT_CLIP, True) cleared_bounded = cp.Execute(pyclipper.CT_INTERSECTION, pyclipper.PFT_EVENODD, pyclipper.PFT_EVENODD) if not GeomUtils.anyValidPath(cleared_bounded): cleared_bounded = cleared newToolPos, newToolDir, cuttingAreaPerDist, cuttingArea, deflectionAngle = findNextPoint( obj, op, of, cp, cleared_bounded, toolPos, toolDir, toolRadiusScaled, stepScaled, targetArea, scale_factor) # # CHECK if we reached the the boundary # if distToBoundary < toolRadiusScaled and isOutsideCutRegion( newToolPos, cut_region_tp_polytree, True): isOutside = True reachedBoundaryPoint = GeomUtils.getIntersectionPointLWP( [toolPos, newToolPos], cut_region_tp) if reachedBoundaryPoint != None: #print "reachedBoundaryPoint:", reachedBoundaryPoint #showTool("TL", reachedBoundaryPoint, scale_factor, (1, 0, 0)) newToolPos = reachedBoundaryPoint else: newToolPos = toolPos cuttingArea, cuttingAreaPerDist = calcCutingArea( toolPos, newToolPos, toolRadiusScaled, cleared_bounded) if cuttingArea > 3 * cuttingAreaPerDist + 10 and cuttingAreaPerDist > 2 * optimalCutAreaPerDist + 10: over_cut_count = over_cut_count + 1 Console.PrintMessage("Break: over cut (%d %f/%f)\n" % (over_cut_count, cuttingAreaPerDist, optimalCutAreaPerDist)) break else: over_cut_count = 0 if len(toClearPath) == 0: toClearPath.append(toolPos) toClearPath.append(newToolPos) if firstEngagePoint: if len(toClearPath) > 10: of.Clear() of.AddPath(toClearPath, pyclipper.JT_ROUND, pyclipper.ET_OPENROUND) toolCoverArea = of.Execute(toolRadiusScaled + 1)[0] cleared = expandClearedArea(cp, toolCoverArea, cleared) toClearPath = [] if cuttingArea > 0: # cut is OK, record it cumulativeCuttingArea = cumulativeCuttingArea + cuttingArea if time.time() - last_tool_gui_update > GUI_UPDATE_PERIOD: if SHOW_MARKERS: sceneClearPaths("TP") sceneDrawFilledTool("TP", newToolPos, scale_factor, (0, 0, 1)) sceneDrawToolDir("TP", newToolPos, newToolDir, scale_factor, (0, 0, 1)) sceneClearPaths("PTP") sceneDrawPath("PTP", passToolPath, scale_factor, (0, 0, 1)) # sceneClearPaths("CL_BOUNDED") # sceneDrawPaths("CL_BOUNDED", cleared_bounded,scale_factor,(1,1,0),True) last_tool_gui_update = time.time() #append next point to toolpath perf_total_len = perf_total_len + stepScaled if i == 0: passToolPath.append( toolPos) #append first point to toolpath passToolPath.append(newToolPos) toolPos = newToolPos gyro.append(newToolDir) gyro.pop(0) no_cut_count = 0 else: #no cut #print "no cut:", no_cut_count no_cut_count = no_cut_count + 1 newToolDir = toolDir break # if no_cut_count > 1: # print "break: no cut" # break if isOutside: # next valid cut not found #print "Breaking: reached boundary" break #distToBoundary = distToBoundary-stepScaled #END OF POINTS INTERATION #expand cleared area if len(toClearPath) > 0: of.Clear() of.AddPath(toClearPath, pyclipper.JT_ROUND, pyclipper.ET_OPENROUND) toolCoverArea = of.Execute(toolRadiusScaled + 1)[0] cleared = expandClearedArea(cp, toolCoverArea, cleared) toClearPath = [] if cumulativeCuttingArea > stepScaled * stepOver * referenceCutArea / 40: #did we cut something significant? sceneClearPaths("PTP") sceneDrawPath("TOOLPATH", passToolPath, scale_factor) passToolPath = GeomUtils.cleanPath(passToolPath, CLEAN_PATH_TOLERANCE) appendToolPathCheckCollision(of, cp, toolPaths, passToolPath, toolRadiusScaled, cleared) if over_cut_count > 5: Console.PrintError( "Break: WARNING: resulting toolpath may be incomplete! (Hint: try changing numeric precision or StepOver)\n" ) break #FIND NEXT ENGAGING POINT if firstEngagePoint: EngagementPoint.moveToClosestPoint(newToolPos) firstEngagePoint = False else: moveDistance = stepOverDistanceScaled / 4 + 1 if not EngagementPoint.moveForwardToCutThreshold( cleared, moveDistance, moveDistance * 2, 1.5 * optimalCutAreaPerDist * moveDistance): break #clear around the engagement point toolPos, toolDir = EngagementPoint.getCurrentPoint() performance = 1.0 * perf_total_len / scale_factor / ( time.time() - perf_start_time) # mm/s Console.PrintMessage( "Passes: %s, Toolpaths: %d, Output Points: %d, Processed Points: %d, Avg.Iterations: %f, Exceeded: %d, Perf.: %f mm/s, Cut shape cache hit: %d/%d (%f perc.)\n" % (pas, len(toolPaths), output_point_count, total_point_count, 1.0 * total_iteration_count / total_point_count, iteration_limit_count, performance, cache_hit_count, cache_pot_count, 100 * cache_hit_count / (cache_pot_count + 0.1))) #generate finishing pass sceneUpdateGui() passToolPath = [] of.Clear() of.AddPaths(feat, pyclipper.JT_ROUND, pyclipper.ET_CLOSEDPOLYGON) finishing = of.Execute2(-toolRadiusScaled) # add only paths containing the startPoint and their childs for child in finishing.Childs: if pyclipper.PointInPolygon(startPoint, child.Contour) != 0: appendToolPathCheckCollision( of, cp, toolPaths, GeomUtils.cleanPath(child.Contour, CLEAN_PATH_TOLERANCE / 2), toolRadiusScaled, cleared, True) for hole in child.Childs: appendToolPathCheckCollision( of, cp, toolPaths, GeomUtils.cleanPath(hole.Contour, CLEAN_PATH_TOLERANCE / 2), toolRadiusScaled, cleared, True) # append the return to the initial point (and check collision) if len(toolPaths) > 0 and len(toolPaths[0]) > 0: appendToolPathCheckCollision( of, cp, toolPaths, [[toolPaths[0][0][0], toolPaths[0][0][1]]], toolRadiusScaled, cleared, True) except: sceneClean() raise sceneClean() return toolPaths, startPoint