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,)