import numpy as np
# ロボットの定義(再掲)
o = np.array([50., 150.]) # ロボット位置
ray_l = 200 # Lidar有効距離
ray_n = 9 # Lidar線数
ray_angles = [math.pi/2 * i / (ray_n - 1.) - math.pi/4 for i in range(ray_n)]
ray_ends = o + ray_l * np.array([[math.cos(theta), math.sin(theta)] for theta in ray_angles])
for l, p in zip(lidar, ray_ends):
l.p1 = o
l.p2 = p
world.RayCast(l, l.p1, l.p2)
import matplotlib.pyplot as plt
for b in world.bodies:
for f in b.fixtures:
w = f.shape.vertices
# 隔壁が2点で定義されている前提の描画処理
plt.plot([w[0][0], w[1][0]], [w[0][1], w[1][1]], color = 'black')
for l in lidar_b2d:
plt.plot([l.p1[0], l.p2[0]], [l.p1[1], l.p2[1]], color = 'blue')
plt.show()