Skip to content
Snippets Groups Projects
Commit 8f1789b9 authored by phwitte's avatar phwitte
Browse files

Upload New File

parent d6c8cb99
Branches
No related tags found
No related merge requests found
from graphics import *
import time
import random
import math
# -------------------------------------------------------------------
# help functions
# Distance function betwen points xi, xii and yj,yii
def distance(xi,xii,yi,yii):
sq1 = (xi-xii)*(xi-xii)
sq2 = (yi-yii)*(yi-yii)
return math.sqrt(sq1 + sq2)
#abs of a vector
def absvec(a, b):
m = math.sqrt(a*a + b*b)
if m == 0: m = 0.001
return m
# angle between vectors x= (x1,y1) and y= (x2,y2), in degrees
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))
# -------------------------------------------------------------------
# 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
# -------------------------------------------------------------------
# couzin
# 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:
#alpha = calc_angle(agent[0].getX(), agent[0].getY(),a[1],a[2])
alpha = calc_angle(agent[1], agent[2],a[1],a[2])
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]
#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]
# check for window boundaries and move agents
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
def main():
winWidth = 500
winHeight = 500
window = GraphWin("Window", winWidth, winHeight)
maxTime = 4000
# radii of zones
zor_r = 20
zoo_r = 100
zoa_r = 120
maxV = 8 # maxVelocity
speed = 8 # constant speed
blind = 30 # angle of blindness
maxTurn = 80
radTurn = math.radians(maxTurn)
negRadTurn = math.radians(360-maxTurn)
agentNum = 50
#generate agent
# 0 Point
# 1 XVelocity
# 2 YVelocity
# 3 Line
# 4 temp. VelocityPoint
agents = [[0 for x in range(5)] for y in range(agentNum)]
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)
"""for testing
agentA = [Point(200, 200) , 0, 0,None,[0,0]]
agentB = [Point(205, 200) , 0, 0,None,[0,0]]
agentC = [Point(210, 200) , 0, 0,None,[0,0]]
agentA[0].draw(window)
agentB[0].draw(window)
agentC[0].draw(window)
agents = [agentA, agentB, agentC]"""
#main loop
for i in range(maxTime):
# 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)
#print str(i) + " zor: " + str(len(neigh_matrix[0]))
#print str(i) + " zoo: " + str(len(neigh_matrix[1]))
#print str(i) + " zoa: " + str(len(neigh_matrix[2]))
# 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
if alpha < maxTurn or alpha > 360-maxTurn:
agent[1] = agent[4][0]
agent[2] = agent[4][1]
elif alpha < 180:
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] = 1/absvec(agent[1], agent[2]) * agent[1] * speed
agent[2] = 1/absvec(agent[1], agent[2]) * agent[2] * speed
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)
time.sleep(0.01)
window.getMouse()
window.close()
main()
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment