ペナルティ法による複数の円の衝突

/**
* collision with many particles
*
* @author aa_debdeb
* @date 2017/01/01
*/

float e = 1.0;
float k = 1.0;

ArrayList<Particle> particles;

void setup(){
  size(500, 500);
  noStroke();
  frameRate(30);
  particles = new ArrayList<Particle>();
  while(particles.size() < 30){
    float radius = random(10, 30);
    PVector loc = new PVector(random(radius, width - radius), random(radius, height - radius));
    boolean isOverlapping = false;
    for(Particle p: particles){
      if(PVector.dist(loc, p.loc) <= radius + p.radius){
        isOverlapping = true;
        break;
      }
    }
    if(!isOverlapping){
      float velSize = 5.0;
      float velAng = random(TWO_PI);
      PVector vel = new PVector(velSize * cos(velAng), velSize * sin(velAng));
      particles.add(new Particle(loc, vel, radius));
    }
  }  
}

void draw(){
  background(#C7C4A5);
  for(Particle p: particles){
    p.render();
    p.move();
  }
  
  for(Particle p1: particles){
    for(Particle p2: particles){
      if(p1 == p2){continue;}
      float d = PVector.dist(p1.loc, p2.loc);
      if(d <= 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 spring = -k * (p1.radius + p2.radius - d);
        float j = (1 + e) * (p1.mass * p2.mass / (p1.mass + p2.mass)) * PVector.dot(v12, n);
        PVector impulse = PVector.mult(n, j + spring); 
        p1.nvel.add(impulse);
      }
    }
  }
  
  for(Particle p: particles){
    p.updateVel();
  }
  
}

class Particle{
  
  PVector loc, vel, nvel;
  float radius, mass;
  color c;
  
  Particle(PVector loc, PVector vel, float radius){
    this.loc = loc;
    this.vel = vel;
    this.nvel = new PVector(vel.x, vel.y);
    this.radius = radius;
    this.mass = 1.0;
    if(radius < 20){
      c = color(#9B2C6B);
    } else if(radius < 26){
      c =color(#197993);
    } else {
      c = color(#6C4787);
    }
  }
  
  void move(){
    PVector mouse = new PVector(mouseX, mouseY);
    PVector acc = PVector.sub(mouse, loc);
    acc.limit(0.5);
    vel.add(acc);
    vel.limit(5.0);
    nvel = new PVector(vel.x, vel.y);
    loc.add(vel);
  }
  
  void render(){
    fill(c);
    ellipse(loc.x, loc.y, radius * 2, radius * 2);
  }
  
  void updateVel(){
    vel = nvel;
    nvel = new PVector(vel.x, vel.y);
  }
}