// A function to rotate a vector
 public void rotateVector(PVector v, float theta) {
   float m = v.mag();
   float a = v.heading2D();
   a += theta;
   v.x = m * PApplet.cos(a);
   v.y = m * PApplet.sin(a);
 }
 // Method to update location
 public void update() {
   // Update velocity
   vel.add(acc);
   // Limit speed
   vel.limit(maxspeed);
   loc.add(vel);
   // Reset accelertion to 0 each cycle
   acc.mult(0);
 }
 // Function to update location
 public void update() {
   // As long as we aren't dragging the pendulum, let it swing!
   if (!dragging) {
     float G = 0.4f; // Arbitrary universal gravitational constant
     theta_acc =
         (-1 * G / r)
             * sin(
                 theta); // Calculate acceleration (see:
                         // http://www.myphysicslab.com/pendulum1.html)
     theta_vel += theta_acc; // Increment velocity
     theta_vel *= damping; // Arbitrary damping
     theta += theta_vel; // Increment theta
   }
   loc.set(r * sin(theta), r * cos(theta), 0); // Polar to cartesian conversion
   loc.add(origin); // Make sure the location is relative to the pendulum's origin
 }
 public void draw() {
   stroke(0, 0, 0);
   change = round(random(1, 10));
   if (change == 10) {
     direction = round(random(0, 8));
     point(pointX, pointY);
     point(pointX + 1, pointY + 1);
     point(pointX - 1, pointY - 1);
     point(pointX - 1, pointY + 1);
     point(pointX + 1, pointY - 1);
     stroke(200, 200, 200);
     point(pointX + 1, pointY);
     point(pointX - 1, pointY);
     point(pointX, pointY + 1);
     point(pointX, pointY - 1);
     stroke(0, 0, 0);
   }
   if (direction == 1) {
     speed.set(0, -1, 0);
   }
   if (direction == 2) {
     speed.set(1, 0, 0);
   }
   if (direction == 3) {
     speed.set(0, 1, 0);
   }
   if (direction == 4) {
     speed.set(-1, 0, 0);
   }
   if (direction == 5) {
     speed.set(-1, -1, 0);
   }
   if (direction == 6) {
     speed.set(1, 1, 0);
   }
   if (direction == 7) {
     speed.set(-1, 1, 0);
   }
   if (direction == 8) {
     speed.set(1, -1, 0);
   }
   if (pointX == width) {
     pointX = 1;
   }
   if (pointX == 0) {
     pointX = width - 1;
   }
   if (pointY == height) {
     pointY = 1;
   }
   if (pointY == 0) {
     pointY = height - 1;
   }
   pointX = pointX + speed.x;
   pointY = pointY + speed.y;
   point(pointX, pointY);
 }
 // Constructor initialize all values
 Boid(PVector l, float ms, float mf) {
   loc = l.get();
   r = 4.0f;
   maxspeed = ms;
   maxforce = mf;
   acc = new PVector(0, 0);
   vel = new PVector(maxspeed, 0);
 }
 public void drag() {
   // If we are draging the ball, we calculate the angle between the
   // pendulum origin and mouse location
   // we assign that angle to the pendulum
   if (dragging) {
     PVector diff =
         PVector.sub(origin, new PVector(mouseX, mouseY)); // Difference between 2 points
     theta = atan2(-1 * diff.y, diff.x) - radians(90); // Angle relative to vertical axis
   }
 }
    // This constructor could be improved to allow a greater variety of pendulums
    Pendulum(PVector origin_, float r_) {
      // Fill all variables
      origin = origin_.get();
      r = r_;
      theta = 0.0f;

      // calculate the location of the ball using polar to cartesian conversion
      float x = r * sin(theta);
      float y = r * cos(theta);
      loc = new PVector(origin.x + x, origin.y + y);
      theta_vel = 0.0f;
      theta_acc = 0.0f;
      damping = 0.995f; // Arbitrary damping
      ballr = 16.0f; // Arbitrary ball radius
    }
    public void render() {
      // Draw a triangle rotated in the direction of velocity
      float theta = vel.heading2D() + PApplet.radians(90);
      fill(100);
      stroke(0);
      pushMatrix();
      translate(loc.x, loc.y);
      rotate(theta);
      beginShape(PConstants.TRIANGLES);
      vertex(0, -r * 2);
      vertex(-r, r * 2);
      vertex(r, r * 2);
      endShape();

      if (debug) {
        stroke(50);
        line(0, 0, 0, -sight);
      }
      popMatrix();
    }
 // Wraparound
 public void borders() {
   if (loc.x < -r) loc.x = width + r;
   if (loc.y < -r) loc.y = height + r;
   if (loc.x > width + r) loc.x = -r;
   if (loc.y > height + r) loc.y = -r;
 }
    public void avoid(ArrayList obstacles) {

      // Make a vector that will be the position of the object
      // relative to the Boid rotated in the direction of boid's velocity
      PVector closestRotated = new PVector(sight + 1, sight + 1);
      float closestDistance = 99999;
      Obstacle avoid = null;

      // Let's look at each obstacle
      for (int i = 0; i < obstacles.size(); i++) {
        Obstacle o = (Obstacle) obstacles.get(i);

        float d = PVector.dist(loc, o.loc);
        PVector dir = vel.get();
        dir.normalize();
        PVector diff = PVector.sub(o.loc, loc);

        // Now we use the dot product to rotate the vector that points from boid to obstacle
        // Velocity is the new x-axis
        PVector rotated = new PVector(diff.dot(dir), diff.dot(getNormal(dir)));

        // Is the obstacle in our path?
        if (PApplet.abs(rotated.y) < (o.radius + r)) {
          // Is it the closest obstacle?
          if ((rotated.x > 0) && (rotated.x < closestRotated.x)) {
            closestRotated = rotated;
            avoid = o;
          }
        }
      }

      // Can we actually see the closest one?
      if (PApplet.abs(closestRotated.x) < sight) {

        // The desired vector should point away from the obstacle
        // The closer to the obstacle, the more it should steer
        PVector desired =
            new PVector(closestRotated.x, -closestRotated.y * sight / closestRotated.x);
        desired.normalize();
        desired.mult(closestDistance);
        desired.limit(maxspeed);
        // Rotate back to the regular coordinate system
        rotateVector(desired, vel.heading2D());

        // Draw some debugging stuff
        if (debug) {
          stroke(0);
          line(loc.x, loc.y, loc.x + desired.x * 10, loc.y + desired.y * 10);
          avoid.highlight(true);
        }

        // Apply Reynolds steering rules
        desired.sub(vel);
        desired.limit(maxforce);
        acc.add(desired);
      }
    }