def cohesion(boids, current_drone): neighbordist = 80 sum_vector = PVector(0, 0) count = 0 position = current_drone.xyz c_vector = PVector(position[0], position[1]) for other in boids: if other.tag != current_drone.tag: other_position = other.xyz o_vector = PVector(other_position[0], other_position[1]) distance = c_vector.distance(o_vector) if distance < neighbordist: sum_vector.addVector(o_vector) count = count + 1 if count > 0: sum_vector.divScalar(count) return seek(sum_vector, current_drone.xyz).return_as_vector() else: return PVector(0, 0).return_as_vector()
def separation(boids, current_drone): alt_d = 4 desiredseparation = 50 steer = PVector(0.0, 0.0) count = 0 position = current_drone.xyz c_vector = PVector(position[0], position[1]) for other in boids: if other.tag != current_drone.tag: other_position = other.xyz o_vector = PVector(other_position[0], other_position[1]) distance = c_vector.distance(o_vector) if distance < desiredseparation: c_vector.subVector(o_vector) c_vector.normalize() c_vector.divScalar(distance) steer.addVector(c_vector) count = count + 1 if count > 0: steer.divScalar(count) vres = steer.return_as_vector() #current_drone.set_a_2D_alt_lya(vres[0:2],-alt_d) return vres
def getAvoidAvoids(position, avoid_radius): relative_position = PVector(0, 0) diff = PVector(0, 0) steer = PVector(0, 0) obstacle_position = PVector(0.0, 0.0) count = 0 obstacle_position.subVector(position) relative_position.addVector(obstacle_position) d = math.sqrt( pow((position.x - obstacle_position.x), 2) + pow((position.y - obstacle_position.y), 2)) #for obstacle : obstacles : # If the distance is greater than 0 and less than an arbitrary amount (0 when you are yourself) if relative_position.normalize() < avoid_radius: #Calculate vector pointing away from neighbor position.subVector(obstacle_position) diff.addVector(position) diff.normalize() diff.divScalar(d) #Weight by distance steer.addVector(diff) #count++; // Keep track of how many return steer.return_as_vector()
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 velocity = PVector(current.v_ned_d[0], current.v_ned_d[1]) desired_velocity = velocity.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, 0.1) #avoid_vector=getAvoidAvoids(position,1) avoid_vector = apply_force(position, obstacle, current) desired_velocity = 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; error_theta = math.atan2(desired_velocity[1], desired_velocity[0]) - theta error_theta = ke * error_theta #updateUnicycle(0, ke*error_theta); current.set_v_2D_alt_lya(error_theta, -alt_d)
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)