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
