Obstacle avoidance
# --- GEOMETRY -------------------------------------------------------------- from math import degrees, atan2 from math import sqrt, pow from math import radians, sin, cos def angle(x0, y0, x1, y1): """ Returns the angle between two points. """ return degrees( atan2(y1-y0, x1-x0) ) def distance(x0, y0, x1, y1): """ Returns the distance between two points. """ return sqrt(pow(x1-x0, 2) + pow(y1-y0, 2)) def coordinates(x0, y0, distance, angle): """ Returns the coordinates of given distance and angle from a point. """ return (x0 + cos(radians(angle)) * distance, y0 + sin(radians(angle)) * distance) # --- WORLD ----------------------------------------------------------------- class World: def __init__(self): self.obstacles = [] # --- OBSTACLE -------------------------------------------------------------- class Obstacle: def __init__(self, x, y, radius): self.x = x self.y = y self.radius = radius # --- CREATURE -------------------------------------------------------------- class Creature: def __init__(self, world, x, y, speed=1.0, size=4): self.x = x self.y = y self.speed = speed self.size = size self.world = world self.feeler_length = 25 self._vx = 0 self._vy = 0 def heading(self): """ Returns the creature's heading as angle in degrees. """ return angle(self.x, self.y, self.x+self._vx, self.y+self._vy) def avoid_obstacles(self, m=0.4, collision=4): # Find out where the creature is going. a = self.heading() for obstacle in self.world.obstacles: # Calculate the distance between the creature and the obstacle. d = distance(self.x, self.y, obstacle.x, obstacle.y) # Respond faster if the creature is very close to an obstacle. if d - obstacle.radius < 4: m *= 10 # Check if the tip of the feeler falls inside the obstacle. # This is never true if the feeler length # is smaller than the distance to the obstable. if d - obstacle.radius <= self.feeler_length: tip_x, tip_y = coordinates(self.x, self.y, d, a) if distance(obstacle.x, obstacle.y, tip_x, tip_y) < obstacle.radius: # Nudge the creature away from the obstacle. m *= self.speed if tip_x < obstacle.x: self._vx -= random(m) if tip_y < obstacle.y: self._vy -= random(m) if tip_x > obstacle.x: self._vx += random(m) if tip_y > obstacle.y: self._vy += random(m) if d - obstacle.radius < 4: return def roam(self): """ Creature changes heading aimlessly. With its feeler it will scan for obstacles and steer away. """ self.avoid_obstacles() v = self.speed self._vx += random(-v, v) self._vy += random(-v, v) self._vx = max(-v, min(self._vx, v)) self._vy = max(-v, min(self._vy, v)) self.x += self._vx self.y += self._vy # --------------------------------------------------------------------------- size(550, 200) # Create a world with obstacles at random positions. world = World() for i in range(15): obstacle = Obstacle(random(WIDTH), random(HEIGHT), random(10, 30)) world.obstacles.append(obstacle) # Create a number of ants. ants = [] for i in range(4): ant = Creature(world, 225, 100, speed=2.0) ants.append(ant) speed(30) def draw(): background(0.2) # Draw all the obstacles in the world. fill(0.5, 0.5) for obstacle in world.obstacles: oval(obstacle.x - obstacle.radius, obstacle.y - obstacle.radius, obstacle.radius * 2, obstacle.radius * 2) # Draw each ant and its feeler. stroke(1) fill(1, 0.5) for ant in ants: push() transform(CORNER) translate(ant.x, ant.y) rotate(-ant.heading()) line(0, 0, ant.feeler_length, 0) pop() oval(ant.x-ant.size*0.5, ant.y-ant.size*0.5, ant.size, ant.size) # Move all the ants around. for ant in ants: ant.roam()
