def quadratic(self, start_point, end_point, cell_point):
# Given a segment going from start_point to end_point, find the points between which the segment is closest
# than the minimum separation distance to the cell point
# returns the interval expressed in term of fraction (between 0 and 1)
interval = []
length = np.linalg.norm(end_point - start_point)
side = start_point - cell_point
if length == 0:
# Agent is staying in place
if np.linalg.norm(side) <= self.minimum_separation_distance:
interval = [0, 1]
else:
u = (end_point - start_point) / length
b = np.dot(u, side)
# Adding slightly superior to 1 k-factor for numerical issues (i.e. make the interval slightly bigger)
c = np.dot(side,
side) - (1.0001 * self.minimum_separation_distance)**2
delta = b**2 - c
if delta > 0:
x1 = -b - math.sqrt(delta)
x2 = -b + math.sqrt(delta)
x1 = clamp(0, length, x1)
x2 = clamp(0, length, x2)
interval = [x1 / length, x2 / length]
return interval
def is_path_available(self, ownship_start_time, start_point, end_point,
end_time):
ownship_velocity = (end_point - start_point) / (end_time -
ownship_start_time)
for flight_plan in self.flight_plans.values():
trajectory, times = flight_plan.get_planned_trajectory_between(
ownship_start_time, end_time)
if trajectory is not None:
n = len(trajectory)
intruder_segment_start_pos = np.copy(trajectory[0])
intruder_segment_start_time = times[0]
analysis_start_time = max(ownship_start_time,
intruder_segment_start_time)
ownship_pos = start_point + (
analysis_start_time -
ownship_start_time) * ownship_velocity
for i in range(1, n):
intruder_segment_end_pos = trajectory[i]
intruder_segment_end_time = times[i]
if intruder_segment_end_time == intruder_segment_start_time:
print('trajectory ' + str(trajectory))
print('times ' + str(times))
print('ownship start time ' + str(ownship_start_time))
print('end time ' + str(end_time))
intruder_velocity = (intruder_segment_end_pos -
intruder_segment_start_pos) / (
intruder_segment_end_time -
intruder_segment_start_time)
intruder_pos = intruder_segment_start_pos + (
analysis_start_time -
intruder_segment_start_time) * intruder_velocity
# Let's compute the closest time of approach
delta_v = intruder_velocity - ownship_velocity
delta_v_squared = np.dot(delta_v, delta_v)
delta_p = intruder_pos - ownship_pos
if delta_v_squared != 0:
t_cpa = analysis_start_time - np.dot(
delta_v, delta_p) / delta_v_squared
# Let's find the time of closest approach on the segment
clamped_t_cpa = clamp(
analysis_start_time, intruder_segment_end_time,
t_cpa
) - analysis_start_time # between 0 and the end of the segment (t_elapsed)
else:
clamped_t_cpa = 0
closest = np.linalg.norm(intruder_pos + intruder_velocity *
clamped_t_cpa - ownship_pos -
ownship_velocity * clamped_t_cpa)
if closest < self.minimum_separation_distance:
return False
intruder_segment_start_time = intruder_segment_end_time
intruder_segment_start_pos = intruder_segment_end_pos
analysis_start_time = max(ownship_start_time,
intruder_segment_start_time)
ownship_pos = start_point + (
analysis_start_time -
ownship_start_time) * ownship_velocity
return True
def velocity_update(self, new_velocity):
# Introduce kinematic constraints
# For now just clamp the velocity and instantly change the orientation
v = np.linalg.norm(new_velocity)
v_clamped = my_utils.clamp(self.minSpeed, self.maxSpeed, v)
if self.agent_dynamics is None:
return new_velocity * v_clamped / v
else:
turn_angle = my_utils.get_angle(self.velocity, new_velocity)
max_angle = 30 * math.pi / 180
if abs(turn_angle) > max_angle:
vel = self.velocity * v_clamped / np.linalg.norm(self.velocity)
theta = math.copysign(max_angle, turn_angle)
return vel @ np.asarray(
[[math.cos(theta), math.sin(theta)],
[-math.sin(theta), math.cos(theta)]])
else:
return new_velocity * v_clamped / v
def is_path_available(self,
ownship_start_time,
start_point,
end_point,
end_time,
debug=False):
""" Assuming the ownship leaves the start_point at the start_time to finish at end_point and end_time, is there a conflict with other
flight plans ?
Returns available (Bool), intruder trajectory
Returns only the section of the intruder path on which the collision will occur => the time interval returned is included in [ownship_start_time, end_time]"""
# Naive approach
# Iterate over all flight plans, find all flight segments that overlap in time with the input time
# Compute for each of those segments the closest time of approach
# Need to intersect time between time_start and time_end and get the resulting trajectory
if debug:
print('is path available time interval ' +
str([ownship_start_time, end_time]))
ownship_velocity = (end_point - start_point) / (end_time -
ownship_start_time)
for flight_plan in self.flight_plans.values():
trajectory, times = flight_plan.get_planned_trajectory_between(
ownship_start_time, end_time)
if times is not None and len(times) >= 2 and abs(
times[0] - times[1]) < 0.0001:
print('there is an issue with get planned trajectory between')
print(trajectory)
print(times)
flight_plan.get_planned_trajectory_between(ownship_start_time,
end_time,
debug=True)
if trajectory is not None:
n = len(trajectory)
intruder_segment_start_pos = np.copy(trajectory[0])
intruder_segment_start_time = times[0]
analysis_start_time = max(ownship_start_time,
intruder_segment_start_time)
ownship_pos = start_point + (
analysis_start_time -
ownship_start_time) * ownship_velocity
for i in range(1, n):
intruder_segment_end_pos = trajectory[i]
intruder_segment_end_time = times[i]
intruder_velocity = (intruder_segment_end_pos -
intruder_segment_start_pos) / (
intruder_segment_end_time -
intruder_segment_start_time)
intruder_pos = intruder_segment_start_pos + (
analysis_start_time -
intruder_segment_start_time) * intruder_velocity
# Let's compute the closest time of approach
delta_v = intruder_velocity - ownship_velocity
delta_v_squared = np.dot(delta_v, delta_v)
delta_p = intruder_pos - ownship_pos
if delta_v_squared != 0:
t_cpa = analysis_start_time - np.dot(
delta_v, delta_p) / delta_v_squared
# Let's find the time of closest approach on the segment
clamped_t_cpa = clamp(
analysis_start_time, intruder_segment_end_time,
t_cpa
) - analysis_start_time # between 0 and the end of the segment (t_elapsed)
else:
# If ownship and intruder have the same velocity vector then the distance between them is constant
clamped_t_cpa = 0
closest = np.linalg.norm(intruder_pos + intruder_velocity *
clamped_t_cpa - ownship_pos -
ownship_velocity * clamped_t_cpa)
if closest < self.minimum_separation_distance:
if debug:
print('The closest point of approach is ' +
str(closest))
print('Total trajectory is ' + str(trajectory))
print('Total times is ' + str(times))
if debug and abs(times[i - 1] - times[i]) < 0.00001:
print('Problem with time')
if debug:
trajectory, times = flight_plan.get_planned_trajectory_between(
ownship_start_time, end_time, debug=True)
return False, [
trajectory[i - 1:i + 1], times[i - 1:i + 1]
]
intruder_segment_start_time = intruder_segment_end_time
intruder_segment_start_pos = intruder_segment_end_pos
analysis_start_time = max(ownship_start_time,
intruder_segment_start_time)
ownship_pos = start_point + (
analysis_start_time -
ownship_start_time) * ownship_velocity
return True, None
def get_time_to_leave(self,
P_o,
V_o,
P_i,
V_i,
max_time,
on_the_ground=False,
debug=False):
# returns None if a conflict is unavoidable between time=0 and max_time
# returns the value of a delay that allows to avoid the conflict
# returns max_time
delta_V = V_i - V_o
delta_P = P_i - P_o
delta_V_squared = np.dot(delta_V, delta_V)
# Initial distance between ownship and intruder
d_initial = np.linalg.norm(P_o - P_i)
# Time of closest approach if the ownship does not move
V_i_squared = np.dot(V_i, V_i)
# To avoid numerical issues, make the avoidance area slightly bigger
k_factor = 1.0001
if V_i_squared == 0:
# If the intruder is not moving there's no point in having a delay
# This function is only called if there is a conflict on the time segment
# Wait in place until the end of the segment
t_cpa_stop = 0
d_cpa_stop = d_initial
if d_initial < self.minimum_separation_distance:
return None
else:
return max_time
else:
t_cpa_stop = -np.dot(-V_i, P_o - P_i) / np.dot(V_i, V_i)
d_cpa_stop = np.linalg.norm(P_i + t_cpa_stop * V_i - P_o)
# Time of closest approach on the segment if the ownship does not move
clamped_t_cpa_stop = clamp(0, max_time, t_cpa_stop)
# Closest distance between the two agents on the segment if the ownship does not move (greater than d_cpa_stop)
closest_distance_stop = np.linalg.norm(P_i + clamped_t_cpa_stop * V_i -
P_o)
if delta_V_squared == 0:
a = -1
Delta = -1
else:
# Time of closest approach if the ownship moves without delay
t_cpa_no_delay = -np.dot(delta_V, delta_P) / delta_V_squared
d_cpa_no_delay = np.linalg.norm(delta_P + t_cpa_no_delay * delta_V)
Gamma = delta_P - delta_V * np.dot(delta_P,
delta_V / delta_V_squared)
Alpha = V_o - delta_V * (np.dot(delta_V, V_o) / delta_V_squared)
c = np.dot(Gamma, Gamma) - (k_factor *
self.minimum_separation_distance)**2
b = 2 * np.dot(Alpha, Gamma)
a = np.dot(Alpha, Alpha)
Delta = b**2 - 4 * a * c
if on_the_ground:
if Delta >= 0 and a != 0:
delay1 = (-b - math.sqrt(b**2 - 4 * a * c)) / (2 * a)
delay2 = (-b + math.sqrt(b**2 - 4 * a * c)) / (2 * a)
if delay1 >= 0:
t_cpa_with_delay = -np.dot(delta_P + V_o * delay1,
delta_V) / delta_V_squared
if delay1 <= t_cpa_with_delay:
return min(math.ceil(delay1 * 10) / 10, max_time)
else:
print(
'delay 1 greater than the time of closest approach found, no solution'
)
return None
else:
if delay2 < 0:
print(
'This should not happen if there is a conflict, delay2<0'
)
else:
t_cpa_with_delay = -np.dot(delta_P + V_o * delay2,
delta_V) / delta_V_squared
if delay2 < t_cpa_with_delay:
return min(math.ceil(delay2 * 10) / 10, max_time)
else:
# print('delay 2 greater than the time of closest approach found, no solution')
# print('P_o='+str(P_o))
# print('V_o=' + str(V_o))
# print('P_i=' + str(P_i))
# print('V_i=' + str(V_i))
# print('max_time='+str(max_time))
# print('on_the_ground='+str(on_the_ground))
a = np.dot(V_i, V_i)
b = 2 * np.dot(delta_P, V_i)
c = np.dot(delta_P, delta_P) - (
k_factor * self.minimum_separation_distance)**2
det = b**2 - 4 * a * c
if det >= 0:
delay3 = (-b + math.sqrt(b**2 - 4 * a * c)) / (
2 * a)
return min(
math.ceil(delay3 * 10) / 10, max_time)
else:
print('det<0, this should not happen')
return None
else:
# There is no delay that makes the closest point of approach be at that specific distance (i.e. if the agent was in the air there would be a conflict)
# Since you're on the ground find when the intruder will be far enough that you can take off
# TODO handle case when V_i is 0
a = np.dot(V_i, V_i)
b = 2 * np.dot(delta_P, V_i)
c = np.dot(delta_P, delta_P) - (
k_factor * self.minimum_separation_distance)**2
det = b**2 - 4 * a * c
if det >= 0:
delay3 = (-b + math.sqrt(b**2 - 4 * a * c)) / (2 * a)
return min(math.ceil(delay3 * 10) / 10, max_time)
else:
print('det<0, this should not happen')
return None
else:
# in the air
if debug:
print('Delta is ' + str(Delta))
if delta_V_squared != 0:
print('a is ' + str(a))
print('b is ' + str(b))
print('c is ' + str(c))
if Delta >= 0 and a != 0:
delay1 = (-b - math.sqrt(b**2 - 4 * a * c)) / (2 * a)
delay2 = (-b + math.sqrt(b**2 - 4 * a * c)) / (2 * a)
if delay1 >= 0:
if debug:
print('looking at delay 1')
# The equation is only valid if the delay is smaller than the time of closest approach found on the next line
t_cpa_with_delay = -np.dot(delta_P + V_o * delay1,
delta_V) / delta_V_squared
if delay1 <= t_cpa_with_delay:
if (0 <= t_cpa_stop < delay1 and
d_cpa_stop < self.minimum_separation_distance
) or d_initial < self.minimum_separation_distance:
# Sanity check
print('Cannot escape the conflict')
return None
else:
return min(math.ceil(delay1 * 10) / 10, max_time)
else:
print(
'delay 1 greater than the time of closest approach found, no solution'
)
return None
else:
if delay2 < 0:
print(
'This should not happen if there is a conflict, delay2<0'
)
else:
if debug:
print('looking at delay 2')
# The equation is only valid if the delay is smaller than the time of closest approach found on the next line
t_cpa_with_delay = -np.dot(delta_P + V_o * delay2,
delta_V) / delta_V_squared
if delay2 <= t_cpa_with_delay:
if (
0 <= t_cpa_stop < delay2 and d_cpa_stop <
self.minimum_separation_distance
) or d_initial < self.minimum_separation_distance:
return None
else:
return min(
math.ceil(delay2 * 10) / 10, max_time)
else:
# The solution delay2 is invalid
if closest_distance_stop <= self.minimum_separation_distance:
return None
else:
# Saved by the gong. Moving without delay will cause a conflict on the time segment.
# Waiting it out might not save you if the intruder keeps the same velocity, but if the intruder turns you have a chance
# Conflict is avoided on that time segment by waiting
return max_time
else:
# There is no delay that makes the closest point of approach be at that specific distance
# This means that even by staying in place the two agents come close
if closest_distance_stop < self.minimum_separation_distance:
return None
else:
# Saved by the gong
return max_time