Upload New File

couzin.py

+
+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()