def testLength(self):
self.assertAlmostEqual(length(Vector(0.0, 0.0)), 0.0, geom_places)
self.assertAlmostEqual(length(Vector(1.0, 1.0)), 2**0.5, geom_places)
v = normalize(Vector(0.23, 1.45))
self.assertAlmostEqual(length(v * 23.145), 23.145)
self.assertAlmostEqual(length(v / 23.145), 1.0/23.145)
def testLength(self):
self.assertAlmostEqual(length(Vector(0.0, 0.0)), 0.0, geom_places)
self.assertAlmostEqual(length(Vector(1.0, 1.0)), 2**0.5, geom_places)
v = normalize(Vector(0.23, 1.45))
self.assertAlmostEqual(length(v * 23.145), 23.145)
self.assertAlmostEqual(length(v / 23.145), 1.0 / 23.145)
def point_is_visible(self, p, obstacles):
if not self.point_in_fov(p):
return False
try:
ray = Ray(self.pos, p - self.pos)
except NormalizationError:
ray = Ray(self.pos, Vector(1.0, 0.0))
i = first_intersection(ray, obstacles)
return (i is None) or (length(i - self.pos) > length(p - self.pos))
def update_vel(self, delta_time, desired_vel):
dv = desired_vel - self.vel
if length(dv) < self.max_accel * delta_time:
self.vel = desired_vel
else:
self.vel += normalize(dv) * self.max_accel * delta_time
if length(self.vel) > self.max_vel:
self.vel = self.max_vel * normalize(self.vel)
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 update_vel(self, delta_time, desired_vel):
ang = signed_angle(self.dir, desired_vel)
if abs(ang) < MIN_ROTATION_ANGLE:
ang = 0.0
ang = ROTATION_GAIN * ang
abs_vel = min(length(desired_vel), self.max_vel)
# don't move forward when doing a sharp turn;
# this seems sensible and also prevents bots from
# slamming into obstacles when trying to move away
#
# !this only makes sense with ROTATION_GAIN = 1.0!
abs_vel *= cos(signed_angle(desired_vel, self.dir))
if abs_vel < 0:
abs_vel = 0
# these are the velocities wheels would get
# if they didn't have to accelerate smoothly
target_lvel = abs_vel - 0.5 * self.width * ang
target_rvel = abs_vel + 0.5 * self.width * ang
#self.lvel = target_lvel
#self.rvel = target_rvel
if abs(self.lvel - target_lvel) < delta_time * BOT_ACCEL_CAP:
self.lvel = target_lvel
else:
self.lvel += copysign(BOT_ACCEL_CAP, target_lvel - self.lvel) * delta_time
if abs(self.rvel - target_rvel) < delta_time * BOT_ACCEL_CAP:
self.rvel = target_rvel
else:
self.rvel += copysign(BOT_ACCEL_CAP, target_rvel - self.rvel) * delta_time
# cap velocity
v = max(self.lvel, self.rvel)
if v > self.max_vel:
self.lvel *= self.max_vel / v
self.rvel *= self.max_vel / v
def draw(self, screen, field):
if graphics.DRAW_SENSOR_RAYS:
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for s, d in zip(self.sensors, self.distances):
r = s.get_ray(self.pos, ang)
graphics.draw_line(screen, field, (115, 115, 200),
r.orig,
r.orig + r.dir * d,
1)
if COLLISION_CHECK and self.collision:
# draw small purple circle indicating collision state
graphics.draw_circle(screen, field, (255, 0, 255),
self.pos,
0.5 * BOT_RADIUS)
# draw projections of virtual velocity onto sensor rays
if graphics.DRAW_SENSOR_RAYS:
vel_ang = signed_angle(self.virtual_vel_before_check, Vector(0.0, 1.0))
abs_vel = length(self.virtual_vel_before_check)
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for s in self.sensors:
r = s.get_ray(self.pos, ang)
c = cos(signed_angle(self.virtual_vel_before_check, r.dir))
proj = c * abs_vel;
if proj < 0:
continue
graphics.draw_line(screen, field, (115, 200, 200),
r.orig,
r.orig + r.dir * proj,
2)
v = Vector(0.0, 0.0)
try:
v = 0.5 * normalize(self.virtual_vel_before_check)
except ZeroDivisionError:
pass
# draw virtual velocity vector that was picked before collision check
graphics.draw_line(screen, field, (0, 200, 0),
self.pos,
self.pos + v,
1)
def update_state(self, delta_time, movement_sigma):
if self.pos_fun is not None:
self.time += delta_time
self.pos = self.pos_fun(self.time)
v = self.vel_fun(self.time, delta_time)
self.vel = length(v)
try:
self.dir = normalize(v)
except ZeroDivisionError:
# keep the previous value of dir
pass
else: # bot is user-controlled
if abs(self.vel) > self.max_vel:
self.vel = copysign(self.max_vel, self.vel)
if self.vel < 0:
self.vel = 0
self.pos += self.dir * self.vel * delta_time
self.shape.center = self.pos
def draw(self, screen, field):
if graphics.DRAW_SENSOR_RAYS:
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for s, d in zip(self.sensors, self.distances):
r = s.get_ray(self.pos, ang)
graphics.draw_line(screen, field, (115, 115, 200), r.orig,
r.orig + r.dir * d, 1)
if COLLISION_CHECK and self.collision:
# draw small purple circle indicating collision state
graphics.draw_circle(screen, field, (255, 0, 255), self.pos,
0.5 * BOT_RADIUS)
# draw projections of virtual velocity onto sensor rays
if graphics.DRAW_SENSOR_RAYS:
vel_ang = signed_angle(self.virtual_vel_before_check,
Vector(0.0, 1.0))
abs_vel = length(self.virtual_vel_before_check)
if ROTATE_SENSORS:
ang = self.get_heading_angle()
else:
ang = 0.0
for s in self.sensors:
r = s.get_ray(self.pos, ang)
c = cos(signed_angle(self.virtual_vel_before_check, r.dir))
proj = c * abs_vel
if proj < 0:
continue
graphics.draw_line(screen, field, (115, 200, 200), r.orig,
r.orig + r.dir * proj, 2)
v = Vector(0.0, 0.0)
try:
v = 0.5 * normalize(self.virtual_vel_before_check)
except ZeroDivisionError:
pass
# draw virtual velocity vector that was picked before collision check
graphics.draw_line(screen, field, (0, 200, 0), self.pos,
self.pos + v, 1)
def point_in_fov(self, p):
if length(p - self.pos) > self.visibility_radius:
return False
d = p - self.pos
ang = atan2(cross(self.real_dir, d), dot(self.real_dir, d))
return -0.5 * self.visibility_fov < ang < 0.5 * self.visibility_fov
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 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
