def distance_to_Delta_l(distance, psi_r, x_r, y_r, x, y):
psi_tmp = dy.atan2(y - y_r, x - x_r)
delta_angle = dy.unwrap_angle(psi_r - psi_tmp, normalize_around_zero=True)
sign = dy.conditional_overwrite(dy.float64(1.0),
delta_angle > dy.float64(0), -1.0)
Delta_l = distance * sign
return Delta_l
def _distance_to_Delta_l( distance, psi_r, x_r, y_r, x, y ):
"""
Add sign information to a closest distance measurement
"""
psi_tmp = dy.atan2(y - y_r, x - x_r)
delta_angle = dy.unwrap_angle( psi_r - psi_tmp, normalize_around_zero=True )
sign = dy.conditional_overwrite(dy.float64(1.0), delta_angle > dy.float64(0) , -1.0 )
Delta_l = distance * sign
return Delta_l
def distance_to_line(x_s, y_s, x_e, y_e, x_test, y_test):
"""
compute the shortest distance to a line
returns a negative distance in case (x_test, y_test) is to the left of the vector pointing from
(x_s, y_s) to (x_e, y_e)
"""
Delta_x = x_e - x_s
Delta_y = y_e - y_s
x_test_ = x_test - x_s
y_test_ = y_test - y_s
psi = dy.atan2(Delta_y, Delta_x)
test_ang = dy.atan2(y_test_, x_test_)
delta_angle = dy.unwrap_angle(test_ang - psi, normalize_around_zero=True)
length_s_to_test = dy.sqrt(x_test_ * x_test_ + y_test_ * y_test_)
distance = dy.sin(delta_angle) * length_s_to_test
distance_s_to_projection = dy.cos(delta_angle) * length_s_to_test
return distance, distance_s_to_projection
def sample_path_finite_difference(path, index):
"""
Compute path orientation angle form x/y data only using finite differences
"""
x1, y1 = sample_path_xy(path, index)
x2, y2 = sample_path_xy(path, index + 1)
Delta_x = x2 - x1
Delta_y = y2 - y1
psi_r = dy.atan2(Delta_y, Delta_x)
x_r = x1
y_r = y1
return x_r, y_r, psi_r
def sample_path_finite_difference(path, index):
y1 = dy.memory_read(memory=path['Y'], index=index)
y2 = dy.memory_read(memory=path['Y'], index=index + dy.int32(1))
x1 = dy.memory_read(memory=path['X'], index=index)
x2 = dy.memory_read(memory=path['X'], index=index + dy.int32(1))
Delta_x = x2 - x1
Delta_y = y2 - y1
psi_r = dy.atan2(Delta_y, Delta_x)
x_r = x1
y_r = y1
return x_r, y_r, psi_r