def update_shoulder_position(self, i):
n = np.array((np.cos(self.angle[i]), np.sin(self.angle[i])))
t = rotate270(n)
offset = t * self.r_ts[i]
self.position_ls[i] = self.position[i] - offset
self.position_rs[i] = self.position[i] + offset
self.front[i] = self.position[i] + n * self.r_t[i]
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_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 shoulders(agents):
"""Positions of the center of mass, left- and right shoulders.
Args:
agents (ndarray):
Numpy array of datatype ``dtype=agent_type_three_circle``.
"""
for agent in agents:
tangent = rotate270(unit_vector(agent['orientation']))
offset = tangent * agent['r_ts']
agent['position_ls'][:] = agent['position'] - offset
agent['position_rs'][:] = agent['position'] + offset
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 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 _default_position_rs(self):
return self.position + self.r_ts * rotate270(
unit_vector(self.orientation))
def test_rotate270(a):
ans = rotate270(a)
assert isinstance(ans, np.ndarray)
