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 testAngle(self):
from math import pi
self.assertAlmostEqual(signed_angle(Vector(1.0, 0.0), Vector(0.0, 1.0)),
pi / 2)
self.assertAlmostEqual(angle(Vector(1.0, 0.0), Vector(0.0, 1.0)),
pi / 2)
self.assertAlmostEqual(signed_angle(Vector(-1.0, 1.0), Vector(1.0, 1.0)),
-pi / 2)
self.assertAlmostEqual(angle(Vector(-1.0, 1.0), Vector(1.0, 1.0)),
pi / 2)
self.assertAlmostEqual(signed_angle(Vector(1.0, 0.0), Vector(100.0, 100.0)),
pi / 4)
self.assertAlmostEqual(angle(Vector(1.0, 0.0), Vector(100.0, 100.0)),
pi / 4)
self.assertAlmostEqual(abs(signed_angle(Vector(-100.0, 0.0), Vector(0.01, 0.0))),
pi)
self.assertAlmostEqual(angle(Vector(-100.0, 0.0), Vector(0.01, 0.0)),
pi)
def testAngle(self):
from math import pi
self.assertAlmostEqual(
signed_angle(Vector(1.0, 0.0), Vector(0.0, 1.0)), pi / 2)
self.assertAlmostEqual(angle(Vector(1.0, 0.0), Vector(0.0, 1.0)),
pi / 2)
self.assertAlmostEqual(
signed_angle(Vector(-1.0, 1.0), Vector(1.0, 1.0)), -pi / 2)
self.assertAlmostEqual(angle(Vector(-1.0, 1.0), Vector(1.0, 1.0)),
pi / 2)
self.assertAlmostEqual(
signed_angle(Vector(1.0, 0.0), Vector(100.0, 100.0)), pi / 4)
self.assertAlmostEqual(angle(Vector(1.0, 0.0), Vector(100.0, 100.0)),
pi / 4)
self.assertAlmostEqual(
abs(signed_angle(Vector(-100.0, 0.0), Vector(0.01, 0.0))), pi)
self.assertAlmostEqual(angle(Vector(-100.0, 0.0), Vector(0.01, 0.0)),
pi)
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 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 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