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)