Update couzin.py

couzin.py

@@ -32,6 +32,21 @@ def absvec(a, 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))
+
 def neigbour_in_zones(a, aas, zor_r, zoo_r, zoa_r):
     zor = []
     zoo = []
@@ -41,13 +56,15 @@ def neigbour_in_zones(a, aas, zor_r, zoo_r, zoa_r):
         if (a == agent):
             True
         else:
-            dis = distance(a[0].getX(), a[0].getY(), agent[0].getX(), agent[0].getY())
+            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]
 
 
@@ -136,19 +153,8 @@ def updateV_couzin(a, matrix, maxV):
         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
+    dx += random.uniform(-1, 1)
+    dy += random.uniform(-1, 1)
 
     return [dx, dy]
 
@@ -176,16 +182,22 @@ def main():
     winWidth = 500
     winHeight = 500
 
-    zor_r = 50
+    zor_r = 20
     zoo_r = 100
-    zoa_r = 200
+    zoa_r = 120
 
     window = GraphWin("Window", winWidth, winHeight)
 
-    maxTime = 20
+    maxTime = 4000
     maxV = 8
-    agentNum = 20
-    agents2 = [[0 for x in range(5)] for y in range(agentNum)]
+    speed = 8
+
+    maxTurn = 80
+    radTurn = math.radians(maxTurn)
+    negRadTurn = math.radians(360-maxTurn)
+
+    agentNum = 50
+    agents = [[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
@@ -193,7 +205,8 @@ def main():
     # 2 YVelocity
     # 3 Line
     # 4 temp. VelocityPoint
-    for agent in agents2:
+
+    for agent in agents:
         agent[0] = Point(random.uniform(0,winWidth), random.uniform(0,winHeight))
 
         agent[1] = random.uniform(-2,2)
@@ -202,11 +215,16 @@ def main():
         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[3].draw(window)
 
-    agentA = [Point(200, 200) , 0, 0,None,[0,0]]
+    """agentA = [Point(200, 200) , 0, 0,None,[0,0]]
     agentB = [Point(205, 200) , 0, 0,None,[0,0]]
-    agents = [agentA, agentB]
+    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]
     #update points
     for i in range(maxTime):
         # Velocity update
@@ -214,20 +232,34 @@ def main():
             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]))
+            #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])
+
+            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)
+
+            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)
 
 
-            #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)
+            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()