agent.velocity = new_vels[i]
agent.position += agent.velocity * dt
screen.fill(pygame.Color(0, 0, 0))
for agent in agents[1:]:
draw_orca_circles(agents[0], agent)
for agent, color in zip(agents, itertools.cycle(colors)):
draw_agent(agent, color)
draw_velocity(agent)
# print(sqrt(norm_sq(agent.velocity)))
for line in all_lines[0]:
# Draw ORCA line
alpha = agents[0].position + line.point + perp(line.direction) * 100
beta = agents[0].position + line.point + perp(line.direction) * -100
pygame.draw.line(screen, (255, 255, 255),
rint(alpha * scale + O).astype(int),
rint(beta * scale + O).astype(int), 1)
# Draw normal to ORCA line
gamma = agents[0].position + line.point
delta = agents[0].position + line.point + line.direction
pygame.draw.line(screen, (255, 255, 255),
rint(gamma * scale + O).astype(int),
rint(delta * scale + O).astype(int), 1)
pygame.display.flip()
for event in pygame.event.get():
agent.velocity = new_vels[i]
agent.position += agent.velocity * dt
screen.fill(pygame.Color(0, 0, 0))
for agent in agents[1:]:
draw_orca_circles(agents[0], agent)
for agent, color in zip(agents, itertools.cycle(colors)):
draw_agent(agent, color)
draw_velocity(agent)
# print(sqrt(norm_sq(agent.velocity)))
for line in all_lines[0]:
# Draw ORCA line
alpha = agents[0].position + line.point + perp(line.direction) * 100
beta = agents[0].position + line.point + perp(line.direction) * -100
pygame.draw.line(screen, (255, 255, 255), rint(alpha * scale + O).astype(int), rint(beta * scale + O).astype(int), 1)
# Draw normal to ORCA line
gamma = agents[0].position + line.point
delta = agents[0].position + line.point + line.direction
pygame.draw.line(screen, (255, 255, 255), rint(gamma * scale + O).astype(int), rint(delta * scale + O).astype(int), 1)
pygame.display.flip()
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
pygame.quit()