flockingThree<br>

// Dan Shiffman "The Nature of Code" Ch. 6 and  https://www.youtube.com/watch?v=IoKfQrlQ7rA

// https://gamedevelopment.tutsplus.com/tutorials/understanding-steering-behaviors-collision-avoidance--gamedev-7777

// http://paulbourke.net/geometry/pointlineplane/

// Keith Peters "Coding Math" line/seg intersection:  https://www.youtube.com/watch?v=4bIsntTiKfM

// https://www.gamedev.net/topic/542870-determine-which-side-of-a-line-a-point-is/

​

// boid, ArrayList, PVector, rect, text, cos, sin, random, map, vertex, ellipse, line segment, line, grayscale

// Mouse click to reset.

​

// Obstacle avoidance Version 2: A moving obstacle.

// A rudimentary collision implementation only.

// If the boid and obstacle are heading in opposite directions at a fast 

// enough closing speed the  boid will travel through the obstacle line.

​

ArrayList<Boid> allBoids;

int numBoids = 200;

PVector obs1, obs2; // each end of the obstacle line

float ang = 0; // to rotate obstacle line

​

void setup() {

  size(850, 650);

  background(60);

  makeBoids();

  obs1 = new PVector();

  obs2 = new PVector();

}

​

void draw() {

  fill(245, 30);

  noStroke();

  rect(0, 0, width, height);

  fill(150);

  textSize(15);

  text("FPS:", 50, height-50);

  text(floor(frameRate), 90, height-50);

  rotator();

    for (Boid b : allBoids) {

      b.run();

    }

}

​

// functions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

​

void makeBoids() {

  allBoids = new ArrayList<Boid>();

  for (int i = 0; i < numBoids; i++) {

    float angle = random(TWO_PI);

    float x = width/2 + cos(angle) * 200;

    float y = height/2 + sin(angle) * 200;

    allBoids.add(new Boid(x, y));

  }

}

​

void mousePressed() {

  makeBoids();

}

​

void rotator() {

  ang +=  .003;

  obs1.x = width/2 + cos(ang) * 300;

  obs1.y = height/2 + sin(ang) * 300;

  obs2.x = width/2 + cos(ang-PI) * 300;

  obs2.y = height/2 + sin(ang-PI) * 300;

  stroke(60);

  strokeWeight(3);

  fill(245);

  line(obs1.x, obs1.y, obs2.x, obs2.y);

  ellipse(width/2, height/2, 12, 12);

}

​

// class %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

​

class Boid {

  PVector pos, vel, accel, aheadLead, aheadTail, interPnt;

  float r = 5, maxforce, maxspeed, theta, distFromEdgeX = 100, distFromEdgeY = 50, aheadLen = 50;

  Boid(float x, float y) {

    accel = new PVector(0, 0);

    vel = new PVector(random(-1, 1), random(-1, 1));

    pos = new PVector(x, y);

    aheadLead = new PVector(0, 0);

    aheadTail = new PVector(0, 0);

    maxspeed = 3;

    maxforce = 0.03;

  }

​

  void run() {

    flock();

    update();

    boundaries();

    // borders();

    collision();

    render();

  }

​

  void update() {

    vel.add(accel);

    vel.limit(maxspeed);

    pos.add(vel);

    accel.mult(0);

  }

​

​

  void flock() {

    PVector sep = separate();   

    PVector ali = align();     

    PVector coh = cohesion();   

​

    accel.add(sep);

    accel.add(ali);

    accel.add(coh);

  }

​

  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  void collision() {

    float d =  distanceToSegment(obs1, obs2, pos); // from boid pos to obstacle line seg

    if (d < aheadLen) { // collision is possible  so check

      // probe: a line in front of the boid from aheadTail (set at pos) to aheadLead

      aheadTail.set(pos); 

      aheadLead.set(vel);

      aheadLead.normalize();

      aheadLead.mult(aheadLen);

      aheadLead.add(aheadTail);

​

      // do the probe and obstacle line intersect,  or is boid too close to the line anyway?

      interPnt = segmentIntersect(aheadTail, aheadLead, obs1, obs2);

      if ((interPnt.mag() > 0)  ||  (d < 5*r)) { // r*2 is boid width, allow plenty of slop   

        // calc a target pos as normal to obstacle line and call it "desired"

        PVector desired = new PVector(0, 0);

        float dx = obs2.x - obs1.x;

        float dy = obs2.y - obs1.y;

        // set normal depending upon which side of line the boid is on

        if (calcWhichSide(obs1, obs2, pos) > 0) desired.set(-dy, dx);

        else desired.set(dy, -dx);

        desired.set(desired);// only needed in JS mode. Why???????????????

​

        desired.sub(pos); // from pos to target (away from line)

        desired.normalize();

        desired.mult(maxspeed);

        desired.sub(vel);

        float setFrce = map(d, 0, aheadLen, maxforce * 3, maxforce);

        desired.limit(setFrce);

        accel.add(desired);

      }

    }

  }

​

  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  // line ends A&B  and point C // result is pos neg or zero

  float calcWhichSide(PVector A, PVector B, PVector C) {

    return ((B.x - A.x) * (C.y - A.y) - (B.y - A.y) * (C.x - A.x));

  }

​

  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  // do 2 line segments intersect?

  PVector segmentIntersect(PVector p0, PVector p1, PVector p2, PVector p3) {

    float A1, B1, C1, A2, B2, C2, denominator;

    float intersectX, intersectY, rx0, ry0, rx1, ry1;

​

    A1 = p1.y - p0.y;

    B1 = p0.x - p1.x;

    C1 = A1 * p0.x + B1 * p0.y;

    A2 = p3.y - p2.y;

    B2 = p2.x - p3.x;

    C2 = A2 * p2.x + B2 * p2.y;

    denominator = A1 * B2 - A2 * B1;

    if (denominator == 0) return new PVector(0, 0);

​

    intersectX = (B2 * C1 - B1 * C2) / denominator; 

    intersectY = (A1 * C2 - A2 * C1) / denominator; 

    rx0 = (intersectX - p0.x) / (p1.x - p0.x); 

    ry0 = (intersectY - p0.y) / (p1.y - p0.y); 

    rx1 = (intersectX - p2.x) / (p3.x - p2.x); 

    ry1 = (intersectY - p2.y) / (p3.y - p2.y);

​

    if (((rx0 >= 0 && rx0 <= 1) || (ry0 >= 0 && ry0 <= 1))

      && ((rx1 >= 0 && rx1 <= 1) || (ry1 >= 0 && ry1 <= 1)))

      return  new PVector(intersectX, intersectY);

    else return new PVector(0, 0);

  }

​

  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  //  calc dist from  a line seg. (p1 to p2) and a point p3

  float distanceToSegment(PVector p1, PVector p2, PVector p3) {

    float xDelta = p2.x - p1.x;

    float yDelta = p2.y - p1.y;

    float u = ((p3.x - p1.x) * xDelta + (p3.y - p1.y) * yDelta) / (xDelta * xDelta + yDelta * yDelta);

    PVector point;

    float howFar = 0;

    if (u < 0) {

      return PVector.dist(p1, p3);

    } else if (u > 1) {

      return  PVector.dist(p2, p3);

    } else {

      point = new PVector(p1.x + u * xDelta, p1.y + u * yDelta); 

      return PVector.dist(point, p3);

    }

  }

​

  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  // Cohesion: Steer towards flockmates that have moved too far away. 

  // Collect the average pos of all nearby boids of same species. This becomes "desired".

  // Steer towards desired.

​

  PVector cohesion () {

    float neighbordist = 60;

    PVector sum = new PVector(0, 0);   // Start with empty vector to accumulate all pos's

    int count = 0;

    for (Boid other : allBoids) {

      float d = PVector.dist(pos, other.pos);

      if ((d > 0) && (d < neighbordist)) { 

        sum.add(other.pos); // Add pos

        count++;

      }

    }

    if (count > 0) {

      sum.div(count); // average  

      sum.sub(pos); //  vec pointing from current pos to "desired" pos

      sum.normalize();

      sum.mult(maxspeed);

      // Steering = Desired - vel

      sum.sub(vel);

      float setMax = map(sum.mag(), 0, 5, 0, maxforce);

      sum.limit(setMax);  

      return sum;

    } else {

      return new PVector(0, 0);

    }

  }

​

​

  // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  // Separate: Steer away from flockmates, of all species, that are too close.

  // Calc vector pointing away from each nearby flockmate.

  // The  average of these vectors becomes "desired".

​

  PVector separate () {

    float sepDist = 30;// r*6;

    PVector sum = new PVector(0, 0); // ends up as "desired"

    int count = 0;

    float dAv = 0, dTotal = 0;

    for (Boid other : allBoids) {

      float d = PVector.dist(pos, other.pos);

      // d>0 stops self-checking but allows some boids to piggy-back from time to time

      if ((d > 0) && (d < sepDist)) { 

        dTotal += d;

        PVector diff = PVector.sub(pos, other.pos); // points away

        diff.normalize();

        sum.add(diff);

        count++;

      }

    }

    // Average == divide by how many

    if (count > 0) {

      sum.div((float)count);

      dAv = dTotal/float(count);

    }

    if (sum.mag() > 0) {

      // force has inverse relatioship to average distance

      // if flockmates are closer, use more force, further away, use less

      float setMaxForce = map(dAv, 0, sepDist, .1, 0);

      //  Steering = Desired - vel

      sum.normalize();

      sum.mult(maxspeed);

      sum.sub(vel); // sum is "desired"

      sum.limit(setMaxForce);

      //sum.setMag(setMaxForce);//not js mode

    }

    return sum;

  }

​

​

  //  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  // Align: Steer towards average heading (not pos) of nearby flockmates of same species.

  // Collect heading vector for each nearby flockmate.

  // the average of these vecs becomes "desired".

  PVector align () {

    float neighbordist = 60;

    PVector sum = new PVector(0, 0);

    int count = 0;

    for (Boid other : allBoids) {

      float d = PVector.dist(pos, other.pos);

      if ((d > 0) && (d < neighbordist)) { 

        sum.add(other.vel);// total of headings of each local flockmate

        count++;

      }

    }

    if (count > 0) {

      sum.div((float)count); // average

      //  Steering = Desired - vel

      sum.normalize();

      sum.mult(maxspeed);

      sum.sub(vel);

      float setMax = map(sum.mag(), 0, 5, 0, .03);

      sum.limit(setMax);

      return sum;

    } else return new PVector(0, 0);

  }

​

  // Wraparound

  void borders() {

    if (pos.x < -r) pos.x = width+r;

    if (pos.y < -r) pos.y = height+r;

    if (pos.x > width+r) pos.x = -r;

    if (pos.y > height+r) pos.y = -r;

  }

​

​

  void boundaries() {

    PVector desired = null;

​

    if (pos.x < distFromEdgeX) {

      desired = new PVector(maxspeed, vel.y);

    } else if (pos.x > width -distFromEdgeX) {

      desired = new PVector(-maxspeed, vel.y);

    } 

​

    if (pos.y < distFromEdgeY) {

      desired = new PVector(vel.x, maxspeed);

    } else if (pos.y > height-distFromEdgeY) {

      desired = new PVector(vel.x, -maxspeed);

    } 

​

    if (desired != null) {

      desired.normalize();

      desired.mult(maxspeed);

      desired.sub(vel);

      desired.limit(.01);

      accel.add(desired);

    }

  } 

​

  void render() {

    theta = vel.heading2D() + PI/2;

    pushMatrix();

    translate(pos.x, pos.y);

    rotate(theta);

    noStroke();

    fill(60);

    beginShape();

    vertex(0, -r*2);

    vertex(-r*1.5, r*1.25);

    vertex(0, 0);

    vertex(r*1.5, r*1.25);

    endShape(CLOSE);

    popMatrix();

  }

}

​

​