撃力ベース法による円の衝突

/**
*  collision with two particles
*
* @author aa_debdeb
* @date 2016/12/31
*/

Particle p1, p2;
float e = 0.5;

void setup(){
  size(500, 500);
  noStroke();
  fill(#BF1E56);
  initialize();
}

void initialize(){
  float velSize = 3;
  float velAng1 = HALF_PI / 2 + random(-PI / 16, PI / 16);
  p1 = new Particle(new PVector(width / 4.0, height / 4.0), new PVector(velSize * cos(velAng1), velSize * sin(velAng1)));
 float velAng2 =  PI + HALF_PI / 2 + random(-PI / 16, PI / 16);
  p2 = new Particle(new PVector(width / 4.0 * 3, height / 4.0 * 3), new PVector(velSize * cos(velAng2), velSize * sin(velAng2)));
}

void draw(){
  if(frameCount % 200 == 199){
    initialize();
  }
  background(#D8E212);
  p1.render();
  p2.render();
  p1.update();
  p2.update();
  if(PVector.dist(p1.loc, p2.loc) <= p1.radius + p2.radius){
    PVector p12 = PVector.sub(p2.loc, p1.loc);
    PVector n = PVector.div(p12, p12.mag());
    PVector v12 = PVector.sub(p2.vel, p1.vel);
    PVector vn1 = PVector.mult(n, PVector.dot(p1.vel, n));
    PVector vt1 = PVector.sub(p1.vel, vn1);
    PVector t = PVector.div(vt1, vt1.mag());
    float j = (1 + e) * (p1.mass * p2.mass / (p1.mass + p2.mass)) * PVector.dot(v12, n);
    PVector J = PVector.mult(n, j);
    PVector nv1 = PVector.add(p1.vel, PVector.div(J, p1.mass));
    PVector nv2 = PVector.add(p2.vel, PVector.div(J, -p2.mass));
    p1.vel = nv1;
    p2.vel = nv2;
    p1.update();
    p2.update();
  }
}

class Particle {

  PVector loc, vel;
  float radius, mass;
  
  Particle(PVector loc, PVector vel){
    this.loc = loc;
    this.vel = vel;
    radius = random(10, 50);
    mass = radius * radius / 1000;
    
  }
  
  void update(){
    loc.add(vel);
  }
  
  void render(){
    ellipse(loc.x, loc.y, radius * 2, radius * 2);
  }
  
}