def get_insert_position(conf, ra, rb, world):
"""
Tries to find a suitable insert position
:param conf:
:param ra:
:param rb:
:param world:
:return:
"""
good = False
new_pos = None
while not good:
# Get position on the line separating the two robots
new_pos = world.get_equilateral_position(ra, rb, random.uniform(0, 2.5))
# Add some randomness to the insert position
new_pos += 0.1 * pick_position(conf)
good = True
for r in world.robots.values():
if not r.last_position:
continue
# Only choose positions that are more than 25cm
# away from the nearest bot but closer than 4m
# from the furthest bot.
diff = r.last_position - new_pos
dist = diff.norm()
if dist < in_cm(25) or dist > 4:
good = False
break
return new_pos
def pick_position(conf):
"""
:param conf:
:param z: z height of the returned vector
:return:
"""
margin = in_cm(40)
x_min, x_max = -margin/2, margin/2
y_min, y_max = -margin/2, margin/2
x = random.uniform(x_min, x_max)
y = random.uniform(y_min, y_max)
return Vector3(x, y, insert_z)
def pick_position(conf):
"""
:param conf:
:param z: z height of the returned vector
:return:
"""
margin = in_cm(20)
x_min, x_max = -0.5 * conf.arena_size[0] + margin, 0.5 * conf.arena_size[0] - margin
y_min, y_max = -0.5 * conf.arena_size[1] + margin, 0.5 * conf.arena_size[1] - margin
x = random.uniform(x_min, x_max)
y = random.uniform(y_min, y_max)
return Vector3(x, y, insert_z)
