// done by Florian Hecht, Sept. 2006
vec gravity = new vec(0, 2);
Particle p1, p2, p3;
int NUM_ITERATIONS = 5;

void setup() {
  size(800, 800);
  colorMode(RGB, 255, 255, 255);
  p1 = new Particle(0, new pt(width/2, 0), new pt(width/2, 0));   // fixed point at the top  
  p2 = new Particle(1, new pt(width/2 - 50, 100), new pt(width/2 - 50, 100));   // the two connected points
  p3 = new Particle(1, new pt(width/2 - 90, 100), new pt(width/2 - 90, 100));
  }

void draw() {
  background(255); stroke(0); strokeWeight(3);
  if (mousePressed) {  p1.pos.setToMouse();     p1.old_pos.setTo(p1.pos.x, p1.pos.y);};
  doPhysix();
  fill(0, 0, 0);  line(p1.pos.x, p1.pos.y, p3.pos.x, p3.pos.y); noStroke(); 
  fill(255, 0, 0);   ellipse(p1.pos.x, p1.pos.y, 5, 5); 
  fill(0, 255,0);  ellipse(p2.pos.x, p2.pos.y, 40, 40);
  fill(0, 0, 255);  ellipse(p3.pos.x, p3.pos.y, 40, 40);
  }

void doPhysix() {
  p2.clearVelocity();  p2.applyForce(gravity);   // accumulate forces
  p3.clearVelocity();  p3.applyForce(gravity);
  p2.verlet(0.1, 0);  p3.verlet(0.1, 0);   // integrate
  for (int i = 0; i < NUM_ITERATIONS; i++) {   // satisfy constraints
    p1.satisfyWallConstraint(0);  p2.satisfyWallConstraint(20);   p3.satisfyWallConstraint(20);
    satisfySpringConstraint(p1, p3, 250, 0.0002);   satisfySpringConstraint(p2, p3, 40, 1.0);
   }
  }
  
class Particle {
  float one_over_m; // 1/mass
  pt pos; // current position
  pt old_pos; // pos of last frame
  vec a; // acceleration
  
  Particle(float pone_over_m, pt ppos, pt pold_pos) {one_over_m = pone_over_m; pos = ppos.make(); old_pos = pold_pos.make(); a = new vec(0, 0);}
  
  void verlet(float time_step, float dampening) {
    float x, y;   
    x = (2.0 - dampening)*pos.x - (1.0 - dampening)*old_pos.x + a.x*time_step*time_step;
    y = (2.0 - dampening)*pos.y - (1.0 - dampening)*old_pos.y + a.y*time_step*time_step;   
    old_pos.setTo(pos.x, pos.y);
    pos.setTo(x, y);
    } // end verlet
  
  void satisfyWallConstraint(float radius) {
    pos.x = min(max(pos.x, radius), width-radius);
    pos.y = min(max(pos.y, radius), height-radius);
    } // end statisfyWallConstraint
  
  void looseEnergy(float p) {vec d = old_pos.vecTo(pos); old_pos.addScaledVec(p, d);  }
  
  void clearVelocity() { a.setTo(0, 0); }
  
  void applyForce(vec f) {vec force = f.make(); force.mul(one_over_m); a.add(force); }
  } // end class particle

void satisfySpringConstraint(Particle p1, Particle p2, float d, float strength) {
  vec diff = p1.pos.vecTo(p2.pos);
  float l = diff.norm();
  float difference = strength*((l - d) / l);
  float corr = difference / (p1.one_over_m + p2.one_over_m);  
  p1.pos.addScaledVec(p1.one_over_m*corr, diff);
  p2.pos.addScaledVec(-p2.one_over_m*corr, diff);
  } // end satisfySpringConstraint