diff --git a/couzin.py b/couzin.py new file mode 100644 index 0000000000000000000000000000000000000000..03fb28e63f8db9d941927e6a177d51e1dce360c0 --- /dev/null +++ b/couzin.py @@ -0,0 +1,236 @@ + +from graphics import * +import time +import random +import math + +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 + +# Distance function +def distance(xi,xii,yi,yii): + sq1 = (xi-xii)*(xi-xii) + sq2 = (yi-yii)*(yi-yii) + return math.sqrt(sq1 + sq2) + +def absvec(a, b): + m = math.sqrt(a*a + b*b) + if m == 0: m = 0.001 + return m + +def neigbour_in_zones(a, aas, zor_r, zoo_r, zoa_r): + zor = [] + zoo = [] + zoa = [] + + for agent in aas: + if (a == agent): + True + else: + dis = distance(a[0].getX(), a[0].getY(), agent[0].getX(), agent[0].getY()) + if dis <= zor_r: + zor.append(agent) + elif dis <= zoo_r: + zoo.append(agent) + elif dis <= zoa_r: + zoa.append(agent) + return [zor, zoo, zoa] + + +# 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 + +#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) + + diX = rX / absvec(rX, rY) + diY = rY / absvec(rX, rY) + + dx += diX + dy += diY + #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) ) + 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() + # dx+= (disX / abs(disX)) * abs(abs(disX)/ disX ) +# +# disY = agent[0].getY() - a[0].getY() +# dy+= (disY / abs(disY)) * abs(abs(disY) / disY ) + for agent in matrix[2]: + disX = agent[0].getX() - a[0].getX() + disY = agent[0].getY() - a[0].getY() + + 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) ) + # 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() + + 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) ) + + dx = 0.5*dx + dy = 0.5*dy + else: + dx = a[1] + dy = a[2] + + if abs(dx) < maxV : + True + elif dx <= -maxV : + dx = -maxV + else: + dx = maxV + + if abs(dy) < maxV : + True + elif dy <= -maxV : + dy = -maxV + else: + dy = maxV + + return [dx, dy] + +# 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 + +def main(): + winWidth = 500 + winHeight = 500 + + zor_r = 50 + zoo_r = 100 + zoa_r = 200 + + window = GraphWin("Window", winWidth, winHeight) + + maxTime = 20 + maxV = 8 + agentNum = 20 + agents2 = [[0 for x in range(5)] for y in range(agentNum)] + #directions = [[0 for x in range(2)] for y in range(agentNum)] + #generate point + # 0 Point + # 1 XVelocity + # 2 YVelocity + # 3 Line + # 4 temp. VelocityPoint + for agent in agents2: + 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) + + agentA = [Point(200, 200) , 0, 0,None,[0,0]] + agentB = [Point(205, 200) , 0, 0,None,[0,0]] + agents = [agentA, agentB] + #update points + for i in range(maxTime): + # Velocity update + for agent in agents: + neigh_matrix = neigbour_in_zones(agent, agents, zor_r, zoo_r, zoa_r) + agent[4] = updateV_couzin(agent, neigh_matrix, maxV) + + print (i ++ " zor: " ++ len(neigh_matrix[0])) + + # move, draw + for agent in agents: + agent[1] = agent[4][0] + agent[2] = agent[4][1] + + 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) + time.sleep(0.01) + + window.getMouse() + window.close() + +main()