def update_bot_collisions(self, botpairs):
for (i, j) in botpairs:
a = self.bots[i]
b = self.bots[j]
dist = a.pos.get_distance(b.pos)
bound = 2 * a.radius
if dist < 1 + bound:
overlap = bound - dist
direction = b.pos - a.pos
if (direction == Vec2d(0, 0)):
direction = Vec2d(1, 0)
direction.length = overlap / 2
b.pos = b.pos + direction
a.pos = a.pos - direction
def __init__(self, app, config):
self.app = app
self.config = config
self.running = False
self.bots = []
self.botmove = None
self.botsel = None
self.botselpos = None
self.orientation = 0
self.links = []
self.linkc = self.config['n'] * [0]
self.conns = []
self.reddots = []
self.gui = KBGUI(self)
self.analyseform = False
self.connectform = False
self.executeform = False
self.clearform = False
for i in range(self.config['n']):
self.bots.append(Kilobot(self))
for b in self.bots: # set semi-random initial position and orientation
b.orientation = random.uniform(0, 360)
b.pos = Vec2d(
self.config['width'] / 2 + random.uniform(0, b.secretID),
(self.config['height'] * 2) / 3 +
random.uniform(0, b.secretID))
def update(self): # called before draw; update variables
# process bots in pairs, shuffled
botn = range(len(self.bots))
botpairs = [(x, y) for x in botn for y in botn if x > y]
random.shuffle(botpairs)
# dodge, collision detection on fellow bots
for (i, j) in botpairs:
a = self.bots[i]
b = self.bots[j]
dist = a.pos.get_distance(b.pos)
bound = 2 * a.radius
if dist < 1 + bound:
overlap = bound - dist
direction = b.pos - a.pos
if (direction == Vec2d(0, 0)):
direction = Vec2d(1, 0)
direction.length = overlap / 2
b.pos = b.pos + direction
a.pos = a.pos - direction
def setFormation(self, formation):
if formation == "PILED":
for b in self.bots:
b.orientation = random.uniform(0, 360)
b.pos = Vec2d(
self.config['width'] / 2 + random.uniform(0, b.secretID),
self.config['height'] / 2 + random.uniform(0, b.secretID))
elif formation == "RANDOM":
xunit = self.config['width'] / 5
yunit = self.config['height'] / 5
for b in self.bots:
b.orientation = random.uniform(0, 360)
b.pos = Vec2d(random.uniform(yunit, 4 * yunit),
random.uniform(xunit, 4 * xunit))
elif formation == "LINE":
for i in range(0, self.config['n']):
self.bots[i].pos = Vec2d(self.config['width'] / 2 + i * 34,
self.config['height'] / 2)
self.bots[i].orientation = random.uniform(0, 360)
self.bots[0].orientation = 180 # make first one point left
elif formation == "CIRCLE":
deg = 360 / (self.config['n'])
pi = 3.141592653589793
radius = (self.bots[0].radius * (self.config['n'] + 5)) / pi
for i, b in enumerate(self.bots):
b.orientation = random.uniform(0, 360)
b.pos = Vec2d(self.config['width'] / 2,
self.config['height'] / 2)
b.pos += Vec2d(0, radius).rotated(deg * i)
else:
print "Unknown formation '%s' - aborting." % (formation)
exit(-42)
def __init__(self, sim):
self.sim = sim
self.secretID = len(sim.bots) # clever hack, init ~ sim.bots += 1
self.secretN = sim.config['n']
self.secretNx = []
self.radius = sim.config['botradius']
self.rradius = sim.config['botrradius']
self.speed = 0
self.pos = Vec2d(0,0)
self.orientation = 0
self.leds = [0,0,0]
self.op = self.fullCW # fpointer
self.opc = 0 # op counter
self.opres = None
self.history = sim.config['hsize'] * [[None]]
self.historyp = 0
self.tx_enabled = 0
self.msgrx = sim.config['msgform']
self.msgtx = sim.config['msgform']
self.rxbuf = sim.config['rxbufsize'] * [sim.config['msgform']]
self.rxbufp = 0
self.rxtempbuf = []
self.running = False
self.PC = 0
self.program = [] # derived replace
self.debug = ""
self.view = KilobotView(self)
def rand(self):
return Sample(Vec2d.rand(), Vec2d.rand(), Vec2d.rand(), random.random())
def turnOnRFoot(self, speed):
self.speed = speed
degrees = self.speed
self.orientation += degrees
pos = Vec2d(self.radius - 1, 0).rotated(self.orientation - 60 + degrees)
self.pos = self.rfoot() + pos
def turnOnLFoot(self, speed):
self.speed = speed
degrees = self.speed
self.orientation -=degrees
pos = Vec2d(self.radius - 1, 0).rotated(self.orientation + 60 - degrees)
self.pos = self.lfoot() + pos
def led(self):
return self.pos + Vec2d(self.radius - 3, 0).rotated(self.orientation + 60)
def lfoot(self):
return self.pos + Vec2d(self.radius - 1, 0).rotated(self.orientation + 240)
def mouseMotion(self, buttons, pos, rel):
if (self.botmove != None):
self.botmove.pos = Vec2d(pos)
elif (self.botsel != None):
self.botselpos = Vec2d(pos)
def keyUp(self, key):
if (key == K_a):
if self.smode:
self.module.abstract = not self.module.abstract
elif (key == K_c):
self.drawlinks = False if self.drawlinks else True
elif (key == K_d):
self.drawdebug = False if self.drawdebug else True
elif (key == K_e):
self.drawreddots = False if self.drawreddots else True
elif (key == K_g):
self.drawgrid = False if self.drawgrid else True
elif (key == K_h): # show
print "SHOW"
elif (key == K_i):
self.drawinfo = False if self.drawinfo else True
elif (key == K_k):
if self.smode:
self.module.fps -= 5
self.module.fps = 0 if self.module.fps < 0 else self.module.fps
elif (key == K_l):
if self.smode:
self.module.fps += 5
out = self.module.fps > self.config['fpsmax']
self.module.fps = self.config[
'fpsmax'] if out else self.module.fps
elif (key == K_m):
if self.smode:
self.module.inoise += 5
if self.module.inoise > 100: self.module.inoise = 100
elif (key == K_n):
if self.smode:
self.module.inoise -= 5
if self.module.inoise < 0: self.module.inoise = 0
elif (key == K_p): # print
print "PRINT"
elif (key == K_r):
self.drawradio = False if self.drawradio else True
elif (key == K_s):
if self.smode:
self.module.stopped = not self.module.stopped
elif (key == K_t):
self.drawtraffic = False if self.drawtraffic else True
elif (key == K_u):
self.drawui = False if self.drawui else True
elif (key == K_COMMA):
if self.smode:
self.module.anoise -= 5
if self.module.anoise < 100: self.module.anoise = 100
elif (key == K_PERIOD):
if self.smode:
self.module.anoise += 5
if self.module.anoise > 200: self.module.anoise = 200
elif (key == K_PAGEDOWN):
for b in self.module.bots:
b.pos = b.pos.rotated(90) + Vec2d(self.h, 0)
b.orientation += 90
elif (key == K_PAGEUP):
for b in self.module.bots:
b.pos = b.pos.rotated(-90) + Vec2d(0, self.w)
b.orientation -= 90
def connecter(self):
print "---\n CONNECTER \n---"
poss = []
for b in self.bots:
tup = b.pos.inttup()
poss.append(tup)
avg = map((lambda x: x / len(self.bots)),
reduce((lambda x, y: [x[0] + y[0], x[1] + y[1]]), poss,
[0, 0]))
print "positions:", poss, "\n", "avg:", avg
avg = Vec2d(avg)
# computer kNN and find those closest to the center of the bot mass
# TODO: make less sloppy; proper generative 2-MST etc
order = []
k = self.config['n']
knns = len(self.bots) * [None]
for i in range(len(self.bots)):
bot = self.bots[i]
dist = avg.get_distance(bot.pos)
order.append((dist, bot))
knns[i] = []
for bat in self.bots:
if (bat == bot): continue
dist = bot.pos.get_distance(bat.pos)
knns[i].append([dist, bat])
knns[i] = sorted(knns[i])[:k] # drop some
order = sorted(order)
# create links
self.links = []
self.linkc = self.config['n'] * [0]
# startup
a = order[0][1]
b = order[1][1]
pair = (a, b) if (a.secretID > b.secretID) else (b, a)
self.links.append(pair)
self.linkc[a.secretID] += 1
self.linkc[b.secretID] += 1
inplace = [a.secretID, b.secretID] # keep track of the existing form
dist = int(round(a.pos.get_distance(b.pos)))
self.conns = [(1, (0, dist), (0, dist))] # start conns
self.orientation = int((b.pos - a.pos).get_angle())
for i in map((lambda x: x[1].secretID), order[2:]):
knn = map((lambda x: x[1].secretID), knns[i])
bot = self.bots[i]
myid = len(inplace)
temp = None
for j in knns[
i]: # try to connect every bot to form as directly as possible
dist = int(round(j[0]))
k = j[1].secretID
bat = self.bots[k]
if (k in inplace):
# add links by secretID
pair = (bot, bat) if i > k else (bat, bot)
self.links.append(pair)
self.linkc[bot.secretID] += 1
self.linkc[bat.secretID] += 1
# add connection by form index
if (temp == None):
inplace.append(i)
temp = [k, inplace.index(k), dist]
else:
but = self.bots[temp.pop(0)]
# trigonometric black magic
# we compute the angle of bot in relation to a vector defined by to others (bat and but)
angle = (bot.pos - bat.pos).rotated(
-(but.pos - bat.pos).get_angle()).get_angle()
left = angle < 0
# print bat.secretID, "-->", but.secretID, ":", bot.secretID, "L" if angle < 0 else "R"
if (left): # we force RIGHTHANDSIDE positioning
self.conns.append(
(myid, temp, [inplace.index(k), dist]))
else:
self.conns.append((myid, [inplace.index(k),
dist], temp))
break
print "conns:"
for i, conn in enumerate(self.conns):
print conn