def scale_adjustment_fnc(qi, qj, dij_star):
k = 10
dij = norm_np(qj - qi)
# print(f"dij - dij_star: {dij - dij_star}.")
push_pull_fnc = 1 / k * atan(dij - dij_star)
return push_pull_fnc * (qj - qi)
def norm(vec):
"""Norm of a Quantity vector that respects units.
Parameters
----------
vec : ~astropy.units.Quantity
Vector with units.
"""
return norm_np(vec.value) * vec.unit
def find_dij_star(destination_fnc, n_agent):
dij_star_fnc = np.zeros((n_agent, n_agent))
for i in range(n_agent): # Go through all the agents
for j in range(n_agent): # Go through all the neighbors
agent = np.array(
[destination_fnc[i * 2], destination_fnc[i * 2 + 1]])
neighbor = np.array(
[destination_fnc[j * 2], destination_fnc[j * 2 + 1]])
dij_star_fnc[i][j] = norm_np(
np.array([destination_fnc[i * 2], destination_fnc[i * 2 +
1]]) -
np.array([destination_fnc[j * 2], destination_fnc[j * 2 + 1]]))
return dij_star_fnc
