def testRotate(self):
from math import pi
self.assertEqual(rotate(Vector(2.0, 0.0), pi / 4),
Vector(2**0.5, 2**0.5))
self.assertEqual(rotate(Vector(0.0, 100.0), pi), Vector(0.0, -100.0))
self.assertEqual(rotate(Vector(1.46, 1.75), -2 * pi),
Vector(1.46, 1.75))
def draw(self, screen, field):
draw_circle(screen, field, BOT_COLOR,
self.pos,
self.radius, 1)
top_angle = 40 * pi / 180
x = self.radius * sin(top_angle)
y = self.radius * cos(top_angle)
ang = signed_angle(Vector(0.0, 1.0), self.dir)
pa = self.pos + rotate(Vector(-x, -y), ang)
pb = self.pos + rotate(Vector( x, -y), ang)
pc = self.pos + rotate(Vector(0.0, self.radius), ang)
draw_line(screen, field, BOT_COLOR, pa, pb, 1)
draw_line(screen, field, BOT_COLOR, pa, pc, 1)
draw_line(screen, field, BOT_COLOR, pb, pc, 1)
def draw(self, screen, field):
if config.DISPLAYED_POINTS_COUNT > 0:
k = config.POSITIONS_BUFFER_SIZE / config.DISPLAYED_POINTS_COUNT
if k == 0:
k = 1
for index, (time, point) in enumerate(self.leader_positions):
if index % k == 0:
draw_circle(screen, field, config.TRAJECTORY_POINTS_COLOR, point, 0.03, 1)
for index, (time, point) in enumerate(self.noisy_leader_positions):
if index % k == 0:
draw_circle(screen, field, config.NOISY_TRAJECTORY_POINTS_COLOR, point, 0.03, 1)
if config.DISPLAYED_USED_POINTS_COUNT > 0:
k = len(self.traj_interval) / config.DISPLAYED_USED_POINTS_COUNT
if k == 0:
k = 1
for index, (time, point) in enumerate(self.traj_interval):
if index % k == 0:
draw_circle(screen, field, config.USED_TRAJECTORY_POINTS_COLOR, point, 0.03, 1)
if config.DRAW_DELAYED_LEADER_POS:
try:
orig_leader_dir = Vector(cos(self.orig_leader_theta),
sin(self.orig_leader_theta))
draw_directed_circle(screen, field, (0, 255, 0),
self.orig_leader_pos, 0.2,
orig_leader_dir, 1)
except AttributeError:
pass
if config.DRAW_SENSOR_RANGE:
ang = atan2(self.real_dir.y, self.real_dir.x)
draw_arc(screen, field, config.SENSOR_COLOR, self.pos, self.visibility_radius,
ang - 0.5 * self.visibility_fov,
ang + 0.5 * self.visibility_fov,
1)
draw_line(screen, field, config.SENSOR_COLOR,
self.pos,
self.pos + rotate(self.real_dir * self.visibility_radius,
0.5 * self.visibility_fov),
1)
draw_line(screen, field, config.SENSOR_COLOR,
self.pos,
self.pos + rotate(self.real_dir * self.visibility_radius,
-0.5 * self.visibility_fov),
1)
try:
if config.DRAW_VISIBILITY and self.leader_is_visible:
draw_circle(screen, field, config.config.VISIBILITY_COLOR, self.leader.real.pos,
0.5 * config.BOT_RADIUS)
except AttributeError:
pass
try:
if config.DRAW_REFERENCE_POS:
draw_circle(screen, field, config.TARGET_POINT_COLOR, self.target_point, 0.2)
if config.DRAW_APPROX_TRAJECTORY and self.trajectory is not None:
#if len(self.traj_interval) > 1:
# p2 = Point(self.traj_interval[0].pos.x,
# self.traj_interval[0].pos.y)
# for t, p in self.traj_interval:
# draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2)
# p2 = p
step = (self.t_fn - self.t_st) / config.TRAJECTORY_SEGMENT_COUNT
for t in (self.t_st + k * step for k in xrange(config.TRAJECTORY_SEGMENT_COUNT)):
p = Point(self.trajectory.x(t),
self.trajectory.y(t))
p2 = Point(self.trajectory.x(t + step),
self.trajectory.y(t + step))
draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2)
p_st = Point(self.trajectory.x(self.t_st), self.trajectory.y(self.t_st))
p_fn = Point(self.trajectory.x(self.t_fn), self.trajectory.y(self.t_fn))
step = 0.5 / config.TRAJECTORY_SEGMENT_COUNT
p = p_fn
p2 = p
t = self.t_fn
it = 0
while it < config.TRAJECTORY_SEGMENT_COUNT and min(dist(p, p_fn), dist(p, p_st)) < 1.0:
it += 1
t += step
p2 = p
p = Point(self.trajectory.x(t), self.trajectory.y(t))
draw_line(screen, field, config.APPROX_TRAJECTORY_COLOR, p, p2)
except AttributeError as e: # approximation hasn't been calculated yet
pass
def update_state(self, delta_time, movement_sigma):
self.lvel += gauss(0.0, movement_sigma)
self.rvel += gauss(0.0, movement_sigma)
self.pos += delta_time * self.vel * self.dir
self.dir = rotate(self.dir, delta_time * self.rot_vel)
self.shape.center = self.pos
def testRotate(self):
from math import pi
self.assertEqual(rotate(Vector(2.0, 0.0), pi/4), Vector(2**0.5, 2**0.5))
self.assertEqual(rotate(Vector(0.0, 100.0), pi), Vector(0.0, -100.0))
self.assertEqual(rotate(Vector(1.46, 1.75), -2 * pi), Vector(1.46, 1.75))
def update_state(self, delta_time):
self.pos += delta_time * self.vel
self.dir = rotate(self.dir, delta_time * self.rot_vel)
def get_ray(self, bot_pos, bot_angle):
ang = self.angle + bot_angle
dir = rotate(Vector(0, self.bot_radius), ang)
dir = normalize(dir)
return Ray(bot_pos + dir * self.bot_radius, dir)
def get_ray(self, bot_pos, bot_angle):
ang = self.angle + bot_angle
dir = rotate(Vector(0, self.bot_radius), ang)
dir = normalize(dir)
return Ray(bot_pos + dir * self.bot_radius, dir)
