# FlockingUpdateRules2.py # This program defines functions to calculate the position of a single bird, and of the entire flock # after one timestep. These functions will be used by the core program to generate movement of # the flock over longer time periods. from numpy import * from Scientific.Geometry import * from FlockingClassDefinitions import * # Function BirdStep # This function is used to calculate the position of a single bird in the flock after one timestep. The algorithm # is both local and autonomous. That is each bird independently calculates where to move based upon local # information (the current position and velocity of other birds within its radius of perception - 'its neighbors'). # To be somewhat realistic each bird has a current velocity, v_current, and only modifies this velocity by a small # amount of acceleration a_current at each time step. The velocity of the bird at the next timestep is calculated # using the formula v_next = v_current + a_current* timestep, and the position at the next timestep is calulated # using the formula p_next = p_current + v_current*timestep + a_current*timestep^2. These accelerations and # velocities are, of course, vectors and their magnitude is limited by the birds maximum acceleration MA and # maximum velocity MV. # Three factors are taken into account for a bird determing how it should adjust its trajectory at each timestep # (i.e. how it calculates a_current). They are: # (1) Static Collision Avoidance (steer away from other birds or stationary obstacles which are too close). # (2) Velocity Matching (adjust your velocity to match the average velocity of your neighbors) # (3) Flock Centering (move towards the average position of your neighbors) # A seperate vector of maximum magnitude MA is calculated for each of these factors, and then a_current is deteremined # by summing them in order of priority until a total magnitude MA is reached or all factors are added in (whichever # happens first). This gives us a prelimary determination of a_current. We then compute v_next using this value # of a_current. If the magnitude of v_next <= MV we are ok, but if the magnitude of v_next is > MV we must scale # v_next to have magnitude MV and then recalculate a_current accordingly. Finally, we use this value of a_current # along with p_current and v_current to calculate p_next. # NOTE: Positions of all birds in the flock are updated simaltaneously. FlockCopy contains the current positions and # velocities of all birds in the flock. This information is used to update the positions and velocities of all # birds at the next time step. The updated info is stored in Flock. This information is changed one bird at a time # as we loop through all birds in the flock using the function 'FlockStep' defined below. # NOTE: after initially assigning the flock by birds 1 by 1. We forget about the individuals variables b1, b2, ... # and only modify the variables Flock[0],Flock[1],.... def BirdStep(bird_number, Flock, FlockCopy, dmin, timestep, gridsize,Obstacles): # find the bird and it's copy pair bird = FlockCopy.ListOfBirds[bird_number] birdcopy = FlockCopy.ListOfBirds[bird_number] # since we are updating bird.p_last = bird.p_current # for convenience define MA = bird.MA MV = bird.MV sightrange = bird.sightrange # Set the neighbors total Avg_Position, Avg_Velocity to 0. # Set the distance to the closest obstacle to the smallest possible radius dmin. # Set obstacles = 'false', since we haven't found any obstacles yet Avg_Position = Vector(0.0,0.0,0.0) Avg_Velocity = Vector(0.0,0.0,0.0) closest_obstacle_dist = dmin obstacles = 'false' # loop through the list of neighbors and add to Avg_Position & Avg_Velocity. # also check for closest obstacle. for b in birdcopy.neighbors: l = len(b) number = int (b[1:l]) neighbor = FlockCopy.ListOfBirds[number] Avg_Position = Avg_Position + neighbor.p_current Avg_Velocity = Avg_Velocity + neighbor.v_current if (bird.p_current - neighbor.p_current).length() < closest_obstacle_dist: closest_obstacle_dist = (bird.p_current - neighbor.p_current).length() closest_obstacle_position = neighbor.p_current obstacles = 'true' # check for non-bird fixed obstacles if len(Obstacles) > 0: closest_fixed_obstacle_dist = 2.0*dmin for o in Obstacles: if (bird.p_current - o).length() < closest_fixed_obstacle_dist: closest_fixed_obstacle_dist = (bird.p_current - o).length() closest_fixed_obstacle_position = o obstacles = 'true' if closest_fixed_obstacle_dist/2.0 < closest_obstacle_dist: closest_obstacle_position = (closest_fixed_obstacle_position - bird.p_current)/2.0 + bird.p_current # divide by total # of neighbors to find real Avg_Position, Avg_Velocity. if len(birdcopy.neighbors) != 0: Avg_Position = Avg_Position/len(birdcopy.neighbors) Avg_Velocity = Avg_Velocity/len(birdcopy.neighbors) # Use all this info to calculate the acceleration factors due to static collision avoidance, # velocity matching, and flock centering (a_static, a_velocity, a_center). a_velocity = Vector([0.0,0.0,0.0]) a_center = Vector([0.0,0.0,0.0]) a_static = Vector([0.0,0.0,0.0]) # Simple computation of a_velocity, a_center # Note: the 3.0 below can be adjusted but this value seems to work well. if len(birdcopy.neighbors) != 0: a_velocity = ( (Avg_Velocity - bird.v_current)/(Avg_Velocity - bird.v_current).length() ) * (MA/3.0) a_center = ( (Avg_Position - bird.p_current)/(Avg_Position - bird.p_current).length() ) * (MA/3.0) # More complicated computation of a_static involving both moving directly away and turning away. # That is where the cross product comes in. if obstacles == 'true': # Define the turn left matrix (TLM) and turn right matrix (TRM) TLM = array([[0.0,-1.0],[1.0,0.0]]) TRM = array([[0.0,1.0],[-1.0,0.0]]) # the birds current velocity and displacement from current position to closest obstacle v = bird.v_current u = closest_obstacle_position - bird.p_current # take the cross product, look at z-component. If w_z > 0 then rotate left. If w_z < 0 then rotate right. w = u.cross(v) if w[2] >= 0: vv = array([ v[0],v[1] ]) turn = dot(TLM,vv) turn = Vector(turn[0],turn[1],0.0) if w[2] < 0: vv = array([ v[0],v[1] ]) turn = dot(TRM,vv) turn = Vector(turn[0],turn[1],0.0) # normalize the turn vector and add it to a direct movement away from the obstacle to get a_current # then rescale a_current depending on how close the obstacle is. (again the 2.0, 0.5 adjustable but this seems to work) turn = ( turn/turn.length() ) * (MA/2.0) dma = closest_obstacle_position - bird.p_current direct_move_away = ( -dma/dma.length() ) * (MA/2.0) a_static = turn + direct_move_away a_static = a_static/(0.5 * (closest_obstacle_position - bird.p_current).length() ) # compute the birds total acceleration, by adding a_static, a_velocity, a_center in order of # priority a_static > a_velocity > a_center until the maximum threshold MA is reached. if a_static.length() >= MA: case = 1.0 elif a_static.length() + a_velocity.length() >= MA: case = 2.0 elif a_static.length() + a_velocity.length() + a_center.length() >= MA: case = 3.0 elif a_static.length() + a_velocity.length() + a_center.length() < MA: case = 4.0 if case == 1.0: a_current = ( a_static / a_static.length() ) * MA elif case == 2.0: length = MA - a_static.length() a_current = a_static + ( a_velocity/a_velocity.length() ) * length elif case == 3.0: length = MA - a_static.length() - a_velocity.length() a_current = a_static + a_velocity + ( a_center/a_center.length() ) * length elif case == 4.0: a_current = a_static + a_velocity + a_center # wait, hold on, you might be near a wall in which case you need to steer away a_wall = Vector(0.0,0.0,0.0) distance = 10.0*gridsize # find closest wall (Comment out this code to remove walls) if bird.p_current[0] < sightrange: closest_wall = Vector(0.0,bird.p_current[1],0.0) distance = (bird.p_current - closest_wall).length() if bird.p_current[0] > gridsize - sightrange: closest_wall = Vector(gridsize,bird.p_current[1],0.0) distance = (bird.p_current - closest_wall).length() if bird.p_current[1] < sightrange: wall = Vector(bird.p_current[0], 0.0, 0.0) distance2 = (bird.p_current - wall).length() if distance2 < distance: closest_wall = wall distance = distance2 if bird.p_current[1] > gridsize - sightrange: wall = Vector(bird.p_current[0], gridsize, 0.0) distance2 = (bird.p_current - wall).length() if distance2 < distance: closest_wall = wall distance = distance2 # if there is a closest wall determine how to steer away from it (find a_wall) if distance < 10*gridsize: # Define the turn left matrix (TLM) and turn right matrix (TRM) TLM = array([[0.0,-1.0],[1.0,0.0]]) TRM = array([[0.0,1.0],[-1.0,0.0]]) # the birds current velocity and displacement from current position to closest wall v = bird.v_current u = closest_wall - bird.p_current # take the cross product, look at z-component. If w_z > 0 then rotate left. If w_z < 0 then rotate right. w = u.cross(v) if w[2] >= 0: vv = array([ v[0],v[1] ]) turn = dot(TLM,vv) turn = Vector(turn[0],turn[1],0.0) if w[2] < 0: vv = array([ v[0],v[1] ]) turn = dot(TRM,vv) turn = Vector(turn[0],turn[1],0.0) # normalize the turn vector and add it to a direct movement away from the obstacle to get a_wall # then rescale a_current depending on how close the obstacle is. turn = ( turn/turn.length() ) * (MA/2.0) dma = closest_wall - bird.p_current direct_move_away = ( -dma/dma.length() ) * (MA/2.0) a_wall = turn + direct_move_away a_wall = a_wall/(0.5 * (closest_wall - bird.p_current).length() ) # average in a_wall to a_current a_current = (a_current + a_wall)/2.0 if a_current.length() > MA: a_current = ( a_current/a_current.length() ) * MA # compute p_next, v_next for the bird, scaling back the acceleration if necessary to not exceed Max Velocity if (bird.v_current + a_current*timestep).length() <= bird.MV: bird.p_current = bird.p_current + bird.v_current*timestep + a_current*timestep**2 bird.v_current = bird.v_current + a_current*timestep if (bird.v_current + a_current*timestep).length() > bird.MV: v_temp = ( (bird.v_current + a_current*timestep)/(bird.v_current + a_current*timestep).length() ) * MV a_current = (v_temp - bird.v_current)/timestep bird.p_current = bird.p_current + bird.v_current*timestep + a_current*timestep**2 bird.v_current = bird.v_current + a_current*timestep # finally reassign our flock member the modified values stored as bird Flock.ListOfBirds[bird_number] = bird # Define the function FindNeighbors # This function determines a birds new neighbors after a movement. For any bird bn, # the neighbors of bn are all birds bm s.t. abs[location(bn)-location(bm)] <= sight range. def FindNeighbors(bird_number, Flock): N = len(Flock.ListOfBirds) bird = Flock.ListOfBirds[bird_number] # set list of neighbors to empty list bird.neighbors = [] # loop over all birds in the flock and add them as neighbors if they are close enough (and are not self) for n in xrange(N): potential_neighbor = Flock.ListOfBirds[n] dvector = bird.p_current - potential_neighbor.p_current d = dvector.length() if d <= bird.sightrange and n != bird_number: bird.neighbors.append(potential_neighbor.label) # finally reassign our flock member with the new set of neighbors Flock.ListOfBirds[bird_number] = bird # Define the function FlockStep # This function moves all birds in the flock one step using the function BirdStep. def FlockStep(Flock,timestep,dmin,gridsize,Obstacles): FlockCopy = Flock N = len(Flock.ListOfBirds) # loop over birds and move them for n in xrange (N): BirdStep(n,Flock,FlockCopy,dmin,timestep,gridsize,Obstacles) # loop over birds and reassing their neighbors for n in xrange(N): FindNeighbors(n,Flock)