def tend_to_place(agents, current):
target = PVector(0, -14)
position = PVector(current.xyz[0], current.xyz[1])
target.subVector(position)
target.divScalar(50)
target.normalize()
return target.return_as_vector()
def apply_force_from_coords(ox, oy, position, current_drone):
"""Apply a simple short range repulsive force from a particle at
the given coordinates on this particle."""
ax = 0
ay = 0
radius = 4
empty = PVector(0, 0)
dx = ox - position.x
dy = oy - position.y
if dx == dy == 0:
return empty.return_as_vector(
) # no directional force from particle at same location
r2 = max(dx * dx + dy * dy, current_drone.min_r2)
if r2 > radius:
return empty.return_as_vector() # out of force range
r = math.sqrt(r2)
# Very simple short range repulsive force
coef = (1 - current_drone.cutoff / r) / r2 / current_drone.mass
ax += coef * dx
ay += coef * dy
direction = PVector(ax, ay)
direction.normalize()
return direction.return_as_vector()
def randomWalkb(x, y):
new = numpy.random.randint(1, 4)
if new == 1:
x += 1
elif new == 2:
y += 1
elif new == 3:
x += -1
else:
y += -1
new_position = PVector(x, y)
new_position.normalize()
return new_position.return_as_vector()
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 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 normalize(vector): vector=PVector(vector[0],vector[1]) vector.normalize() return vector.return_as_vector()
class Enemy(pygame.sprite.Sprite):
respawn = 600
limit = 50
animation_cycle = 120
image_data = {}
images = [0,0]
rects = {}
def __init__(self, player_pos, visual, close, mover, chaser, fastie):
pygame.sprite.Sprite.__init__(self, self.containers)
self.pos = [0, 0]
if close:
self.pos[0] = player_pos[0] * random_direction() + random.randint(70, 100)
self.pos[1] = player_pos[1] * random_direction() + random.randint(70, 100)
else:
self.pos[0] = player_pos[0] * random_direction() + random.randint(300, 500)
self.pos[1] = player_pos[1] * random_direction() + random.randint(300, 500)
if visual == "blob":
self.images = Enemy.image_data['blob']
elif visual == "butterfly":
self.images = Enemy.image_data['butterfly']
elif visual == "octopus":
self.images = Enemy.image_data['octopus']
else:
self.images = Enemy.image_data['blob']
self.image = self.images[0]
# if visual == "blob":
# self.image = Enemy.images['blob']
# elif visual == "butterfly":
# self.image = Enemy.images['butterfly']
# elif visual == "octopus":
# self.image = Enemy.images['octopus']
# else:
# self.image = Enemy.images['blob']
self.rect = self.image.get_rect()
self.rect.move_ip(Constant.SCREEN_RECT.width/2, Constant.SCREEN_RECT.bottom)
self.mask = pygame.mask.from_surface(self.image)
self.frame = 0
# self.image = pygame.Surface([20, 20])
# pygame.draw.circle(self.image, Color("red"), (10, 10), 10, 0)
self.rect.center = (self.pos[0], self.pos[1])
self.close = close
self.mover = mover
self.chaser = chaser
self.fastie = fastie
if fastie:
self.speed = random.randint(2,4)
else:
self.speed = 1
self.direction = None
GameState.enemies_spawned += 1
def move(self, player_pos):
mouse_pos = pygame.mouse.get_pos()
angle = math.atan2(player_pos[0] - self.pos[0], player_pos[1] - self.pos[1]) + math.pi
self.angle = angle * 57.2957795 # Radians to degrees
if Bounds.outside(self.pos[0], self.pos[1]) or self.chaser:
self.direction = PVector(player_pos[0], player_pos[1]) - PVector(self.pos[0], self.pos[1])
else:
self.direction = PVector(random.randint(0,640), random.randint(0,480)) - PVector(self.pos[0], self.pos[1])
self.direction.normalize()
self.direction.mag = self.speed
self.pos[0] = self.pos[0] + self.direction.x * self.direction.mag
self.pos[1] = self.pos[1] + self.direction.y * self.direction.mag
def update(self):
self.frame = pygame.time.get_ticks()
self.rect.center = (self.pos[0], self.pos[1])
self.image = self.images[self.frame//self.animation_cycle % 2]
self.image = pygame.transform.rotate(self.image, self.angle)
@property
def value(self):
value = 1
if self.close:
value += 1
if self.mover:
value += 2
if self.fastie:
value *= 2
if self.chaser:
value += 3
return value
class Enemy(pygame.sprite.Sprite):
respawn = 600
limit = 50
animation_cycle = 120
image_data = {}
images = [0, 0]
rects = {}
def __init__(self, player_pos, visual, close, mover, chaser, fastie):
pygame.sprite.Sprite.__init__(self, self.containers)
self.pos = [0, 0]
if close:
self.pos[0] = player_pos[0] * random_direction() + random.randint(
70, 100)
self.pos[1] = player_pos[1] * random_direction() + random.randint(
70, 100)
else:
self.pos[0] = player_pos[0] * random_direction() + random.randint(
300, 500)
self.pos[1] = player_pos[1] * random_direction() + random.randint(
300, 500)
if visual == "blob":
self.images = Enemy.image_data['blob']
elif visual == "butterfly":
self.images = Enemy.image_data['butterfly']
elif visual == "octopus":
self.images = Enemy.image_data['octopus']
else:
self.images = Enemy.image_data['blob']
self.image = self.images[0]
# if visual == "blob":
# self.image = Enemy.images['blob']
# elif visual == "butterfly":
# self.image = Enemy.images['butterfly']
# elif visual == "octopus":
# self.image = Enemy.images['octopus']
# else:
# self.image = Enemy.images['blob']
self.rect = self.image.get_rect()
self.rect.move_ip(Constant.SCREEN_RECT.width / 2,
Constant.SCREEN_RECT.bottom)
self.mask = pygame.mask.from_surface(self.image)
self.frame = 0
# self.image = pygame.Surface([20, 20])
# pygame.draw.circle(self.image, Color("red"), (10, 10), 10, 0)
self.rect.center = (self.pos[0], self.pos[1])
self.close = close
self.mover = mover
self.chaser = chaser
self.fastie = fastie
if fastie:
self.speed = random.randint(2, 4)
else:
self.speed = 1
self.direction = None
GameState.enemies_spawned += 1
def move(self, player_pos):
mouse_pos = pygame.mouse.get_pos()
angle = math.atan2(player_pos[0] - self.pos[0],
player_pos[1] - self.pos[1]) + math.pi
self.angle = angle * 57.2957795 # Radians to degrees
if Bounds.outside(self.pos[0], self.pos[1]) or self.chaser:
self.direction = PVector(player_pos[0], player_pos[1]) - PVector(
self.pos[0], self.pos[1])
else:
self.direction = PVector(random.randint(0, 640),
random.randint(0, 480)) - PVector(
self.pos[0], self.pos[1])
self.direction.normalize()
self.direction.mag = self.speed
self.pos[0] = self.pos[0] + self.direction.x * self.direction.mag
self.pos[1] = self.pos[1] + self.direction.y * self.direction.mag
def update(self):
self.frame = pygame.time.get_ticks()
self.rect.center = (self.pos[0], self.pos[1])
self.image = self.images[self.frame // self.animation_cycle % 2]
self.image = pygame.transform.rotate(self.image, self.angle)
@property
def value(self):
value = 1
if self.close:
value += 1
if self.mover:
value += 2
if self.fastie:
value *= 2
if self.chaser:
value += 3
return value
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)
