Upload New File

Final/python files/leadership_simulation.py

+from graphics import * # get here: http://mcsp.wartburg.edu/zelle/python/graphics.py
+import time
+import random
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+# imports from our files
+from help_functions import *
+from classes import Agent
+from check_boundarys import *
+
+
+# ------------------------------------------------------------------------
+# global variables
+
+# whether average distance will be ploted at end of simulation
+plot =  False
+
+# window dimensions
+winWidth = 500
+winHeight = 500
+
+window = GraphWin("Window", winWidth, winHeight)
+
+# max time for one simulation
+maxTime = 4000
+
+# whether the boundary is "free" or not
+# <->
+# can they swim through the side of the window y/n?
+free = True
+
+# max velocity of the agents
+maxV = 8
+# constant speed of agents
+speed = 4
+
+# number of agents(including leadergroups)
+agentNum = 50
+
+# angle of blindness
+blind = 80
+
+# maximum turning angle
+maxTurn = 50
+radTurn = math.radians(maxTurn)
+negRadTurn = math.radians(360-maxTurn)
+
+# radii of zones
+zor_r = 10
+zoo_r = 80
+zoa_r = 110
+
+# leadergroup 1, red
+
+lgOneNum = 5            # number of agents in group
+prefDirOne = [-1 ,-1]   # preferred direction of group
+weightOne = 0.2         # weight of group, determines how much they move in prefDir
+
+# leadergroup 2, blue
+lgTwoNum = 2            # number of agents in group
+prefDirTwo = [1 ,1]     # preferred direction of group
+weightTwo = 0.2         # weight of group, determines how much they move in prefDir
+
+
+
+# -------------------------------------------------------------------------------
+# leadership simulation
+
+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, x-x2, y-y2]
+
+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.point.getX(), a.point.getY(), agent.point.getX(), agent.point.getY(), winWidth, winHeight)
+        dis = wpd[0]
+        disVecX = wpd[2]
+        disVecY = wpd[3]
+        alpha = calc_angle(a.x_velocity,a.y_velocity, disVecX, disVecY)
+
+        if (a == agent):
+            True
+        elif alpha < 180 - blind and alpha > 180 + blind:
+            True
+        else:
+            dis = absvec(agent.point.getX() - a.point.getX() , agent.point.getY() - a.point.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]])
+
+    return [zor, zoo, zoa]
+
+def updateV_leadership_couzin(a, matrix):
+    dx=0
+    dy=0
+
+    #zor
+    if matrix[0] != []:
+        for agent in matrix[0]:
+
+            disX = agent[0].point.getX() - a.point.getX()
+            disY = agent[0].point.getY() - a.point.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.x_velocity / absvec(agent.x_velocity, agent.y_velocity)
+                dy += agent.y_velocity / absvec(agent.x_velocity, agent.y_velocity)
+            else:
+                dx -= agent.x_velocity / absvec(agent.x_velocity, agent.y_velocity)
+                dy -= agent.y_velocity / absvec(agent.x_velocity, agent.y_velocity)
+
+        dx += a.x_velocity / absvec(a.x_velocity, a.y_velocity)
+        dy += a.y_velocity / absvec(a.x_velocity, a.y_velocity)
+        
+    # zoo leer ; zoa
+    elif matrix[1] == []  and matrix[2] != []:
+        for b in matrix[2]:
+            agent = b[0]
+            disX = agent.point.getX() - a.point.getX()
+            disY = agent.point.getY() - a.point.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) )
+
+    #both zones filled
+    elif matrix[1] != []  and matrix[2] != []:
+
+        for b in matrix[1]:
+            agent = b[0]
+
+            dx += agent.x_velocity / absvec(agent.x_velocity, agent.y_velocity)
+            dy += agent.y_velocity / absvec(agent.x_velocity, agent.y_velocity)
+            
+        for b in matrix[2]:
+            agent = b[0]
+            disX = agent.point.getX() - a.point.getX()
+            disY = agent.point.getY() - a.point.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.x_velocity / absvec(a.x_velocity, a.y_velocity)
+            dy += a.y_velocity / absvec(a.x_velocity, a.y_velocity)
+
+    # all zones empty
+    else:
+        dx = a.x_velocity
+        dy = a.y_velocity
+
+    # 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.weight * a.prefdir[0]) ) / absvec((dx + (a.weight * a.prefdir[0])) , (dy + (a.weight * a.prefdir[1])))
+    dy = (dy + (a.weight * a.prefdir[1]) ) / absvec((dx + (a.weight * a.prefdir[0])) , (dy + (a.weight * a.prefdir[1])))
+
+    return [dx, dy]
+
+# update function
+def update_leadership(agent, agents):
+        # Velocity update
+        for agent  in agents:
+            neigh_matrix = neigbour_in_zones_free(agent, agents, zor_r, zoo_r, zoa_r,blind,winWidth, winHeight)
+            agent.set_temp_velocity(updateV_leadership_couzin(agent, neigh_matrix))
+            
+        # move, draw
+        for agent in agents:
+            # check if rotation is in range of maxTurn
+            agent = move_agent_couzin(agent, maxTurn, radTurn, negRadTurn)
+
+            # if agent is in a leadergroup, find angle between
+            # preferred direction and actual direction,
+            # if angle is smaller than 20% increase weight, else decrease
+            if (agent.prefdir != [0,0]):
+                angle_pref = math.atan2(agent.prefdir[0], agent.prefdir[1])
+                angle_new = math.atan2(agent.x_velocity, agent.y_velocity)
+                beta = math.degrees(angle_new - angle_pref)
+
+                if abs(beta) > 180:
+                    if beta < 0:
+                        beta += 360
+                    else:
+                        beta -= 360
+
+                if abs(beta) < 20 :
+                    agent.set_weight(min(1, agent.weight+0.001))
+                else:
+                    agent.set_weight(max(0, agent.weight-0.0001))
+
+
+	    # normalise direction vector to 1, and multiply by constant speed
+            x = agent.x_velocity/absvec(agent.x_velocity, agent.y_velocity)  * agent.speed
+            agent.y_velocity = agent.y_velocity/absvec(agent.x_velocity, agent.y_velocity)  * agent.speed
+            agent.x_velocity = x
+            
+            if free:
+                agent = checkBoundary_free(agent, winWidth, winHeight)
+            else:
+                agent = checkBoundary(agent, winWidth, winHeight)
+
+	    # draw arrow
+            agent.draw_Line()
+            
+        return agents
+
+
+# -----------------------------------------------------------------------------------
+# main
+def main():
+    
+    agents = []
+
+    if plot:
+        # data for plotting
+        distancedata = np.array([])
+        distancestd = np.array([])
+        numpycount = np.array([])
+
+    # generate agents
+    for i in range(agentNum):
+        
+        agent = Agent(Point(random.uniform(0,winWidth), random.uniform(0,winHeight)),window, maxV)
+        agent.speed = speed
+        # give them a random starting direction
+        agent.set_xvelocity(random.uniform(-2,2))
+        agent.set_yvelocity(random.uniform(-2,2))
+        agent.draw_Line()
+
+        agents.append(agent)
+        
+    # set preferred direction/weight/colour of leadergroup one
+    for i in range(lgOneNum):
+        agents[i].set_prefdir(prefDirOne) 
+        agents[i].set_weight(weightOne)
+        agents[i].set_colour("red")
+
+    # set preferred direction/weight/colour of leadergroup one
+    for i in range(lgOneNum, lgOneNum + lgTwoNum):
+        agents[i].set_prefdir(prefDirTwo) 
+        agents[i].set_weight(weightTwo)
+        agents[i].set_colour("blue")
+
+    # main loop 
+    for i in range(maxTime):
+        if plot:
+            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_leadership(agent, agents)
+        time.sleep(0.01)
+
+    if plot:
+        plt.errorbar(numpycount,distancedata,yerr=distancestd)
+
+
+    window.getMouse()
+    window.close()
+
+main()