//2D Physics Library
//by Michael Chang, UCLA
//http://users.design.ucla.edu/~mflux
//May 21, 2005

//  I won't care to explain my physics here, but please feel free to poke around.
//  It's not difficult.

boolean WORLDBORDER=true;
float WORLDBORDERBUFFER=0;
float FRICTION=0.98;
vec GRAVITY=new vec(0,.4);
class mass
{
  vec p;        //spatial position
  vec v;        //velocity vector
  vec a;        //acceleration vector
  mass cm[];
  float sl[];
  float sk[];
  float x()
  {
    return p.x;
  }
  float y()
  {
    return p.y;
  }
  mass()
  {
    p=new vec();
    v=new vec();
    a=new vec();
  }
  mass(mass m)
  {
    this.p=new vec(m.p);
    this.v=new vec(m.v);
    this.a=new vec(m.a);
    if(cm==null)
      return;
    cm=new mass[m.cm.length];
    for(int i=0;i<cm.length;i++)
    {
      cm[i]=m.cm[i];
    }
//    System.arraycopy(m.cm,0,cm,0,cm.length);
    sl=new float[m.sl.length];
    System.arraycopy(m.sl,0,sl,0,sl.length);    
    sk=new float[m.sk.length];
    System.arraycopy(m.sk,0,sk,0,sk.length);     
  }
  mass(float x,float y)
  {
    p=new vec(x,y);
    v=new vec();
    a=new vec();
  }
  mass(vec p)
  {
    this.p=new vec(p);
    v=new vec();
    a=new vec();
  }
  mass(vec p,vec v)
  {
    this.p=new vec(p);
    this.v=new vec(v);
    a=new vec();
  }
  mass(vec p,vec v,vec a)
  {
    this.p=new vec(p);
    this.v=new vec(v);
    this.a=new vec(a);
  }
  void update()
  {
//    v.add(a);
    p.add(v);
    if(WORLDBORDER)
      borders(WORLDBORDERBUFFER);
      
    if(cm!=null)
      updateSprings();
  }
  void dampScalar(float k)
  {
    v.mul(k);
  }
  void borders(float buff)
  {
    if(p.x>width+buff)
    {
      p.x=width+buff;      
//      vec nextV=new vec(p);
//      nextV.add(v);
//      sfSingle(this,new mass(nextV),0,-.5);
    }
    if(p.x<-buff)
    {
      p.x=-buff; 
//      vec nextV=new vec(p);
//      nextV.add(v);
//      sfSingle(this,new mass(nextV),0,-.5);
    }
    if(p.y>height+buff)
    {
      p.y=height+buff;
//      vec nextV=new vec(p);
//      nextV.add(v);
//      sfSingle(this,new mass(nextV),0,-.5);
    }
    if(p.y<-buff)
    {
      p.y=-buff;      
//      vec nextV=new vec(p);
//      nextV.add(v);
//      sfSingle(this,new mass(nextV),0,-.5);
    }
  }

  void connectSpringTo(mass m,float springLength,float springConstant)
  {
    if(m==null)
      return;
    if(cm==null)
      cm=new mass[0];
    mass temp[]=new mass[cm.length+1];
    System.arraycopy(cm,0,temp,0,cm.length);
    temp[cm.length]=m;
    cm=temp;
    if(sl==null)
      this.sl=new float[0];
    this.sl=append(sl,springLength);
    if(sk==null)
      this.sk=new float[0];
    this.sk=append(sk,springConstant);
  }
  void connectSpringTo(mass m,float k)
  {  
    float d=dist(this.p,m.p);
    connectSpringTo(m,d,k);
  }  
  void updateSprings()
  {
    if(cm==null)
      return;      
    for(int i=0;i<cm.length;i++)    
    {
       sfDouble(this,cm[i],sl[i],sk[i]);

    }
  }
  void clearSprings()
  {
    cm=null;
    sk=null;
    sl=null;
  }
}
vec centerOfMass(mass m[])
{
  if(m==null||m.length==0)
    return null;
  vec v=new vec();
  for(int i=0;i<m.length;i++)
  {
    v.add(m[i].p);
  }
  v.div(float(m.length));
  return v;
}
mass[] rotateMasses(mass m[],vec v,float ang)
{
  mass nm[]=new mass[m.length];  
  for(int i=0;i<m.length;i++)
  {
    nm[i]=new mass(m[i]);
    nm[i].p.rotate(v,ang);
  }
  return nm;
}
vec springForce(mass a,mass b,float rl,float k)
{
//  k=constrain(k,-1,1);
  vec displace=new vec(a.p,b.p);
  displace.mul(.5);
  float dl=displace.mag();
  if(dl==0)
    return new vec();
  float ratio=rl/2/dl;
  float zx=displace.x-displace.x*ratio;
  float zy=displace.y-displace.y*ratio;
  vec sf=new vec(zx,zy);
  sf.mul(k);
  return sf;
}
void sfSingle(mass a,mass b,float rl,float k)
{
  vec sf=springForce(a,b,rl,k);
  a.v.add(sf);
}
void sfDouble(mass a,mass b,float rl,float k)
{
  vec sf=springForce(a,b,rl,k);
  a.v.add(sf);
  b.v.sub(sf);  
}