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)