def interaction_agent_agent_three_circle(i, j, agents):
"""Interaction between two three circle agents."""
# Positions: center, left, right
x_i = (agents[i]['position'], agents[i]['position_ls'],
agents[i]['position_rs'])
x_j = (agents[j]['position'], agents[j]['position_ls'],
agents[j]['position_rs'])
# Radii of torso and shoulders
r_i = (agents[i]['r_t'], agents[i]['r_s'], agents[i]['r_s'])
r_j = (agents[j]['r_t'], agents[j]['r_s'], agents[j]['r_s'])
h, n, r_moment_i, r_moment_j = distance_three_circles(x_i, r_i, x_j, r_j)
if h < SIGTH_SOC:
force_i, force_j = force_social_three_circle(agents, i, j)
if h < 0:
t = rotate270(n)
v = agents[i]['velocity'] - agents[j]['velocity']
force_i += force_contact(h, n, v, t, agents[i]['mu'],
agents[i]['kappa'], agents[i]['damping'])
force_j -= force_contact(h, n, v, t, agents[j]['mu'],
agents[j]['kappa'], agents[j]['damping'])
agents[i]['force'][:] += force_i
agents[j]['force'][:] += force_j
agents[i]['torque'] += cross(r_moment_i, force_i)
agents[j]['torque'] += cross(r_moment_j, force_j)
def interaction_agent_agent_circular(i, j, agents):
"""Interaction between two circular agents."""
h, n = distance_circles(agents[i]['position'], agents[i]['radius'],
agents[j]['position'], agents[j]['radius'])
if h < SIGTH_SOC:
force_i, force_j = force_social_circular(agents, i, j)
if h < 0:
t = rotate270(n)
v = agents[i]['velocity'] - agents[j]['velocity']
force_i += force_contact(h, n, v, t, agents[i]['mu'],
agents[i]['kappa'], agents[i]['damping'])
force_j -= force_contact(h, n, v, t, agents[j]['mu'],
agents[j]['kappa'], agents[j]['damping'])
agents[i]['force'][:] += force_i
agents[j]['force'][:] += force_j
def interaction_agent_circular_obstacle(i, w, agents, obstacles):
"""Interaction between circular agent and line obstacle."""
h, n = distance_circle_line(agents[i]['position'], agents[i]['radius'],
obstacles[w]['p0'], obstacles[w]['p1'])
if h < 0:
t = rotate270(n) # Tangent
v = agents[i]['velocity']
force = force_contact(h, n, v, t, agents[i]['mu'], agents[i]['kappa'],
agents[i]['damping'])
agents[i]['force'][:] += force
def interaction_agent_three_circle_obstacle(i, w, agents, obstacles):
"""Interaction between three circle agent and line obstacle."""
x_i = (agents[i]['position'], agents[i]['position_ls'],
agents[i]['position_rs'])
r_i = (agents[i]['r_t'], agents[i]['r_s'], agents[i]['r_s'])
h, n, r_moment = distance_three_circle_line(x_i, r_i, obstacles[w]['p0'],
obstacles[w]['p1'])
if h < 0:
t = rotate270(n) # Tangent
v = agents[i]['velocity']
force = force_contact(h, n, v, t, agents[i]['mu'], agents[i]['kappa'],
agents[i]['damping'])
agents[i]['force'][:] += force
agents[i]['torque'] += cross(r_moment, force)
def test_force_contact(h, n, v, t, mu, kappa, damping):
ans = force_contact(h, n, v, t, mu, kappa, damping)
assert isinstance(ans, np.ndarray)
assert ans.dtype.type is np.float64
assert ans.shape == (2,)