vicsek.py

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

# -------------------------------------------------------------------
# -------------------------------------------------------------------
# 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, zoo_r):
    zoo = []
    for agent in aas:
            dis = absvec(agent[0].getX() - a[0].getX() , agent[0].getY() - a[0].getY() )
            if dis <= zoo_r:
                zoo.append(agent)
    #print len(zoo)+len(zor)+len(zoa)
    return zoo

#update Velocity
def updateV_vicsek(a, zoo, noise):

    """                 sum_i_from_1_to_N sin(a[i])
        a = arctangent ---------------------------
                        sum_i_from_1_to_N cos(a[i])

        Nullrichtung: (1,0)
                   """
    dx = 0
    dy = 0
    for agent in zoo:
        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])

    #dx =  1 * math.cos(avgTheta) - 0  * math.sin(avgTheta)
    #dy =  1 * math.sin(avgTheta) + 0  * math.cos(avgTheta)

	# randomness factor / error
    dx += random.uniform(-(noise>>1), noise>>1)
    dy += random.uniform(-(noise>>1), noise>>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] % winWidth
        x = agent[0].getX() % winWidth
        agent[0] = Point(x, y)
        #agent[0].move(agent[1],agent[2])

    elif y <= 0 or y >= winHeight:
        #agent[2] = agent[2] % winHeight
        #agent[0].move(agent[1],agent[2])
        y = agent[0].getY() % winHeight
        agent[0] = Point(x, y)
    return agent

def main():
    L = 500             #size
    agentNum = 20       #number agents
    speed = 8			# constant speed
    noise = 2
    zoo_r = 200


    winWidth = L
    winHeight = L

    window = GraphWin("Window", winWidth, winHeight)
    maxTime = 4000

	# radii of zones
    # swarm: 10, 20, 200
    # torus: 5, 60, 200
    # dynamic parallel group: 5, 100, 200
    # highly parallel group: 5, 180, 200



    maxV = 8 			# maxVelocity


    blind = 100			# angle of blindness

    maxTurn = 80
    radTurn = math.radians(maxTurn)
    negRadTurn = math.radians(360-maxTurn)



    #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:
            zoo = neigbour_in_zones(agent, agents, zoo_r)
            agent[4] = updateV_vicsek(agent, zoo, noise)

            #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:

			# normalise diection vector to 1, and multiply by constant speed
            agent[1] = agent[4][0]
            agent[2] = agent[4][1]

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