def drawWheel(ax, shape, body):
r = shape.radius
pos = body.transform * shape.pos
drawCircle(ax, pos, r)
for a in [0, 2 * np.pi / 3, 4 * np.pi / 3]:
v = vrotate((1, 0), body.angle + a)
x = [pos[0], pos[0] + 0.99 * r * v[0]]
y = [pos[1], pos[1] + 0.99 * r * v[1]]
ax.plot(x, y, linestyle='-', color='b', lw=1)
def drawEpuck(ax, shape, body):
r = shape.radius
pos = body.position
drawCircle(ax, pos, r)
irangles = body.userData["IRAngles"]
nir = len(irangles)
for a in [-np.pi / 4, np.pi / 4]:
v = vrotate((1, 0), body.angle + a)
x = [pos[0], pos[0] + 0.99 * r * v[0]]
y = [pos[1], pos[1] + 0.99 * r * v[1]]
ax.plot(x, y, linestyle='-', color='b', lw=1)
for k in range(nir):
v = vrotate((1, 0), body.angle + irangles[k])
c = pos + [0.97 * r * v[0], 0.97 * r * v[1]]
value = body.userData["IRValues"][k]
if (value > 1): value = 1
drawCircle(ax, c, r=0.07, color=[1 - value, value, 0])
def drawWheel(ax,shape,body):
r = shape.radius
pos = body.transform*shape.pos
drawCircle(ax,pos,r)
for a in [0,2*np.pi/3,4*np.pi/3]:
v = vrotate((1,0),body.angle+a)
x = [pos[0],pos[0]+0.99*r*v[0]]
y = [pos[1],pos[1]+0.99*r*v[1]]
ax.plot(x,y, linestyle='-', color='b', lw=1)