def calc_desired_velocity(self, bots, obstacles, targets):
vel = self.vel
if self.movement != Movement.Accel:
vel = Vector(0, 0)
for inter in bots:
if (not KNOW_BOT_POSITIONS) and dist(inter.real.pos, self.pos) > self.max_sensing_distance:
continue
force = -potential.gradient(potential.morse(r0=2 * BOT_RADIUS, k=2.5, a=4.0),
dist(inter.real.pos, self.pos),
self.pos - inter.real.pos,
self.radius + inter.virtual.radius)
vel += _FORCE_SENSITIVITY * force
for target in targets:
force = -potential.gradient(potential.linear(k=-2.0),
dist(target, self.pos),
target - self.pos,
0)
vel += _FORCE_SENSITIVITY * force
for obstacle in obstacles:
if obstacle.distance(self.pos) <= self.max_sensing_distance:
force = -potential.gradient(potential.inverse_quadratic(k=1.0),
obstacle.distance(self.pos),
obstacle.repulsion_dir(self.pos),
OBSTACLE_CLEARANCE + self.radius)
vel += _FORCE_SENSITIVITY * force
if self.movement == Movement.Dir:
if length(vel) > 0:
vel = normalize(vel)
return vel
def get_distance(self, bot_pos, bot_angle, obstacles):
ray = self.get_ray(bot_pos, bot_angle)
p = first_intersection(ray, obstacles)
if p is None:
return self.max_distance
else:
return min(self.max_distance, dist(ray.orig, p))
def get_distance(self, bot_pos, bot_angle, obstacles):
ray = self.get_ray(bot_pos, bot_angle)
p = first_intersection(ray, obstacles)
if p is None:
return self.max_distance
else:
return min(self.max_distance, dist(ray.orig, p))
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 calc_desired_velocity(self, bots, obstacles, targets):
vel = self.real_vel
self.collision_delta_time = length(vel) / BOT_ACCEL_CAP
if self.movement != Movement.Accel:
vel = Vector(0, 0)
for inter in bots:
if (not KNOW_BOT_POSITIONS) and dist(inter.real.pos, self.pos) > self.max_sensing_distance:
continue
force = -potential.gradient(potential.morse(r0=2 * BOT_RADIUS, k=2.5, a=4.0),
dist(inter.real.pos, self.pos),
self.pos - inter.real.pos,
self.radius + inter.virtual.radius)
vel += _FORCE_SENSITIVITY * force
for target in targets:
force = -potential.gradient(potential.linear(k=-10.0),
dist(target, self.pos),
target - self.pos,
0)
vel += _FORCE_SENSITIVITY * force
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
self.distances = []
for s in self.sensors:
d = s.get_distance(self.pos, ang, obstacles)
self.distances.append(d)
if d < self.max_sensing_distance:
force = -potential.gradient(potential.inverse_quadratic(k=1.5),
d,
-s.get_ray(self.pos, ang).dir,
OBSTACLE_CLEARANCE)
vel += _FORCE_SENSITIVITY * force * self.obstacle_force_coeff
if self.movement == Movement.Dir:
if length(vel) > 0:
vel = normalize(vel)
# TODO: get max_vel from corresponding Model
if length(vel) > BOT_VEL_CAP:
vel = normalize(vel) * BOT_VEL_CAP
self.virtual_vel_before_check = vel
if COLLISION_CHECK:
# check for possible collisions:
# stop moving forward if an obstacle is nearby
collision = False
abs_vel = length(vel)
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for cur_d, s in zip(self.distances, self.sensors):
if cur_d == s.max_distance:
continue
r = s.get_ray(self.pos, ang)
c = cos(signed_angle(vel, r.dir))
new_d = cur_d - c * abs_vel * self.collision_delta_time
if new_d < CRITICAL_DIST and new_d < cur_d:
collision = True
break
if not collision:
# separate check for possible collision with another bot
for inter in bots:
# don't count collsions with self
if inter.virtual is self:
continue
new_d = dist(inter.real.pos + self.collision_delta_time * inter.real.vel,
self.pos + self.collision_delta_time * vel)
new_d -= (self.radius + inter.real.radius)
cur_d = dist(inter.real.pos, self.pos)
cur_d -= (self.radius + inter.real.radius)
if d <= CRITICAL_DIST and new_d < cur_d:
collision = True
break
if collision:
vel = normalize(vel) * CRITICAL_VEL
self.collision = True
else:
self.collision = False
return vel
def testDist(self):
self.assertAlmostEqual(dist(Point(0.0, 0.0), Point(0.0, 0.0)),
0.0, geom_places)
self.assertAlmostEqual(dist(Point(1.0, 2.0), Point(0.0, 3.0)),
2**0.5, geom_places)
def calc_desired_velocity(self, bots, obstacles, targets):
vel = self.real_vel
self.collision_delta_time = length(vel) / BOT_ACCEL_CAP
if self.movement != Movement.Accel:
vel = Vector(0, 0)
for inter in bots:
if (not KNOW_BOT_POSITIONS) and dist(
inter.real.pos, self.pos) > self.max_sensing_distance:
continue
force = -potential.gradient(
potential.morse(r0=2 * BOT_RADIUS, k=2.5, a=4.0),
dist(inter.real.pos, self.pos), self.pos - inter.real.pos,
self.radius + inter.virtual.radius)
vel += _FORCE_SENSITIVITY * force
for target in targets:
force = -potential.gradient(potential.linear(
k=-10.0), dist(target, self.pos), target - self.pos, 0)
vel += _FORCE_SENSITIVITY * force
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
self.distances = []
for s in self.sensors:
d = s.get_distance(self.pos, ang, obstacles)
self.distances.append(d)
if d < self.max_sensing_distance:
force = -potential.gradient(potential.inverse_quadratic(k=1.5),
d, -s.get_ray(self.pos, ang).dir,
OBSTACLE_CLEARANCE)
vel += _FORCE_SENSITIVITY * force * self.obstacle_force_coeff
if self.movement == Movement.Dir:
if length(vel) > 0:
vel = normalize(vel)
# TODO: get max_vel from corresponding Model
if length(vel) > BOT_VEL_CAP:
vel = normalize(vel) * BOT_VEL_CAP
self.virtual_vel_before_check = vel
if COLLISION_CHECK:
# check for possible collisions:
# stop moving forward if an obstacle is nearby
collision = False
abs_vel = length(vel)
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for cur_d, s in zip(self.distances, self.sensors):
if cur_d == s.max_distance:
continue
r = s.get_ray(self.pos, ang)
c = cos(signed_angle(vel, r.dir))
new_d = cur_d - c * abs_vel * self.collision_delta_time
if new_d < CRITICAL_DIST and new_d < cur_d:
collision = True
break
if not collision:
# separate check for possible collision with another bot
for inter in bots:
# don't count collsions with self
if inter.virtual is self:
continue
new_d = dist(
inter.real.pos +
self.collision_delta_time * inter.real.vel,
self.pos + self.collision_delta_time * vel)
new_d -= (self.radius + inter.real.radius)
cur_d = dist(inter.real.pos, self.pos)
cur_d -= (self.radius + inter.real.radius)
if d <= CRITICAL_DIST and new_d < cur_d:
collision = True
break
if collision:
vel = normalize(vel) * CRITICAL_VEL
self.collision = True
else:
self.collision = False
return vel
def testDist(self):
self.assertAlmostEqual(dist(Point(0.0, 0.0), Point(0.0, 0.0)), 0.0,
geom_places)
self.assertAlmostEqual(dist(Point(1.0, 2.0), Point(0.0, 3.0)), 2**0.5,
geom_places)
