def hdg_diff_next_wp(lat, lon, wpc, wpn):
hdg_ac_wpn = calc_compass_bearing((lat, lon), wpn)
hdg_wpc_wpn = calc_compass_bearing(wpc, wpn)
hdiff = heading_diff(hdg_ac_wpn, hdg_wpc_wpn)
return hdiff
def check_path_intersection(c1, c2, h1, h2):
alpha_1 = calc_compass_bearing(c1, c2) - h1
alpha_2 = calc_compass_bearing(c2, c1) - h2
if ((abs(alpha_1) + abs(alpha_2)) < 180) and (alpha_1 * alpha_2 < 0):
return True
else:
return False
def ac_dist_stoch(t, cte_1, ate_1, cte_2, ate_2, lat_1, lon_1, lat_2, lon_2,
hdg_1, hdg_2, spd_1, spd_2):
knots_to_ms = knts_ms_ratio
ipz_lim = ipz_range # meters
alpha_1 = heading_diff(
calc_compass_bearing((lat_1, lon_1), (lat_2, lon_2)), hdg_1)
alpha_2 = heading_diff(
calc_compass_bearing((lat_2, lon_2), (lat_1, lon_1)), hdg_2)
gamma = 180 - (alpha_1 + alpha_2)
if gamma < 0:
return np.nan
else:
dx1_e = math.sin(math.radians(heading_diff(0, hdg_1))) * ate_1 + \
math.cos(math.radians(heading_diff(0, hdg_1))) * cte_1
dy1_e = math.sin(math.radians(heading_diff(hdg_1, 0))) * cte_1 + \
math.cos(math.radians(heading_diff(hdg_1, 0))) * ate_1
dx2_e = math.sin(math.radians(heading_diff(0, hdg_2))) * ate_2 + \
math.cos(math.radians(heading_diff(0, hdg_2))) * cte_2
dy2_e = math.sin(math.radians(heading_diff(hdg_2, 0))) * cte_2 + \
math.cos(math.radians(heading_diff(hdg_2, 0))) * ate_2
dy_1 = math.cos(math.radians(heading_diff(hdg_1, 0))) * spd_1 * \
knots_to_ms + (dy1_e / t)
dy_2 = math.cos(math.radians(heading_diff(hdg_2, 0))) * spd_2 * \
knots_to_ms + (dy2_e / t)
dx_1 = math.sin(math.radians(heading_diff(hdg_1, 0))) * spd_1 * \
knots_to_ms + (dx1_e / t)
dx_2 = math.sin(math.radians(heading_diff(hdg_2, 0))) * spd_2 * \
knots_to_ms + (dx2_e / t)
dx = abs(dx_1 - dx_2) * t
dy = abs(dy_1 - dy_2) * t
s = calc_coord_dst((lat_1, lon_1), (lat_2, lon_2))
d = s - np.sqrt((dy**2 + dx**2))
return d
def gamma_angle(lat_1, lon_1, lat_2, lon_2, hdg_1, hdg_2):
deg_f1_f2 = calc_compass_bearing((lat_1, lon_1), (lat_2, lon_2))
alpha = (360 + (hdg_1 - deg_f1_f2)) % 360
beta = (360 + (hdg_2 - deg_f1_f2)) % 360
if alpha > beta > 180:
gamma = alpha - beta
alpha_mod = 360 - alpha
beta_mod = beta % 180
return gamma, alpha_mod, beta_mod
elif alpha < beta < 180:
gamma = beta - alpha
alpha_mod = alpha
beta_mod = 180 - beta
return gamma, alpha_mod, beta_mod
else:
return np.nan, np.nan, np.nan
def preprocess_intent_conflicts(batch):
res_batch_intent = []
res_batch_reg = []
for bi in batch:
b = {}
if bi:
b_tlen = max(bi['ts_1']) - min(bi['ts_1'])
if b_tlen < la_time:
continue
fl_keys = [
'ts', 'lat', 'lon', 'hdg', 'alt', 'spd', 'roc', 'ep_seg_b',
'lat_seg_b', 'lon_seg_b', 'lat_seg_e', 'lon_seg_e', 'fl_seg_b',
'fl_seg_e', 'seq'
]
try:
fl1 = {}
for k in fl_keys:
fl1[k] = bi["%s%s" % (k, '_1')]
try:
fl1 = crop_ddr2_flight_seg(fl1)
except Exception as e:
print('Cropping DDR2 flight failed, error: ')
print(e)
continue
if fl1 is None:
continue
try:
fl1 = add_waypoint_segments(fl1)
# fl1 = fl1[fl1.wp_seg.notnull()]
except Exception as e:
print('Adding waypoints to flight failed, error: ')
print(e)
continue
if fl1 is None:
continue
if len(fl1) == 0:
continue
fl2 = {}
for k in fl_keys:
fl2[k] = bi["%s%s" % (k, '_2')]
try:
fl2 = crop_ddr2_flight_seg(fl2)
except Exception as e:
print('Cropping DDR2 flight failed, error: ')
print(e)
continue
if fl2 is None:
continue
try:
fl2 = add_waypoint_segments(fl2)
# fl2 = fl2[fl2.wp_seg.notnull()]
except Exception as e:
print('Adding waypoints to flight failed, error: ')
print(e)
continue
if fl2 is None:
continue
if len(fl2) == 0:
continue
fl2_n = fl2[fl2.wp_seg.notnull() & fl1.wp_seg.notnull()]
fl1_n = fl1[fl2.wp_seg.notnull() & fl1.wp_seg.notnull()]
fl2_n = fl2_n.reset_index(drop=True)
fl1_n = fl1_n.reset_index(drop=True)
if len(fl2_n) == 0 or len(fl1_n) == 0:
continue
fl1_n['hdg_int'] = fl1_n.apply(lambda x: calc_compass_bearing(
(x['wp_seg'][2][0], x['wp_seg'][2][1]),
(x['wp_seg'][2][2], x['wp_seg'][2][3])),
axis=1)
fl2_n['hdg_int'] = fl2_n.apply(lambda x: calc_compass_bearing(
(x['wp_seg'][2][0], x['wp_seg'][2][1]),
(x['wp_seg'][2][2], x['wp_seg'][2][3])),
axis=1)
for k in ['ts', 'lat', 'lon', 'alt', 'hdg', 'spd', 'wp_seg']:
b['%s_1' % k] = fl1_n[k].tolist()
b['%s_2' % k] = fl2_n[k].tolist()
b['hdg_e_1'] = fl1_n['hdg_int'].tolist()
b['hdg_e_2'] = fl2_n['hdg_int'].tolist()
b['spd_e_1'] = fl1_n['spd'].tolist()
b['spd_e_2'] = fl2_n['spd'].tolist()
res_batch_intent.append(b)
b['hdg_e_1'] = fl1_n['hdg']
b['hdg_e_2'] = fl2_n['hdg']
b['spd_e_1'] = fl1_n['spd']
b['spd_e_2'] = fl2_n['spd']
res_batch_reg.append(b)
except Exception as e:
print('Preprocessing data failed, error:')
print(e)
continue
return (res_batch_intent, res_batch_reg)
def get_intent_ttc_est(lat_1, lon_1, lat_2, lon_2, hdg_1, hdg_2, spd_1, spd_2,
_tmax, _wps_1, _wps_2):
wps_seq_1 = sequentialize(_wps_1)
wps_seq_2 = sequentialize(_wps_2)
lat_n_1, lon_n_1, lat_n_2, lon_n_2 = lat_1, lon_1, lat_2, lon_2
try:
try:
if not len(wps_seq_1) < 2:
wp1_curr_iter = iter(wps_seq_1)
wp1_next_iter = iter(wps_seq_1[1:])
_wp1_curr = next(wp1_curr_iter, wps_seq_1[-2])
_wp1_next = next(wp1_next_iter, wps_seq_1[-1])
else:
wp1_curr_iter, wp1_next_iter, _wp1_curr, _wp1_next = \
None, None, None, None
if not len(wps_seq_2) < 2:
wp2_curr_iter = iter(wps_seq_2)
wp2_next_iter = iter(wps_seq_2[1:])
_wp2_curr = next(wp2_curr_iter, wps_seq_2[-2])
_wp2_next = next(wp2_next_iter, wps_seq_2[-1])
else:
wp2_curr_iter, wp2_next_iter, _wp2_curr, _wp2_next = \
None, None, None, None
except Exception as e1:
print('wp init failed')
print(e1)
for t in range(1, _tmax):
if t == 1:
lat_n_1, lon_n_1 = get_next_crd(lat_1, lon_1, hdg_1, spd_1)
lat_n_2, lon_n_2 = get_next_crd(lat_2, lon_2, hdg_2, spd_2)
else:
lat_n_1, lon_n_1 = get_next_crd(lat_n_1, lon_n_1, hdg_1, spd_1)
lat_n_2, lon_n_2 = get_next_crd(lat_n_2, lon_n_2, hdg_2, spd_2)
di = calc_coord_dst((lat_n_1, lon_n_1), (lat_n_2, lon_n_2))
if di < ipz_range:
return t
if not len(wps_seq_1) < 2:
_hdiff1 = hdg_diff_next_wp(lat_n_1, lon_n_1, _wp1_curr,
_wp1_next)
dst_currwp_1 = calc_coord_dst((lat_n_1, lon_n_1), _wp1_curr)
if abs(_hdiff1) < 5 and dst_currwp_1 < 8000:
hdg_1 = calc_compass_bearing(_wp1_curr, _wp1_next)
_wp1_curr = next(wp1_curr_iter, sequentialize(_wps_1)[-2])
_wp1_next = next(wp1_next_iter, sequentialize(_wps_1)[-1])
if not len(wps_seq_2) < 2:
_hdiff2 = hdg_diff_next_wp(lat_n_2, lon_n_2, _wp2_curr,
_wp2_next)
dst_currwp_2 = calc_coord_dst((lat_n_2, lon_n_2), _wp2_curr)
if abs(_hdiff2) < 5 and dst_currwp_2 < 8000:
hdg_2 = calc_compass_bearing(_wp2_curr, _wp2_next)
_wp2_curr = next(wp2_curr_iter, sequentialize(_wps_2)[-2])
_wp2_next = next(wp2_next_iter, sequentialize(_wps_2)[-1])
return np.nan
except Exception as e:
print('intent ttc func failed')
print(e)
return np.nan