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)