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)
