Skip to content
Snippets Groups Projects
Commit 8a897cb0 authored by markr's avatar markr
Browse files

need to copy updateV_leadership into main file and change name

parent a1089c67
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
leadership_with_full_couzin.py 0 → 100644
+ 598
− 0
View file @ 8a897cb0
from graphics import *
import time
import random
import math
import matplotlib.pyplot as plt
import numpy as np
# -------------------------------------------------------------------
# help functions
# Distance function betwen points xi, yi and xii,yii
def distance(xi,xii,yi,yii):
sq1 = (xi-xii)*(xi-xii)
sq2 = (yi-yii)*(yi-yii)
return math.sqrt(sq1 + sq2)
# -------------------------------------------------------------------
# simplest simulation
def nearest_neighbour(a,aas):
minDis = float('inf')
nn = None
for b in aas:
if (a == b):
True
elif (nn == None):
nn = b
else:
dis = distance(a[0].getX(),b[0].getX(), a[0].getY(), b[0].getY())
if(dis < minDis):
minDis = dis
nn = b
return b
# updateVelociy
def updateV(agent, nn, maxV):
vx = agent[1] + 0.1*nn[1] + random.uniform(-3, 3)
vy = agent[2] + 0.1*nn[2] + random.uniform(-3, 3)
if(abs(vx) < maxV) :
agent[1] = vx
elif (vx <= -maxV):
agent[1] = -maxV
else :
agent[1] = maxV
if(abs(vy) < maxV ):
agent[2] = vy
elif (vy <= -maxV):
agent[2] = -maxV
else :
agent[2] = maxV
return agent
# check for window boundaries
def checkBoundary(agent, winWidth, winHeight):
point = agent[0]
point.move(agent[1],agent[2])
x = point.getX()
y = point.getY()
if x > 0 and y < winHeight and x < winWidth and y > 0:
agent[0] = point
elif x <= 0 or x >= winWidth:
agent[1] = agent[1] * (-1)
agent[0].move(agent[1],agent[2])
elif y <= 0 or y >= winHeight:
agent[2] = agent[2] * (-1)
agent[0].move(agent[1],agent[2])
return agent
# check for window boundaries and move agents
def checkBoundary_free(agent, winWidth, winHeight):
point = agent[0]
point.move(agent[1],agent[2])
x = point.getX()
y = point.getY()
if x > 0 and y < winHeight and x < winWidth and y > 0:
agent[0] = point
elif x <= 0 or x >= winWidth:
#agent[1] = agent[1] % winWidth
x = agent[0].getX() % winWidth
agent[0] = Point(x, y)
#agent[0].move(agent[1],agent[2])
elif y <= 0 or y >= winHeight:
#agent[2] = agent[2] % winHeight
#agent[0].move(agent[1],agent[2])
y = agent[0].getY() % winHeight
agent[0] = Point(x, y)
return agent
def update_simplest(agent, agents, maxV, winWidth, winHeight, window):
for agent in agents:
nn = nearest_neighbour(agent, agents)
agent = updateV(agent, nn, maxV)
agent = checkBoundary(agent, winWidth, winHeight)
agent[3].undraw()
agent[3] = Line(agent[0], Point(agent[0].getX() + agent[1], agent[0].getY() + agent[2]))
agent[3].setArrow("last")
agent[3].draw(window)
return agents
# -------------------------------------------------------------------
# couzin and vicsek simulation
def absvec(a, b):
m = math.sqrt(a*a + b*b)
if m == 0: m = 0.001
return m
def calc_angle(x1, y1, x2, y2):
skalar = x1*x2 + y1*y2
abs1 = absvec(x1, y1)
abs2 = absvec(x2, y2)
erg = skalar/(abs1* abs2)
if erg > 1:
#print erg
erg=1
elif erg < -1:
#print erg
erg=-1
return math.degrees(math.acos(erg))
# returns three lists, one for each zone,
# contaning all other agent in the zone.
# ignores al egents ind the angle behind the current agent defined by blind.
def neigbour_in_zones(a, aas, zor_r, zoo_r, zoa_r, blind):
zor = []
zoo = []
zoa = []
for agent in aas:
disVecX = agent[0].getX() - a[0].getX()
disVecY = agent[0].getY() - a[0].getY()
alpha = calc_angle(a[1],a[2], disVecX, disVecY)
if (a == agent):
True
elif alpha < 180 - blind and alpha > 180 + blind:
True
else:
dis = absvec(agent[0].getX() - a[0].getX() , agent[0].getY() - a[0].getY() )
if dis <= zor_r:
zor.append(agent)
elif dis <= zoo_r:
zoo.append(agent)
elif dis <= zoa_r:
zoa.append(agent)
#print len(zoo)+len(zor)+len(zoa)
return [zor, zoo, zoa]
def wraparound_distance(x1, y1, x2, y2, winWidth, winHeight):
xDis = x1-x2
yDis = y1-y2
x=x1
y=y1
flag = False
if(xDis > winWidth/2):
x -= winWidth
flag = True
elif (xDis < -winWidth/2):
x += winWidth
flag = True
if(yDis > winHeight/2):
y -= winHeight
flag = True
elif (yDis < -winHeight/2):
y += winHeight
flag = True
return [distance(x, y, x2, y2), flag]
def neigbour_in_zones_free(a, aas, zor_r, zoo_r, zoa_r, blind, winWidth, winHeight):
zor = []
zoo = []
zoa = []
for agent in aas:
wpd = wraparound_distance(a[0].getX(), a[0].getY(), agent[0].getX(), agent[0].getY(), winWidth, winHeight)
dis = wpd[0]
#disVecX = agent[0].getX() - a[0].getX()
#disVecY = agent[0].getY() - a[0].getY()
#alpha = calc_angle(a[1],a[2], disVecX, disVecY)
if (a == agent):
True
#elif alpha < 180 - blind and alpha > 180 + blind:
# True
else:
dis = absvec(agent[0].getX() - a[0].getX() , agent[0].getY() - a[0].getY() )
if dis <= zor_r:
zor.append([agent, wpd[1]])
elif dis <= zoo_r:
zoo.append([agent, wpd[1]])
elif dis <= zoa_r:
zoa.append([agent, wpd[1]])
#print len(zoo)+len(zor)+len(zoa)
return [zor, zoo, zoa]
#update Velocity a la couzin
def updateV_couzin(a, matrix, maxV):
dx=0
dy=0
#zor
if matrix[0] != []:
for agent in matrix[0]:
disX = agent[0].getX() - a[0].getX()
disY = agent[0].getY() - a[0].getY()
rX = disX/absvec(disX, disY)
rY = disY/absvec(disX, disY)
dx += rX / absvec(rX, rY)
dy += rY / absvec(rX, rY)
dx = -dx
dy = -dy
# zoo ; zoa leer
elif matrix[1] != [] and matrix[2] == []:
for agent in matrix[1]:
dx += agent[1] / absvec(agent[1], agent[2])
dy += agent[2] / absvec(agent[1], agent[2])
dx += a[1] / absvec(a[1], a[2])
dy += a[2] / absvec(a[1], a[2])
# zoo leer ; zoa
elif matrix[1] == [] and matrix[2] != []:
for agent in matrix[2]:
disX = agent[0].getX() - a[0].getX()
disY = agent[0].getY() - a[0].getY()
rX = disX/absvec(disX, disY)
rY = disY/absvec(disX, disY)
dx += rX / absvec(rX, rY)
dy += rY / absvec(rX, rY)
# zoo ; zoa
elif matrix[1] != [] and matrix[2] != []:
for agent in matrix[1]:
dx += agent[1] / absvec(agent[1], agent[2])
dy += agent[2] / absvec(agent[1], agent[2])
dx += a[1] / absvec(a[1], a[2])
dy += a[2] / absvec(a[1], a[2])
for agent in matrix[2]:
disX = agent[0].getX() - a[0].getX()
disY = agent[0].getY() - a[0].getY()
rX = disX/absvec(disX, disY)
rY = disY/absvec(disX, disY)
dx += rX / absvec(rX, rY)
dy += rY / absvec(rX, rY)
dx = 0.5*dx
dy = 0.5*dy
# all zones empty
else:
dx = a[1]
dy = a[2]
# randomness factor / error
dx += random.uniform(-1, 1)
dy += random.uniform(-1, 1)
return [dx, dy]
def update_couzin(agent, agents, maxV, winWidth, winHeight, window, maxTurn, radTurn, negRadTurn,zor_r, zoo_r, zoa_r, blind, speed, free):
# Velocity update
for agent in agents:
neigh_matrix = neigbour_in_zones(agent, agents, zor_r, zoo_r, zoa_r,blind)
agent[4] = updateV_couzin(agent, neigh_matrix, maxV)
# move, draw
for agent in agents:
# alpha = calc_angle(agent[1], agent[2],agent[4][0],agent[4][1])
# test if in ragne of maxturn, if not rotate angle by maxTurn in
# direction of new direction
angle_old = math.atan2(agent[2], agent[1])
angle_new = math.atan2(agent[4][1], agent[4][0])
alpha = math.degrees(angle_old - angle_new)
if abs(alpha) > 180:
if alpha < 0:
alpha += 360
else:
alpha -= 360
if abs(alpha)<maxTurn:
agent[1] = agent[4][0]
agent[2] = agent[4][1]
elif alpha < 0:
agent[1] = agent[1] * math.cos(radTurn) - agent[2] * math.sin(radTurn)
agent[2] = agent[1] * math.sin(radTurn) + agent[2] * math.cos(radTurn)
else:
agent[1] = agent[1] * math.cos(negRadTurn) - agent[2] * math.sin(negRadTurn)
agent[2] = agent[1] * math.sin(negRadTurn) + agent[2] * math.cos(negRadTurn)
# normalise diection vector to 1, and multiply by constant speed
agent[1] = agent[1]/absvec(agent[1], agent[2]) * speed
agent[2] = agent[2]/absvec(agent[1], agent[2]) * speed
if free:
agent = checkBoundary_free(agent, winWidth, winHeight)
else:
agent = checkBoundary(agent, winWidth, winHeight)
# draw arrow
agent[3].undraw()
agent[3] = Line(agent[0], Point(agent[0].getX() + agent[1], agent[0].getY() + agent[2]))
agent[3].setArrow("last")
agent[3].draw(window)
return agents
#--------------leadership
def updateV_leadership(a, matrix, maxV):
dx=0
dy=0
#zor
if matrix[0] != []:
for agent in matrix[0]:
disX = agent[0][0].getX() - a[0].getX()
disY = agent[0][0].getY() - a[0].getY()
rX = disX/absvec(disX, disY)
rY = disY/absvec(disX, disY)
#flag checkBoundary
if not agent[1]:
dx += rX / absvec(rX, rY)
dy += rY / absvec(rX, rY)
else:
dx -= rX / absvec(rX, rY)
dy -= rY / absvec(rX, rY)
dx = -dx
dy = -dy
# randomness factor / error
#dx += random.uniform(-1, 1)
#dy += random.uniform(-1, 1)
#normalise
dx = dx / absvec(dx, dy)
dy = dy / absvec(dx, dy)
# zoo ; zoa leer
elif matrix[1] != [] and matrix[2] == []:
for b in matrix[1]:
agent = b[0]
if not b[1]:
dx += agent[1] / absvec(agent[1], agent[2])
dy += agent[2] / absvec(agent[1], agent[2])
else:
dx -= agent[1] / absvec(agent[1], agent[2])
dy -= agent[2] / absvec(agent[1], agent[2])
dx += a[1] / absvec(a[1], a[2])
dy += a[2] / absvec(a[1], a[2])
# zoo leer ; zoa
elif matrix[1] == [] and matrix[2] != []:
for b in matrix[2]:
agent = b[0]
disX = agent[0].getX() - a[0].getX()
disY = agent[0].getY() - a[0].getY()
if not b[1]:
dx += ( disX / absvec(disX, disY) ) / absvec( disX / absvec(disX, disY) , disY/ absvec(disX,disY) )
dy += ( disY / absvec(disX, disY) ) / absvec( disX / absvec(disX, disY) , disY/ absvec(disX,disY) )
else:
dx -= ( disX / absvec(disX, disY) ) / absvec( disX / absvec(disX, disY) , disY/ absvec(disX,disY) )
dy -= ( disY / absvec(disX, disY) ) / absvec( disX / absvec(disX, disY) , disY/ absvec(disX,disY) )
elif matrix[1] != [] and matrix[2] != []:
for b in matrix[1]:
agent = b[0]
dx += agent[1] / absvec(agent[1], agent[2])
dy += agent[2] / absvec(agent[1], agent[2])
for b in matrix[2]:
agent = b[0]
disX = agent[0].getX() - a[0].getX()
disY = agent[0].getY() - a[0].getY()
rX = disX/absvec(disX, disY)
rY = disY/absvec(disX, disY)
if not b[1]:
dx += rX / absvec(rX, rY)
dy += rY / absvec(rX, rY)
else:
dx -= rX / absvec(rX, rY)
dy -= rY / absvec(rX, rY)
dx += a[1] / absvec(a[1], a[2])
dy += a[2] / absvec(a[1], a[2])
# all zones empty
else:
dx = a[1]
dy = a[2]
# randomness factor / error
#dx += random.uniform(-1, 1)
#dy += random.uniform(-1, 1)
#normalise
dx = dx / absvec(dx, dy)
dy = dy / absvec(dx, dy)
#prefered direction
dx = (dx + (a[6] * a[5][0]) ) / absvec((dx + (a[6] * a[5][0])) , (dy + (a[6] * a[5][1])))
dy = (dy + (a[6] * a[5][1]) ) / absvec((dx + (a[6] * a[5][0])) , (dy + (a[6] * a[5][1])))
return [dx, dy]
def update_leadership(agent, agents, maxV, winWidth, winHeight, window, maxTurn, radTurn, negRadTurn,zor_r, zoo_r, zoa_r, blind, speed, free):
# Velocity update
for agent in agents:
neigh_matrix = neigbour_in_zones_free(agent, agents, zor_r, zoo_r, zoa_r,blind,winWidth, winHeight)
agent[4] = updateV_leadership(agent, neigh_matrix, maxV)
# move, draw
for agent in agents:
# alpha = calc_angle(agent[1], agent[2],agent[4][0],agent[4][1])
# test if in ragne of maxturn, if not rotate angle by maxTurn in
# direction of new direction
angle_old = math.atan2(agent[2], agent[1])
angle_new = math.atan2(agent[4][1], agent[4][0])
alpha = math.degrees(angle_old - angle_new)
if abs(alpha) > 180:
if alpha < 0:
alpha += 360
else:
alpha -= 360
if abs(alpha)<maxTurn:
agent[1] = agent[4][0]
agent[2] = agent[4][1]
elif alpha < 0:
agent[1] = agent[1] * math.cos(radTurn) - agent[2] * math.sin(radTurn)
agent[2] = agent[1] * math.sin(radTurn) + agent[2] * math.cos(radTurn)
else:
agent[1] = agent[1] * math.cos(negRadTurn) - agent[2] * math.sin(negRadTurn)
agent[2] = agent[1] * math.sin(negRadTurn) + agent[2] * math.cos(negRadTurn)
#agent[1] = agent[4][0]
#agent[2] = agent[4][1]
# normalise diection vector to 1, and multiply by constant speed
agent[1] = agent[1]/absvec(agent[1], agent[2]) * speed
agent[2] = agent[2]/absvec(agent[1], agent[2]) * speed
if free:
agent = checkBoundary_free(agent, winWidth, winHeight)
else:
agent = checkBoundary(agent, winWidth, winHeight)
# draw arrow
agent[3].undraw()
agent[3] = Line(agent[0], Point(agent[0].getX() + agent[1], agent[0].getY() + agent[2]))
agent[3].setArrow("last")
if (agent[6]!=0):
agent[3].setFill("red")
agent[3].draw(window)
return agents
#---------------vicsek
def update_vicsek(agent, agents, maxV, winWidth, winHeight, window, zoo_r,speed):
return update_couzin(agent, agents, maxV, winWidth, winHeight, window, 180, math.radians(180), -math.radians(180), 0, zoo_r, 0, 0,speed, True)
# -----------------plot stuff
def avgdistancetoall(agent,agents):
i= 0
totaldistance = 0
for i in range (len (agents)):
if agents[i] == agent:
True
else:
totaldistance += distance (agent[0].getX(),agent[0].getY(),agents[i][0].getX(),agents[i][0].getY())
avgdistance = totaldistance/i
return avgdistance
def totalavgdistance(agents,numplist):
for agent in agents:
numplist = np.append(numplist,avgdistancetoall(agent,agents))
return numplist
def main():
winWidth = 500
winHeight = 500
window = GraphWin("Window", winWidth, winHeight)
maxTime = 4000
maxV = 8
speed = 2 # constant speed
blind = 80 # angle of blindness
maxTurn = 50 # maximum turning angle
radTurn = math.radians(maxTurn)
negRadTurn = math.radians(360-maxTurn)
agentNum = 50
infNum = 10
prefDir = [0 ,1]
weight = 1
# data for plotting
distancedata = np.array([])
distancestd = np.array([])
numpycount = np.array([])
# zones for couzin, vicsek
# radii of zones
# swarm: 10, 20, 200
# torus: 5, 60, 200
# dynamic parallel group: 5, 100, 200
# highly parallel group: 5, 180, 200
zor_r = 10
zoo_r = 100
zoa_r = 200
agents = [[0 for x in range(7)] for y in range(agentNum)]
#generate point
# 0 Point
# 1 XVelocity
# 2 YVelocity
# 3 Line
# 4 temp. VelocityPoint
# 5 prefered directions
# 6 weight for pref
for agent in agents:
agent[0] = Point(random.uniform(0,winWidth), random.uniform(0,winHeight))
agent[1] = random.uniform(-2,2)
agent[2] = random.uniform(-2,2)
agent[0].draw(window)
agent[3] = Line(agent[0], Point(agent[0].getX() + agent[1], agent[0].getY() + agent[2]))
agent[3].setArrow("last")
agent[3].draw(window)
agent[5] = [0,0]
agent[6] = 0
for i in range(infNum):
agents[i][5] = prefDir
agents[i][6] = weight
#update points
for i in range(maxTime):
rawdata = totalavgdistance(agents,distancedata)
#print ("rawdata: "+ str(rawdata))
distancedata = np.append(distancedata,np.mean(rawdata))
distancestd = np.append(distancestd,np.std(rawdata))
numpycount = np.append(numpycount,i)
#agents = update_simplest(agent, agents, maxV, winWidth, winHeight, window)
#agents = update_couzin(agent, agents, maxV, winWidth, winHeight, window, maxTurn, radTurn, negRadTurn, zor_r, zoo_r, zoa_r, blind,speed, False)
#agents = update_vicsek(agent, agents, maxV, winWidth, winHeight, window, zoo_r, speed)
agents = update_leadership(agent, agents, maxV, winWidth, winHeight, window, maxTurn, radTurn, negRadTurn, zor_r, zoo_r, zoa_r, blind,speed, True)
time.sleep(0.01)
plt.errorbar(numpycount,distancedata,yerr=distancestd)
#plt.show()
window.getMouse()
window.close()
main()
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment