def flocking(current, radius, kva, ks, kc, ke): agents = current.group.neibourgh_list print len(agents), "length agents" print current.tag neighbor_count = 0 velAvg = PVector(0, 0) centroid = PVector(0, 0) separation = PVector(0, 0) cohesion = PVector(0, 0) obstacle = PVector(0, 0) desired_velocity = PVector(0, 0) position = PVector(current.xyz[0], current.xyz[1]) theta = 0 alt_d = 10 limitX = 15 limitY = 15 avoid_vector = numpy.array([0.0, 0.0]) avoid_coefficient = 0 is_fire = locate_fire(current, position) if is_fire == True: #avoid_vector=is_fire avoid_coefficient = -1 if len(agents) == 0: velocity = PVector(current.v_ned_d[0], current.v_ned_d[1]) return velocity.return_as_vector() #We check all the agents on the screen. #Any agent closer than radius units is a neighbor. for it in agents: neighbor = PVector(it.xyz[0], it.xyz[1]) relative_position = PVector(0, 0) neighbor.subVector(position) #relative_position.addVector(neighbor) #relative_position=PVector(relativePosition[0],relativePosition[1]) d = math.sqrt( pow((position.x - neighbor.x), 2) + pow((position.y - neighbor.y), 2)) if d < 10000: #We have found a neighbor neighbor_count = neighbor_count + 1 #We add all the positions #centroid += it->getPosition(); it_position = PVector(it.xyz[0], it.xyz[1]) centroid.addVector(it_position) it_velocity = PVector(it.v_ned[0], it.v_ned[1]) #We add all the velocities velAvg.addVector(it_velocity) #Vector pointing at the opposite direction w.r.t. your #neighbor #separation -= relativePosition; separation.subVector(relative_position) if neighbor_count == 0: velocity = PVector(current.v_ned_d[0], current.v_ned_d[1]) return velocity.return_as_vector() centroid.divScalar( neighbor_count) # All the positions over the num of neighbors velAvg.divScalar( neighbor_count) # All the velocities over the numb of neighbors #Relative position of the agent w.r.t. centroid centroid.subVector(position) cohesion.addVector(centroid) #In order to compare the following vectors we normalize all of them, # so they have the same magnitude. Later on with the gains #kva, ks and kc we assing which vectors are more important. velAvg.normalize() cohesion.normalize() separation.normalize() if neighbor_count == 7: print "I am here" #desired_velocity = velocity #desiredVel=PVector(current.v_ned_d[0],current.v_ned_d[1]) #desired_velocity=desiredVel.return_as_vector() else: vel_avg = velAvg.return_as_vector() v_separation = separation.return_as_vector() v_cohesion = cohesion.return_as_vector() v_target = tend_to_place(agents, current) random_walk = randomWalkb(position.x, position.y) v_bound_position = bound_position(17, -17, 17, -17, position.x, position.y, 3) desired_velocity = kva * vel_avg + ks * v_separation + kc * v_cohesion + ke * v_target #+avoid_coefficient*position.return_as_vector() desiredVel = PVector(desired_velocity[0], desired_velocity[1]) if (desiredVel.magnitude() > 2): desiredVel.normalize() desiredVel.mulScalar(2) desired_vel = desiredVel.return_as_vector() current.set_v_2D_alt_lya(desired_vel, -alt_d)
def flocking(agents, current, radius, kva, ks, kc, ke): neighbor_count = 0 velAvg = PVector(0, 0) centroid = PVector(0, 0) separation = PVector(0, 0) cohesion = PVector(0, 0) obstacle = PVector(0, 0) desired_velocity = PVector(0, 0) position = PVector(current.xyz[0], current.xyz[1]) theta = 0 alt_d = 10 limitX = 15 limitY = 15 #We check all the agents on the screen. #Any agent closer than radius units is a neighbor. for it in agents: neighbor = PVector(it.xyz[0], it.xyz[1]) relative_position = PVector(0, 0) neighbor.subVector(position) #relative_position.addVector(neighbor) #relative_position=PVector(relativePosition[0],relativePosition[1]) d = math.sqrt( pow((position.x - neighbor.x), 2) + pow((position.y - neighbor.y), 2)) if d / 10 < radius: #We have found a neighbor neighbor_count = neighbor_count + 1 #We add all the positions #centroid += it->getPosition(); it_position = PVector(it.xyz[0], it.xyz[1]) centroid.addVector(it_position) it_velocity = PVector(it.v_ned[0], it.v_ned[1]) #We add all the velocities velAvg.addVector(it_velocity) #Vector pointing at the opposite direction w.r.t. your #neighbor #separation -= relativePosition; separation.subVector(relative_position) if neighbor_count == 0: velocity = PVector(current.v_ned_d[0], current.v_ned_d[1]) return velocity.return_as_vector() centroid.divScalar( neighbor_count) # All the positions over the num of neighbors velAvg.divScalar( neighbor_count) # All the velocities over the numb of neighbors #Relative position of the agent w.r.t. centroid centroid.subVector(position) cohesion.addVector(centroid) #In order to compare the following vectors we normalize all of them, # so they have the same magnitude. Later on with the gains #kva, ks and kc we assing which vectors are more important. velAvg.normalize() cohesion.normalize() separation.normalize() if neighbor_count == 1: #desired_velocity = velocity desiredVel = PVector(current.v_ned_d[0], current.v_ned_d[1]) desired_velocity = desiredVel.return_as_vector() else: vel_avg = velAvg.return_as_vector() v_separation = separation.return_as_vector() v_cohesion = cohesion.return_as_vector() v_target = tend_to_place(agents, current) random_walk = randomWalkb(position.x, position.y) v_bound_position = bound_position(17, -17, 17, -17, position.x, position.y, 3) #avoid_vector=getAvoidAvoids(position,1) avoid_vector = apply_force(position, obstacle, current) desired_velocity = kva * vel_avg + ks * v_separation + kc * v_cohesion #+5*ke*v_target #kva*vel_avg + ks*v_separation + 5*kc*v_cohesion + 3*ke*v_bound_position+ke*v_target #kva*vel_avg + ks*v_separation + kc*v_cohesion +ke*v_bound_position+ks*avoid_vector desiredVel = PVector(desired_velocity[0], desired_velocity[1]) #desired_velocity +=kva*velAvg + ks*separation + kc*cohesion; if (desiredVel.magnitude() > 2): desiredVel.normalize() desiredVel.mulScalar(2) desired_vel = desiredVel.return_as_vector() current.set_v_2D_alt_lya(desired_vel, -alt_d)